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

Executing slam and navigation: configuration of nodes in launch file

If we look at the launch/do-slam-navigation.launch file we can see how each of the nodes involved is configured:

1. Stage_Sim. It displays the stageros node and its argument is the world file /world/willow-erratic.world

Publications:
* /tf [tf/tfMessage]
* /odom [nav_msgs/Odometry]
* /rosout [roslib/Log]
* /base_scan [sensor_msgs/LaserScan]
* /clock [roslib/Clock]
* /base_pose_ground_truth [nav_msgs/Odometry]

Subscriptions:
* /time [unknown type]
* /clock [roslib/Clock]
* /cmd_vel [geometry_msgs/Twist]

2. Teleop_Base: It runs the teleop_base_keyboard node and it tells it to subscribe to the cmd_vel topic.

Publications:
* /cmd_vel [geometry_msgs/Twist]
* /rosout [roslib/Log]

Subscriptions:
* /time [unknown type]
* /clock [roslib/Clock]


3. TF_Broadcaster: Runs the tf_broadcaster.cpp node that makes the transform between /base_laser_link and base_scan frames (as will be later explained).

Publications:
* /tf [tf/tfMessage]
* /rosout [roslib/Log]

Subscriptions:
* /time [unknown type]
* /clock [roslib/Clock]

4. Gmapping: Runs the slam_gmapping node and publishes the laser scan information under the base_scan node.

Publications:
* /map_metadata [nav_msgs/MapMetaData]
* /tf [tf/tfMessage]
* /map [nav_msgs/OccupancyGrid]
* /rosout [roslib/Log]

Subscriptions:
* /time [unknown type]
* /base_scan [sensor_msgs/LaserScan]
* /tf [tf/tfMessage]
* /clock [roslib/Clock]
* /reset_time [unknown type]


5. map_server: Runs the map_server node whose function is to load the image that informs of the occupancy state of the map. (/world/willow-full.pgm)

Publications:
* /map_metadata [nav_msgs/MapMetaData]
* /map [nav_msgs/OccupancyGrid]
* /rosout [roslib/Log]

Subscriptions:
* /time [unknown type]
* /clock [roslib/Clock]

6. move_base.xml: determines the characteristics of the costmaps

* move_base node
* costmap_common_params.yaml
* local_costmap_params.yaml
* global_costmap_params.yaml
* base_local_planner_params.yaml

7. amcl_node.xml: determines the configuration of the amcl node.

Executing slam and navigation: topics functionality

The displays determined by the configuration file rviz_conf.vcg and their descriptions are the following:

1. Floor scan (laser scan): Shows data from a laser scan, with different options for rendering modes, accumulation, etc. sensor_msgs/LaserScan

2. Grid (Grid): Displays a 2D or 3D grid along a plane

3. Obstacles (Grid cells): Displays the obstacles that the navigation stack sees in its costmap. For the robot to avoid collision, the robot footprint should never intersect with a cell that contains an obstacle. nav_msgs/GridCells

4. Inflated obstacles (Grid Cells): Displays obstacles in the navigation stack's costmap inflated by the inscribed radius of the robot. For the robot to avoid collision, the center point of the robot should never overlap with a cell that contains an inflated obstacle. nav_msgs/GridCells

5. Map (Map): Displays a map on the ground plane. nav_msgs/OccupancyGrid

6. Global Plan (Path): Displays the portion of the global plan that the local planner is currently pursuing. nav_msgs/Path

7. Local Plan (Path): Displays the trajectory associated with the velocity commands currently being commanded to the base by the local planner.

8. Current goal (Pose): Displays the goal pose that the navigation stack is attempting to achieve. geometry_msgs/PoseStamped

9. Localisation (Pose Array): Draws a "cloud" of arrows, one for each pose in a pose array. geometry_msgs/PoseArray

10. Robot Footprint (Polygon): Displays the footprint of the robot. geometry_msgs/Polygon

