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

Testing SICK LMS-200

I tested the correct functionality of the SICK LMS-200 laser using both player and Aria demo

1. Creation of cfg file

In order to test it with player I had to create a new .cfg file named sick.cfg which only content is the driver for the laser as follows:

driver
(
name "sicklms200"
provides ["laser:0"]
port "/dev/ttyUSB1")

2. Running the cfg file and testing with playerv

$ robot-player sick.cfg
$ robot-playerv



3. Testing the laser with Aria demo.

3.1 I first needed to change the port that makes the communication with the laser

Note: Although we are working with a pioneer p3at, its laser integration board belongs to a pioneer p3dx

$ roscd Aria
$ cd Aria/params
$ gedit p3dx.p

In line 110 change LaserPort from COM2 to /dev/ttyUSB1

$ rosmake Aria
$ roscd Aria
$ cd Aria/examples

Run demo with the corresponding robot port
$ ./demo -rp /dev/ttyUSB0

Connected to robot.
Name: ITESM_1853
Type: Pioneer
Subtype: p3dx
Loaded robot parameters from p3dx.p
Stabilizing gyro

Then choose 'l' for laser mode and you should see the readings as follows:

You are in 'laser' mode currently.

Laser mode connects to a laser, or uses a previously established connection.
Laser mode then displays the closest and furthest reading from the laser.
'z' or 'Z': toggle between far reading and middle reading with reflectivity
1: lms2xx_1

ArModeLaser is connecting to lms2xx_1.
lms2xx_1: waiting for laser to power on.
lms2xx_1: Connected to the laser.

Laser mode has connected to the laser.

Close: 836mm -26.4 deg Far: 4278mm 49.8 deg 119 readings

Installation of SICK lms200 laser

Since we are using the robot without its original computer, we are connecting both the SICK laser and the controller board to our laptops through a usb-serial cable.

Therefore we had to place a female db9 connector on the SICK laser data cable.


Just as a reminder, here is the configuration of the laser integration board and the db9 connector:

* Cable that goes from the laser to the laser integration board:
- Purple: gnd
- Green: rx
- Blue: tx

* Cable that goes from laser integration board to computer and db9 connector:
- White: rx : pin 2
- Black: tx : pin3
- Grey: gnd: pin 5
- Purple: dtr: pin4

Now, the ports we are using as default for each of the connections are

- Robot port: /dev/ttyUSB0
- Laser port: /dev/ttyUSB1

Run teleoperation node for pioneer

1. Teleopeate robot using the stageros node

$ roscd stage
$ ./bin/stageros
$ roscd teleop_base
$ ./teleop_base_keyboard base_controller/command:=cmd_vel

2. Check which topics are being published and subscribed to

$ rxgraph

We can see that stageros node subscribes to /cmd_vel topic of type geometry_msgs/Twist which is published by tbk_node.

3. Check the topics to which RosAria node subscribes

$ rosnode info RosAria

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

4. Edit teleop_base_keyboard.cpp from the teleop_base package.

$ roscd teleop_base
$ cd src
$ gedit teleop_base_keyboard.cpp

In line 82 we can see that a topic of type geometry_msgs/Twist is published by TBK_Node, therefore you need to change it to the topic to which RosAria subscribes (/RosAria/cmd_vel).

5. Copy the content of teleop_base_keyboard.cpp and create a new file in sscrovers_2dnav/src directory

$ roscd sscrovers_2dnav
$ cd src
$ gedit teleop_base_pioneer.cpp

And change line 82 as mentioned:

pub_ = n_.advertise("RosAria/cmd_vel",1); //TBK_Node publishes RosAria/cmd_vel topic and RosAria node subscribes to it

I also edited the max velocities in order to avoid accidents. In line 66:
double max_speed = 0.100; // m/second

Finally, I indicated in the menu (lines 135-146) which is the task of every key in accordance to the code written in lines 175-251:

puts("Reading from keyboard"); puts("-------------------------------------------------------------------------------");
puts("q/z : increase/decrease max angular and linear speeds by 10%");
puts("w/x : increase/decrease max linear speed by 10%");
puts("e/c : increase/decrease max angular speed by 10%"); puts("--------------------------------------------------------------------------------");
puts("Moving around:");
puts(" u=turn left and go forward i=go forward o=turn right and go forward");
puts(" j=turn left k=stop l=turn right");
puts(" m=turn left and go backwards ,=go backwards .=turn right and go backwards");
puts(" anything else : stop");
puts("--------------------------------------------------------------------------------");

6. Add the new file to CMakelists.txt

$ roscd sscrovers_2dnav
$ gedit CMakelists.txt

rosbuild_add_executable(teleop_base_pioneer src/teleop_base_pioneer.cpp)

7. Compile the package

$ rosmake sscrovers_2dnav

8. Run RosAria node

9. Run teleop_base_pioneer node

$ roscd sscrovers_2dnav
$ ./teleop_base_pioneer

10. The menu with indications should be displayed and you should be able to teleoperate you Pioneer.

Run RosAria node on Pioneer

Run RosAria node on Pioneer

1. First, you need to change the port with which RosAria is making the connection and compile after making the changes.

$ roscd ROSARIA
$ cd src
$ gedit RosAria.cpp

In line 78 determine /dev/ttyUSB0

n.param( "port", serial_port, std::string("/dev/ttyUSB0") );

$ rosmake ROSARIA

2. Run a core

$ roscore

3. Run RosAria node

$ roscd ROSARIA
$ ./bin/RosAria

You should hear the characteristic sounds that you Pioneer makes when connection is achieved and in your terminal you should see something like the following:

[ INFO] 1279891763.130941000: using serial port: [/dev/ttyUSB0]
ArLog::init: File(aria.log) Verbose Logging Time Not also printing
[ INFO] 1279891763.891941000: rcv: 1279891763.891849 0.000000 0.000000
[ INFO] 1279891763.990366000: rcv: 1279891763.990318 0.000000 0.000000
[ INFO] 1279891764.090697000: rcv: 1279891764.090649 0.000000 0.000000
[ INFO] 1279891764.190707000: rcv: 1279891764.190658 0.000000 0.000000
[ INFO] 1279891764.290465000: rcv: 1279891764.290418 0.000000 0.000000
[ INFO] 1279891764.390697000: rcv: 1279891764.390650 0.000000 0.000000
[ INFO] 1279891764.491022000: rcv: 1279891764.490974 0.000000 0.000000
[ INFO] 1279891764.590368000: rcv: 1279891764.590322 0.000000 0.000000