Move base: comments on particle cloud part II

I have figured out a transform tree that avoids the previously displayed warn:

Dropped 100.00% of messages so far. Please turn the [ros.costmap_2d.message_notifier] rosconsole logger to DEBUG for more information.

What I did was the following:

1. Run the Ariaoriginal node.
2. For the odom->base_link->base_scan tf I used Pablo's tf, the one I have previously used for mapping.
3. Run gmapping in order to make the tf map-> removing the scan:=base_scan parameter.
4. Run move_base.launch.
5. Run rviz.

The actual problem is that the robot's footprint isn't positioned when I click the 2DPose Estimate button. The conflict according to rviz is that the messages from the particlecloud topic aren't being received. In order to diagnose the problem I

1.Run rostopic info in order to know more about the particlecloud topic.
2. Run rxgraph and verify that the node is actually establishing communication between amcl and rviz. I am not sure if move_base should be involved as well.

3. I run rostopic echo particlecloud and nothing is displayed meaning that, in fact, no messages are being published.

I now have to inquire on the nature of this topic.

The final observation I have is that what stopped the Dropped messages warn from being displayed was that I dind't run the sicklms topic which I thik shouldn't be happening since the localization might use information from the laser as the tf tree includes it's frame. I might be wrong, however, and discover that only odometry is needed.

Standard Units of Measure and Coordinate Conventions on ROS

I found a useful link concerning measurement conventions on ROS: http://www.ros.org/reps/rep-0103.html.

This link talks about the convention of coordinate frames: http://www.ros.org/reps/rep-0105.html

Move base: comments on particle cloud

The robot's footprint is correctly being displayed in rviz.



The particle cloud isn't. The warning message displayed indicates no messages are being received. However, when displaying rxgraph, we can observe that the topic particle cloud is being published by amcl and rviz is subscribing to it.

Here is the result of displaying the roswtf debug tool where we can observe that the topic /particlecloud isn't disconnected:

Suddenly, the following error was displayed by the move_base node and was shown as well in rxconsole:

Move_base: Comments on map_server

According to the tutorial Using rviz with the navigation stack: http://www.ros.org/wiki/navigation/Tutorials/Using%20rviz%20with%20the%20Navigation%20Stack

The map should be displayed by the nav_msgs::GetMap service. However, the message displayed by the map service in rviz with the configuration I am running is nav_msgs::OccupancyGrid. This message is published by the map_server node.

I don't really think this is a problem however. The map is still displayed in rviz and the move_base node identifies it and accepts it.

Another conflict I had at the beginning was that running gmapping during the process caused the current laser's view to be displayed instead of the .png map. I couldn't stop running this node since it is in charge of the map-odom transform. My solution to the problem was to eliminate the scan:=base_scan parameter from my launch file.

Now, the .png map is being displayed.

Costmap configuration

I created a new file in the itesm_demo package in order to include all of the configuration files related to the configuration of move_base.

$roscd itesm_demo
$mkdir config
$cd config

I created four files in this folder. The description of each of them follows:

1. costmap_common_params.yaml: Parameters that are common both for local and global costmaps:


obstacle range: determines the maximum range sensor reading that will result in an obstacle being put into the costmap.
raytrace range: determines the range to which we will raytrace freespace given a sensor reading.

footprint: in the case of specifying the footprint, the center of the robot is assumed to be at (0.0, 0.0) and both clockwise and counterclockwise specifications are supported.

Inflation radius: The inflation radius should be set to the maximum distance from obstacles at which a cost should be incurred.

2. global_costmap_params.yaml: long term plans over the entire environment


global frame: defines what coordinate frame the costmap should run in
robot base frame: defines the coordinate frame the costmap should reference for the base of the robot.
static map: determines whether or not the costmap should initialize itself based on a map served by the map_server.

3. local_costmap_params.yaml: local planning and obstacle avoidance

Rolling window: parameter to true means that the costmap will remain centered around the robot as the robot moves through the world.

