[ros-users] Creating map with nxtros

Hello,
i posted before some problems while i was trying to create a map.

-First, to use slam algorithm u need a topic with a laser_scan msg.
This was my first to do.
Because nxt_robot_sensor_car is not publishing a topic with laser_scan.
I tried to write a Node wich is subscribing the topic ultrasonic_sensor and then converting the data and publishing a topic named base_scan with the msg type laser_scan.
The node is also turning the robot 390 degree. You can change that if u want.

I attached this node if anyone wants to look in to that code.
But before u look into that code, i am warning you, it is not good implementated.
Even i dont know if its working corretly.
So i would make me happy, if i get some feedbacks :)

-Next i recorded base_scan and tf in a bag file.(base_scan is given by the node that i have programmed,the /tf topic is given by nxtros...)
-now if i play the bag file the slam_gmapping node is givin warnings like:

[ WARN] [1303911122.112710800]: TF_OLD_DATA ignoring data from the past for frame /accel_link at time 1.30391e+09 according to authority /play_1303911121895057400
Possible reasons are listed at

-So what is my problem now?
-what should i do next or fix next?

Greetings,
Bünyamin


//============================================================================

// Name        : range_to_laser.cpp
// Author      : Bünyamin Yilmaz
// Description : Ein Konvertierer vom range zu LaserScan
//============================================================================
/* Noch zu erledigen sind:
 * -durch die Drehung soll auch immer angle_increment bestimmt werden
 * */

#include "range_to_laser.h"
//Erzeuge Zwischenspeicher für die einzelnen Daten

float angle_min =0.0;
float angle_max=390.0;//+30 grad bei der loop schließung
float range_min=2.5;//da die maximale entfernung die der Ultraschall sensor messen kann 2,5 m ist
float range_max=0.1;
double alpha=30;
float aktuellerwinkel=0;
float winkelgeschwindigkeit=0.0;
unsigned long secNew=0;
unsigned long secOld=0;
unsigned long secDiff=0;
bool roundOne=true;
bool messung=true;
bool fahre;

unsigned long pi=3.141592653589;


int gyromessungen=0;
int anzahl_messung=(angle_min+angle_max)/alpha;//eine volle messung
int zaehler=0;//Zählt die aktuelle anzahl der messung


ros::Publisher pub;
ros::Publisher pub_cmd;
ros::Subscriber gyro_subscriber;
std::vector<float> entfernungen;
geometry_msgs::Twist wende;

