Pablo's pioneer_tf.cpp

Renato shared with me a post of a ROS user that does slam using the ROSARIA node. Here is the url where this post can be found and following the program itself can be found:

http://ros-users.122217.n3.nabble.com/Pioneer3-td1265101.html#a1288712




#include
#include
#include



void poseCallback(const nav_msgs::Odometry::ConstPtr& odomsg)
{
//TF odom=> base_link

static tf::TransformBroadcaster odom_broadcaster;
odom_broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(tf::Quaternion(odomsg->pose.pose.orientation.x,
odomsg->pose.pose.orientation.y,
odomsg->pose.pose.orientation.z,
odomsg->pose.pose.orientation.w),
tf::Vector3(odomsg->pose.pose.position.x/1000.0,
odomsg->pose.pose.position.y/1000.0,
odomsg->pose.pose.position.z/1000.0)),
odomsg->header.stamp, "/odom", "/base_link"));
ROS_DEBUG("odometry frame sent");
}




int main(int argc, char** argv){
ros::init(argc, argv, "pioneer_tf_publisher");
ros::NodeHandle n;

ros::Rate r(20);

tf::TransformBroadcaster broadcaster;

//subscribe to pose info
ros::Subscriber pose_sub = n.subscribe("RosAria/pose", 1, poseCallback);

while(n.ok()){
//base_link => laser
broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(tf::Quaternion(0, 0, 0), tf::Vector3(0.13, -0.04, 0.294)),
ros::Time::now(), "/base_link", "/laser"));

ros::spinOnce();
r.sleep();
}
}

Installing p2os

Most of the useful instructions I followed come from the p2os' tutorial: http://www.ros.org/wiki/p2os/Tutorials/Getting%20Started%20with%20p2os

1. I first went to the stacks ros' directory and checked out the svn for the usc packages:

$ cd ~/ros/stacks/
$ svn co https://usc-ros-pkg.svn.sourceforge.net/svnroot/usc-ros-pkg/trunk/ usc-ros-pkg

2. Then I updated the stack list and compiled it

$ rospack profile && rosstack profile
$ rosmake p2os --rosdep-install

3. In order to connect to the /dev/ttyUSB0 port instead of /dev/ttyS0 port (which is default) 
I changed the p2os.cc code as follows:

$ roscd p2os
$ cd p2os_driver
$ cd src
$ gedit p2os.cc

And around line 64 I changed the parameter of the port:

n_private.param( "port",psos_serial_port, std::string("/dev/ttyUSB0"));

instead of

n_private.param( "port",psos_serial_port, def );

4. I then just ran the p2os node and it made the connection successfully.

Now, since I currently have no access to the joystick (but I will have it, however), I am going
to modify the teleoperation launch file to do it directly with my keyboard.

Create a map of the lab with the robot Pioneer and SICK LMS:2

Yesterday I made a map of the laboratory using data from the laser recorded into a bag file and running slam_gmapping node.

I found a very simple mistake in my tf_listener.cpp code and searching through the sscrovers svn I found the same one. It is approximately at line 20 and it concerns the transformation of the new point created.

The original file displays the following:

try{ geometry_msgs::PointStamped base_point;
listener.transformPoint("base_laser_link", laser_point, base_point);

The transformation is in fact supposed to be from base_laser_link frame to base_link frame. And that is why base_point is created, to store the same information as laser_point (of base_laser_link frame) but now in the desired frame that should be base_link. The result is the following:

try{ geometry_msgs::PointStamped base_point;
listener.transformPoint("base_link", laser_point, base_point);

I then ran all of the nodes in exactly the same way I had been doing it and this time a map.pgm and a map.yaml file were created.

The only difference in my procedure was that, exactly at the time I was going to play the bag file, I pushed the reset button on rviz. In spite of this, gmapping still displays the TF_OLD_DATA error. I have been reading about it and several users in the mailing list argue that they had dealt with this same problem, both when running gmapping and other nodes.

Comments on sscrovers_tf_conf

Based on the tf robot setup tutorials I am commenting the codes of the broadcaster and listener of this package in order to find out a solution to the recently presented problem.

tf_broadcaster.cpp

The only comment I have is related to the timestamp of ros::Time::now(). Perhaps this is the reason why tf is ignoring the desired transform

tf_listener.cpp

• When creating the point for the base_scan frame (the one in the laser) it is used the following message type:

             geometry_msgs::PointStamped                             



       The stamped at the end of the message name means that it includes headers:                                        1. The frame_id header is assigned to the frame (base_scan) itself:                                        laser_point.header.frame_id = "base_scan";                                        2. The timestamp header is assigned to ros::Time which asks for the latest available transform.                     I think this might be a key concept for debugging the program:                                        laser_point.header.stamp = ros::Time(); 

• The next step is to declare an object (base_point) that is going to store the same information that the 

• laser_point (on the base_scan frame) owns but now in the base_link frame.

             try{geometry_msgs::PointStamped base_point;                

                      listener.transformPoint("base_link", laser_point, base_point);  

TF troubles

There has been some trouble while running the slam_gmapping node to generate a map with the readings of a bag file. The most probable reason is that the TF tree is doing something incorrectly. The process I usually run trough to verify the complete process of slam is the following:

1. I run a core
$ roscore

2. I run RosAria, teleop and sicklms nodes. The three of them work correctly and I verify that the laser scans are taking place by launching rviz.

3. I run tf_broadcaster from sscrovers_tf_conf package.

4. I run gmapping node as follows:
$ rosrun gmapping slam_gmapping scan:=base_laser_link

5. At this point I verify the tree of transforms and it is correct: map->odom->base_link->base_laser_link

6. Afterwards I run rosrecord like this:
$ rosrecord -f dos /base_laser_link /tf

7. Once the data is recorded to the bag file I play it back with rosplay while the gmapping_node is still executing.

8. When running the bag file I get the following error from gmapping: "TF_OLD_DATA ignoring data from the past for frame base_laser_link"

9. I run the map_saver and the map is never created.

What was suggested to me in relation with this error (tf_old_data) was to run roswtf and so I did. The errors displayed talked about some sort of transform "duplication". For instance:

"node [/slam_gmapping] publishing transform [/odom] with parent [/map] already published by node [/rosplay_1281403114388600000]."

I the decided to kill all the proceses (except for slam_gmapping and roscore) when running rosplay. I still got the same error.

Renato, on the other hand, has been having similar tf trouble when trying to run gmapping:

"The base_scan observation buffer has not been updated for 7.06 seconds, and it should be updated every 0.80 seconds."

10. Afterwards, I tried running the gmapping node under the topic of base_scan. The terminal displays that a first scan has been registered but then stops the process and shows me the following error:

"TF_OLD_DATA ignoring data from the past for frame

The majority of dropped messages were due to messages growing older than the TF cache time."

If I run roswtf at this point the following warning is displayed:

"WARNING The following node subscriptions are unconnected:
* /tf_monitor_

1281485781963475000:
* /reset_time
* /tf_message"

I am trying to figure out if there is a direct relation with the fact that the TF mesages are older than the cache and the failing subscription of /reset_time.


Concerning TF_OLD_DATA error what the ros documentation argues is the following:

• TF_OLD_DATA errors mean that a transform is attempted to be added to the system, but the data is greater than cache_time_ before the most recent data received for this link.
• The most common cause of TF_OLD_DATA warnings are if rostime has gone backwards. This can be caused by restarting a bag playback or restarting a simulator. The fix for this is to send an Empty message the topic /reset_time. There is a button in rviz to do this.
• Another possible source of TF_OLD_DATA is if there is any outdated or delayed source of transforms. The error message will tell you which authority is sending the outdated data. If it is running in a ros node you can use view_frames to determine the more recent authority. See view_frames.

I am going to try troubleshooting with these recommendations.

Create a map of the lab with the robot Pioneer and SICK LMS:1

1. Renato made an actualization to the RosAria node in which we can notice that the transformation of which this node is in charge is odom->base_link.

//first, we'll publish the transform over tf geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link"; odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;

//send the transform
odom_broadcaster.sendTransform(odom_trans);
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time; odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";

I could successfully compile it and also ran the teleoperation node I made for the pioneer.

2. During the meeting with Renato we reviewed the process of transformations that should take place when creating the map using the RosAria node and the sicklms node. We agreed that the transform tree should look like this:


3. I then ran the sicklms node and could see its readings in rviz which means that the connection was being made properly. However, when I ran rosrecord I didn't get a bag file that had a correct content. I did several attempts using rosrecord with the odom, base_laser_link and base_scan topics.

4. I saw that the topic published by sicklms node is base_scan. However, it isn't yet clear for me if this node should have a frame that participates in the transform tree. What I saw in the node's code was the following:

ros::NodeHandle nh;
ros::NodeHandle nh_ns("~");
ros::Publisher scan_pub = nh.advertise("base_scan", 1);
nh_ns.param("port", port, string("/dev/ttyUSB1"));
nh_ns.param("baud", baud, 38400);
nh_ns.param("inverted", inverted, false);
nh_ns.param("frame_id", frame_id, "laser");

I wonder if the "laser" frame should be included in the tree or it should be substituted by the base_laser_link frame.
Or perhaps the base_scan topic should be substituted by a topic of the same type.

To simulate two robots

This is ticket #3.

1. The file that should be actually copied in order to create twoRobots.world is in directory .../stacks/simulator_stage/stage/world

2. The modification mentioned for the world file will work if you placed the copy (twoRobots.world) directly on the stage directory.

3. Here is a screenshot of the world as well as the two terminals with which we can teleoperate the two robots:



4. The nodes that are running and their topics are as follow:











If we look at the launch files we will see that in tworobots.launch the robot1 node is determined and is running the stageros node which determines that we are going to use the recently created twoRobots.world file. In each of the files we will find one node for teleoperation (Teleop_Base1 and Teleop_Base2) each of them with their particular publishing velocity topic.

Here we can see that each one of the robots is assigned an independent set of transformations.