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);
}