If we open rxgraph we can see that most of the above displays are topics published by the move_base_node and they are subscribing to the rviz node.

If we sk rosnode to display information about the move_base_node it can tell us more clearly which topics it published and to which of them it is subscribing as well as the messages over which they communicate:

Node [/move_base_node]

Publications:
* /move_base_node/local_costmap/obstacles [nav_msgs/GridCells]
* /move_base_node/NavfnROS/NavfnROS_costmap/obstacles [nav_msgs/GridCells]
* /move_base_node/current_goal [geometry_msgs/PoseStamped]
* /move_base_node/global_costmap/unknown_space [nav_msgs/GridCells]
* /move_base_node/local_costmap/inflated_obstacles [nav_msgs/GridCells]
* /move_base_node/NavfnROS/NavfnROS_costmap/robot_footprint [geometry_msgs/PolygonStamped]
* /move_base_node/TrajectoryPlannerROS/global_plan [nav_msgs/Path]
* /move_base_node/local_costmap/voxel_grid [costmap_2d/VoxelGrid]
* /move_base/result [move_base_msgs/MoveBaseActionResult]
* /move_base_node/global_costmap/inflated_obstacles [nav_msgs/GridCells]
* /move_base_node/TrajectoryPlannerROS/local_plan [nav_msgs/Path]
* /move_base/status [actionlib_msgs/GoalStatusArray]
* /cmd_vel [geometry_msgs/Twist]
* /move_base_node/global_costmap/obstacles [nav_msgs/GridCells]
* /rosout [roslib/Log]
* /move_base/goal [move_base_msgs/MoveBaseActionGoal]
* /move_base_node/NavfnROS/plan [nav_msgs/Path]
* /move_base_node/local_costmap/unknown_space [nav_msgs/GridCells]
* /move_base_node/NavfnROS/NavfnROS_costmap/inflated_obstacles [nav_msgs/GridCells]
* /move_base_node/local_costmap/robot_footprint [geometry_msgs/PolygonStamped]
* /move_base/feedback [move_base_msgs/MoveBaseActionFeedback]
* /move_base_node/global_costmap/robot_footprint [geometry_msgs/PolygonStamped]
* /move_base_node/NavfnROS/NavfnROS_costmap/unknown_space [nav_msgs/GridCells]

Subscriptions:
* /time [unknown type]
* /move_base_simple/goal [geometry_msgs/PoseStamped]
* /move_base/goal [move_base_msgs/MoveBaseActionGoal]
* /base_scan [sensor_msgs/LaserScan]
* /map [nav_msgs/OccupancyGrid]
* /tf [tf/tfMessage]
* /clock [roslib/Clock]
* /odom [nav_msgs/Odometry]
* /move_base/cancel [unknown type]
* /reset_time [unknown type]
* /tf_message [unknown type]


This can be done for everyone of the nodes that is running.

Executing slam and navigation: RVIZ functionality and configuration elements

Rviz is a 3D visualization environment.

• On the left hand side there is the displays list that will list the topics rviz is related to.

• Target frame: reference frame for the camera view. In this case /map frame is the target frame which means that we will see the robot driving around the map.

• Fixed frame: where all incoming data is transformed to before being displayed. This frame should not be moving relative to the world. In this case /map frame is he fixed frame.

• The 2D Nav goal topic is /move_base_simple/goal

• The 2D pose estimate topic is initialpose

All of this characteristics can be determined either directly on rviz o through a configuration file that in this case is launch/rviz_conf.vcg which is determined when running rviz node like this:

$ roscd rviz

$ ./bin/rviz -d `rosstack find sscrovers-ros-pkg`/launch/rviz_conf.vcg

Stage and navigation stack tutorial

This entry is based on the following tutorial:
http://www.ros.org/wiki/move_base_stage/Tutorials/stage%20and%20navigation%20stack
Following are the elements of the awesomeros package and a small description of their functions.

