ROS + Gmapping + P3DX + SICK

This post deals with the complete procedure of using ROS to record a bag file which contains the LaserScan data from the SICK (mounted on a P3DX) and using the gmapping ROS package to build a map.

Operating P3DX with a sidewinder joystick

Things needed:
1) P3DX
2) Laptop
3) Sidewinder joystick (you could you use any other joystick too with a corresponding launch file)
4) ROS installed on your system

ROS packages required:
1) p2os
2) joy (dependency for p2os)
3) teleop_joy_sidewinder.launch (you could also use teleop_joy.launch depending on your joystick). The contents of this file are as follows:


Step 1:
Run roscore in your first terminal.

roscore

Step 2:
Run the p2os_dashboard:

$ rosrun p2os_dashboard p2os_dashboard

It has three basic functions:
1) Provides the robot's battery information
2) A button to view rxconsole
3) A button to enable/disable motors

Step 3:
Run the p2os node to connect to p3dx.

$ rosrun p2os_driver  _port:=/dev/ttyUSB0

_port should be given the device address of the p3dx serial cable.

Step 4:

Launch the joystick launch file:

$ roslaunch p2os_launch teleop_joy_sidewinder.launch

Remember to keep the teleop_joy_sidewinder.launch file in the p2os_launch folder.

Step 5:

Enable the motors using the button(gear-shaped) on the p2os-dashboard.

Drive the robot around using the following button presses:

L + R + Axis

Gathering Laser scans in a bag file:

Things needed:
1) SICK Laser (mounted on your p3dx)
2) All the other things from the previous step

ROS Packages required:
1) sicktoolbox
2) sicktoolbox_wrapper

Installation procedure:
1) First download and install the sicktoolbox using rosmake.
2) Fire up your favorite editor and open this file (your_ros_installation_directory)/ros/diamondback/stacks/sicktoolbox/build/sicktoolbox-1.0/c++/drivers/lms/sicklms-1.0/SickLMS.cc . Go to line 2319.
3) In this line, add the O_NDELAY flag as shown here:

if((_sick_fd = open(_sick_device_path.c_str(), O_RDWR | O_NOCTTY| O_NDELAY)) < 0) {
throw SickIOException("SickLMS::_setupConnection: - Unable to open serial port");
}

4) After this line, add a system sleep call for 30 seconds. This is required so as to allow the sick laser to start up and start scanning.

sleep(30);

5) After completing these changes, use the makefile to remake the package. Do not use rosmake.

$ make all

6) Now download and install the sicktoolbox_wrapper .

STEP 1:
Begin joystick operation as explained in the previous step.

STEP 2:
Run the sicklms node in the sicktoolbox_wrapper as shown:

$ rosrun sicktoolbox_wrapper sicklms _port:=/dev/ttyUSB1 _baud:=38400 _resolution:=1.0

The sicklms node publishes sensor_msgs/LaserScan on the topic called scan .
Check rxgraph to examine the scenario and check whether the sicklms node is publishing data to the scan topic.

NOTE:
1) _resolution:=1.0 is important for gmapping
2) Set _port to the device address of the SICK's serial cable.

STEP 3:
Run rosbag to start recording data from the topics scan and tf

$ rosbag record -o gmapping_data.bag /scan /tf

The bag file be stored in your current directory along with a time stamp appended to the file name.

STEP 4:
Use the joystick to drive the p3dx around while it collects data from the laser into the bag file.

STEP 5:
After finishing a drive around your room, check your bag file using rxbag to see if contains bot the tf and the scan data.

Running Gmapping on the saved bagfile:

Now that you have successfully recorded the laser data into a bag file, it can be used anytime. We will now use it run gmapping on this set.

ROS packages required:
1) gmapping -- http://www.ros.org/wiki/gmapping
2)pr2_navigation -- http://www.ros.org/wiki/pr2_navigation -- this package is required only for a file move_base.vcg

STEP 1:
Run roscore

STEP 2:
Set the use_sim_time paramter to true

$ rosparam set use_sim_time "true"

STEP 3:
Run a transform publisher node to publish tf information between the base link and the laser:

$ rosrun tf static_transform_publisher 0 0 0 0 0 0 base_link laser 100 

STEP 4:
Run the slam_gmapping node:

$ rosrun gmapping slam_gmapping

This node will subscribe to the tf and scan data from the bag file.

STEP 5:
Play the bag file:

$ rosbag play --clock gmapping_data.bag

The bag file start playing and you will be able to see the time elapsed on the terminal.

STEP 6:
To visualize the map now, there are two options:
1) Store the map to a png file:

$ rosrun map_server map_saver

2) Real time visualization:
Fire up rviz:

         $ rosrun rviz rviz

Open the following file in rviz:

(YOUR_ROS_INSTALLATION_DIRECTORY)/ros/diamondback/stacks/pr2_navigation/pr2_navigation_global/rviz/move_base.vcg

After the file gets loaded, you will be able to see the progress of the mapping process in real time.