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_ */