void pointCallback(const nxt_msgs::Range  &msgrange)
{
    if(roundOne){
        ros::Time h=ros::Time::now();
        secOld=h.nsec;
        roundOne=false;
    }
    sensor_msgs::LaserScan laser;
    v_convert(msgrange,laser);
}
//Hier soll Konvertiert werden
void v_convert(const nxt_msgs::Range & msgrange,sensor_msgs::LaserScan & laser){
    if(messung){
        ROS_INFO("Zaehler: %d",zaehler);
        if(range_min>msgrange.range){range_min=msgrange.range;}//setze minimum entfernung
        if(range_max<msgrange.range){range_max=msgrange.range;}//setze maximum range
        zaehler++;
        entfernungen.push_back(msgrange.range);
        fahre=true;
        aktuellerwinkel=0.0;
    }
    if(fahre){
        berechneWinkel(alpha);
    }

    if(zaehler==anzahl_messung){    //falls wir eine vollständige messung haben dann die zwischengespeicherten Werte in pub eintragen und publishen
        messung=false;
        fahre=false;
        ros::NodeHandle r;
        pub = r.advertise<sensor_msgs::LaserScan>("base_scan", 50);//publisher für die Laserscan msg
        laser.header=msgrange.header;
        laser.angle_min=angle_min;
        laser.angle_max=angle_max;
        laser.angle_increment=alpha;
        laser.range_min=range_min;
        laser.range_max=range_max;
        laser.ranges=entfernungen;
        pub.publish(laser);
        ROS_INFO("\n angle_min: %f \n agnle_max: %f\n angle_increment: %f\n range_min: %f\n range_max: %f\n anzahl_messung: %d",laser.angle_min,laser.angle_max,laser.angle_increment,laser.range_min,laser.range_max,anzahl_messung);
        entfernungen.clear();
        turnRobot(0.0,0.0,0.0,0.0,0.0,0.0);

    }
}
void berechneWinkel(double winkel){
    messung=false;
    ros::NodeHandle s;
    gyro_subscriber= s.subscribe("gyro", 500, gyroCallback);
    ros::spinOnce();
    ros::Time h=ros::Time::now();
        secNew=h.nsec;
        if(secNew<secOld){
            secDiff=(1000000000-secOld)+secNew;
            secOld=secNew;
        }else{
            secDiff=secNew-secOld;
            secOld=secNew;
        }
        //winkelgeschwindigkeit in rad/s
        long double mil=1000000000;
        aktuellerwinkel=aktuellerwinkel+(winkelgeschwindigkeit*(secDiff/mil));
    //    ROS_INFO("\n winkelgeschwindigkeit: %f \n AktuellerWinkel: %f\n  Winkel : %lf\n Differenz: %lu",winkelgeschwindigkeit,aktuellerwinkel,alpha,secDiff);
        if(aktuellerwinkel<=winkel){
            turnRobot(0.0,0.0,0.0,0.0,0.0,-0.5);
        }else{
            messung=true;//nimm die messung auf
            //aktuellerwinkel=0.0;
        }
}
void turnRobot(float lx,float ly,float lz,float ax,float ay,float az){
    ros::NodeHandle r;
    pub_cmd = r.advertise<geometry_msgs::Twist>("cmd_vel", 50);
    wende.linear.x=lx;
    wende.linear.y=ly;
    wende.linear.z=lz;
    wende.angular.x=ax;
    wende.angular.y=ay;
    wende.angular.z=-az;
    pub_cmd.publish(wende);

}
void gyroCallback(const nxt_msgs::Gyro &gyromsg){
    winkelgeschwindigkeit=fabs(gyromsg.angular_velocity.z);//Gyro msg,hat einen Vector3 Datenstruktur, was unter geometry_msgs/vector3.h definiert ist
    //secGyro=gyromsg.header.stamp.sec;
    winkelgeschwindigkeit *= 10;
    winkelgeschwindigkeit += 0.5;
    winkelgeschwindigkeit= floor(winkelgeschwindigkeit);
    winkelgeschwindigkeit/= 10;

}
double deg_to_rad(double degree){
    return (degree*pi)/180;
}
int main(int argc, char **argv)
{
  /*
  * Wir machen einen Subscriber und immer wenn eine neue Nachricht von Range angekommen
  *ist, wird die pointCallback funktion aufgerufen, wo wir dann die bekommene nachricht convertieren und als
  *LaserScan publishen.
  * */
  alpha=deg_to_rad(alpha);
  ROS_INFO("RAD:%f",alpha);
  ros::init(argc, argv, "range_to_laser");
  ros::NodeHandle n;

  while(ros::ok()){
      ros::Subscriber sub = n.subscribe("ultrasonic_sensor",1,pointCallback);
      ros::spin();
  }

return 0;
}
/*

// Name        : cloud_to_laser.h
// Author      : Bünyamin Yilmaz
// Version     :
 */

#ifndef CLOUD_TO_LASER_H_
#define CLOUD_TO_LASER_H_


#include <ros/ros.h>
#include "sensor_msgs/LaserScan.h"
#include <nxt_msgs/Range.h>
#include <nxt_msgs/Gyro.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Twist.h>
#include <math.h>


//methoden und funktionen
void v_convert(const nxt_msgs::Range & msg,sensor_msgs::LaserScan& pub);
void laser_scan_publisher(sensor_msgs::LaserScan&pub);
void pointCallback(const nxt_msgs::Range & msg);
void gyroCallback(const nxt_msgs::Gyro & gyromsg);
void berechneWinkel(double Grad);
void turnRobot(float,float,float,float,float,float);
double deg_to_rad(double degree);
#endif /* CLOUD_TO_LASER_H_ */