1. World file: willow.world

It is located at awesomeros/world/willow.world, it is required by stage and it describes the characteristics of the simulation environment.

2. Map server: willow.yaml

Located at world//willow.yaml. A map is required by navigation stack to work on. The yaml file determines the occupancy, resolution and location characteristics of the map being used.

3. Costmaps: they store information about obstacles in the map

3.1 costmap_common_params.yaml: configurations that both costmaps (local and global) should follow.
3.2 global_costmap_params.yaml: configuration of long-term plans over the entire environment.
3.3 local_costmap_params.yaml: configuration for local planning and obstacle avoidance.

4. Base local planner: base_local_planner_params.yaml

It is responsible for computing velocity commands to send to the mobile base of the robot.

5. Navfn: navfn_params.yaml

It provides a fast interpolated navigation function that can be used to create plans for a mobile base.

6. Move_base: move_base.xml

This package lets you move your robot to the desired position using all the previously mentioned nodes of the navigation stack.

7. Amcl: amcl_node.xml

It is a probabilistic location system for mobile robots in 2D.

8. Rviz: rviz.xml and rviz.vcg.

It is a 3D visualization environment

9. Launch file: launch/robot.launch

It acts as a container for all the elements of the package.

Running the launch file and the rviz configuration file lets us set goals in rviz.

The most significant topics published while running this tutorials are the ones of the move_base node.
And the transforms among frames are broadcasted only by stageros and amcl.

Building a map using logged data

Based on one of the gmapping tutorials:

http://www.ros.org/wiki/slam_gmapping/Tutorials/MappingFromLoggedData#download

1. Run a core

$ roscore

2. Set use_sim_time param to true

$ rosparam set use_sim_time true

3. Run the slam_gmapping node that will take scans on the base_scan topic

$ rosrun gmapping slam_gmapping scan:=base_scan

4. Play a bag file, in this case I played the example available

$ wget http://pr.willowgarage.com/data/gmapping/basic_localization_stage.bag
$ rosbag play basic_localization_stage.bag

5. In order to watch the mapping process I ran nav_view

$ rosrun nav_view nav_view /static_map:=/dynamic_map

 6. Save map  $ rosrun map_server map_saver  [ INFO] 0.000000000: Writing map occupancy data to map.pgm [ INFO] 0.000000000: Writing map occupancy data to map.yaml [ INFO] 0.000000000: Done

This is how the publications and subscriptions of topics looks like:


 

Running the sicklms node and rviz

The sscrovers_laser_pub package is based on the sicktoolbox_wrapper package.

Renato made a modification to the sicklms node which consisted on applying a delay of 22 seconds to it since it is approximately the time that the physical laser takes to power up properly.
I had to modify the ssc_laser.vcg file since with the original one the readings of the laser weren't displayed on rviz. What I did was to check the topics published by and subscribed to the rviz and sicklms nodes.

The changes I made are in lines 5 and 22. Instead of /laser topic I placed /base_scan topic. This worked honestly thanks to a process of trial and error.
In order to run the rviz and sicklms nodes I followed these steps:

1. Run a core

$ roscore

2. Configure the laser port

$ sudo chmod a+rw /dev/ttyUSB1
$ ls -l /dev/ttyUSB1

And something like this should be displayed:

crw-rw-rw- 1 root dialout 166, 0 2009-10-27 14:18 /dev/ttyUSB1

3. Run the sicklms node

$ roscore
$ roscd sscrovers_laser_pub
$ cd bin
$ ./sicklms _port:=/dev/ttyUSB1 _baud:=38400
Connection must be achieved:

 Requesting measured value data stream...
             Data stream started!

4. Start rviz

$ roscd rviz
$ ./bin/rviz -d `rospack find sscrovers_laser_pub`/ssc_laser.vcg


5. Check the communication among nodes through specific topics

$ rxgraph