4. base_local_planner_configuration.-yaml: The base_local_planner is responsible for computing velocity commands to send to the mobile base of the robot given a high-level plan

Successful map

The map created today seems to be fairly coherent. In order to create it I drove the robot around the hallway. Here it is:

Mapping

Today I could make a more or less coherent map of lab conditions. The problem is that my lab's conditions aren't ideal at all ... However, the overlapping of the map, which was the main issue to be solved, seems to have disappeared.

What I did this time was to use the original RosAria node since the one modified by the sscrovers team seems to throw odometry errors due to the introduction of an additional function (update). I will post later my comments concerning this code.

What I did afterwards, was to verify that the cmd_vel message published by the teleoperation node coincided with the one to which RosAria node subscribes to. In this case the specific name of the topic is /RosAria/cmd_vel and its type is geometry::msg.

Tomorrow I will physically construct a map with ideal conditions in order to verify that the odometry issue isn't present at the original RosAria node.

Configuration of the Joystick

1. Verify that Linux recognizes my joystick

$ ls /dev/input/
by-id    event1   event12  event2  event5  event8  mice    mouse2
by-path  event10  event13  event3  event6  event9  mouse0
event0   event11  event14  event4  event7  js0     mouse1

js0 is my joystick's port.

2. Make sure my joystick is working

$ sudo jstest /dev/input/js0

I see the data parameters changing.

3. List the permissions of the joystick

$ ls -l /dev/input/js0

crw-rw-r--+ 1 root root 13, 0 2010-10-12 20:07 /dev/input/js0

This indicates I need to change the mode of the port through:

$ sudo chmod a+rw /dev/input/js0

Listing the permissions again will show this result:

crw-rw-rw-+ 1 root root 13, 0 2010-10-12 20:07 /dev/input/js0

4. Start the joy node

$ rosrun joy joy_node

5. Echo the joy topic

$ rostopic echo joy

And I see the data of my joystick is being published through the joy topic. 

Comments on ROS Time

"ROS has builtin time and duration primitive types, which roslib provides as the ros::Time and ros::Duration classes, respectively. A Time is a specific moment (e.g. "today at 5pm") whereas a Duration is a period of time (e.g. "5 hours"). Durations can be negative."

The function Time::now(), which is used in most of the codes I am working with uses the system time:

Time Time::now()
{
if (!use_system_time_)
{
boost::mutex::scoped_lock lock(g_sim_time_mutex);
Time t = sim_time_;
return t;
}

Time t;
getWallTime(t.sec, t.nsec);

return t;
}

When I create the map in "real time" the time used is the one from my computer:

"When running with a real robot, the ROS client libraries will use "wall-clock" time (i.e. the time your computer reports). "

In the case of the playback of the bag files I have noticed the following:

"if you are playing back sensor data into your system, you may wish to have your time correspond to the timestamps of the sensor data. "

Concerning Clock server:

"If you are playing back a bag file with rosbag play, using the --clock option will run a Clock Server while the bag file is being played."

Map overlapping

After modifying the launch file Renato shared with me, gmapping was no longer displaying neither the TF_OLD_DATA error, nor the dropped messages warnings. However, the map is still overlaping. Here is the content of the launch file I am currently using named launch_rovers.launch. The attempts to change nodes are presented as comments:

What I am doing right now is playing this launch file simultaneously with rviz, with which I have as well some doubts. Here are two screenshots of what the mapping process looks like:



I have been playing with the fixed and the target frames as can be appreciated in the screenshots. I, however, get the same result always: overlapping of the map.

I have noticed that during this process the mobile frame isn't able to fully rotate when it is necessary and this is what I think is causing the overlapping.

I have several suppositions for the generator of this problem:

1. Odometry isn't being correctly actualized.
2. Gmapping has an error (which I have no idea about).
3. The time ins ROS isn't properly actualized nor the buffer is properly being cleaned out when necessary.

I hope the question I placed at the mailing list is soon answered.

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.

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