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.

Executing slam and navigation: generating TF

The full tree of coordinate frames looks like this:
Now, I am going to analyze where is each of the transforms taking place and how does it look like.

Note: In order to print the information about particular transformations we use

$ rosrun tf tf_echo

1. /base_scan to /base_laser_link

TF_ECHO
At time 5734.700
- Translation: [-0.100, 0.000, -0.200]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
in RPY [0.000, -0.000, 0.000]

FILE

Broadcaster:
$ roscd sscrovers_tf_config
$ gedit src/tf_broadcaster.cpp

In lines 13 to 17:

broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
ros::Time::now(),"base_laser_link", "base_scan"));
r.sleep();

Here we can observe that the parent frame is base_laser_link and the child is base_scan

Listener
$ gedit src/tf_listener.cpp

try{
geometry_msgs::PointStamped base_point;
//argmuments: name of the frame we want to transform, the point we're transforming, storage for the transformed point
listener.transformPoint("base_laser_link", laser_point, base_point);

ROS_INFO("base_scan: (%.2f, %.2f. %.2f) -----> base_laser_link: (%.2f, %.2f, %.2f) at time %.2f",
laser_point.point.x, laser_point.point.y, laser_point.point.z,
base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
}

2. /base_laser_link to /base_link

TF_ECHO
At time 6346.700
- Translation: [-0.050, 0.000, -0.150]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
in RPY [0.000, -0.000, 0.000]

FILE
We can see in the frames tree that the broadcaster of this transform is the Stage_Sim node.

Lines 291 to 300

// Also publish the base->base_laser_link Tx. This could eventually move
// into being retrieved from the param server as a static Tx.
Stg::stg_pose_t lp = this->lasermodels[r]->GetPose();
tf::Quaternion laserQ;
laserQ.setRPY(0.0, 0.0, lp.a);
tf::Transform txLaser = tf::Transform(laserQ,
tf::Point(lp.x, lp.y, 0.15));
tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
mapName("base_link", r),
mapName("base_laser_link", r)));

3. /base_link to base_footprint

TF_ECHO
At time 7503.700
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
in RPY [0.000, -0.000, 0.000]

FILE

We can see in the frames tree that the broadcaster of this transform is the Stage_Sim node.

Lines 302 to 308

// Send the identity transform between base_footprint and base_link
tf::Transform txIdentity(tf::createIdentityQuaternion(),
tf::Point(0, 0, 0));
tf.sendTransform(tf::StampedTransform(txIdentity,
sim_time,
mapName("base_footprint", r),
mapName("base_link", r)));

4. /base_footprint to /odom

We can see in the frames tree that the broadcaster of this transform is the Stage_Sim node.

TF_ECHO
At time 8086.600
- Translation: [-5.229, -1.957, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
in RPY [0.000, -0.000, 0.000]

FILE

Lines 328 to 336

// broadcast odometry transform
tf::Quaternion odomQ;
tf::quaternionMsgToTF(odomMsgs[r].pose.pose.orientation, odomQ);
tf::Transform txOdom(odomQ,
tf::Point(odomMsgs[r].pose.pose.position.x,
odomMsgs[r].pose.pose.position.y, 0.0));
tf.sendTransform(tf::StampedTransform(txOdom, sim_time,
mapName("odom", r),
mapName("base_footprint", r)));

5. /odom to /map

TF_ECHO
At time 8026.200
- Translation: [-2.472, 1.931, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.405, 0.914]
in RPY [0.000, -0.000, -0.834]


FILE

Altough the transform tree shows that the transform between this two frames is broadcasted by /amcl, it is actually done by /gmapping since there is a slam process involved. It would be donde by /amcl if the map wasn't moving at all.

However it wasn't clear for me at which point in the code of the gmapping node was the transform being done. I found something that might be the transform in the amcl code instead:

Lines 754 to 768 of amcl_node.cpp

// subtracting base to odom from map to base and send map to odom instead
tf::Stamped odom_to_map;
try
{
tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
0.0));
tf::Stamped tmp_tf_stamped (tmp_tf.inverse(),
laser_scan->header.stamp,
base_frame_id_);
this->tf_->transformPose(odom_frame_id_,
tmp_tf_stamped,
odom_to_map);
}