ROS navigation stack on P3DX

roscore

Record bagfile containint /scan and /tf messages

Run gmapping (as shown in previous tutorial) and save the map using:

rosrun map_server map_saver

Close all terminals and roscores

In costmap_common_params.yaml

obstacle_range: 2.5
raytrace_range: 3.0
inflation_radius: 0.35

footprint: [ [0.3302, -0.0508], [0.254, -0.0508], [0.254, -0.254], [-0.254, -0.254], [-0.254, 0.254], [0.254, 0.254], [0.254, 0.0508], [0.3302, 0.0508] ]

transform_tolerance: 0.2
map_type: costmap

observation_sources: laser_scan_sensor

laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true, expected_update_rate: 0.4}

In global_costmap_params.yaml

global_costmap:
  global_frame: /map
  robot_base_frame: base_link
  update_frequency: 2.0
  publish_frequency: 5.0
  static_map: true

In local_costmap_params.yaml

local_costmap:
  global_frame: /odom
  robot_base_frame: base_link
  update_frequency: 2.0
  publish_frequency: 5.0
  static_map: false
  rolling_window: true
  width: 5.0
  height: 5.0
  resolution: 0.025

In p3d_configuration.launch

<launch>
  <node pkg="sicktoolbox_wrapper" type="sicklms" name="sicklms_node" output="screen">
    <param name="port" value="/dev/ttyUSB1" />
    <param name="baud" value="38400" />
    <param name="resolution" value="1.0" />
  </node>

  <node pkg="p2os_driver" type="p2os" name="p2os" output="screen">
  <param name="port" value="/dev/ttyUSB0"/>
  </node>

  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0 0 0.1397 0 0 0 base_link laser 100" />
  <!-- node pkg="robot_setup_tf" type="static_transform_publisher" name="tf_broadcaster" output="screen" -->
  <!--/node-->

</launch>

In move_base.launch

<launch>
        <!--master auto="start"/-->


        <!--- Run AMCL -->
        <include file="$(find p2os_launch)/amcl.launch" />

        <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
                <rosparam file="costmap_common_params.yaml" command="load" ns="global_costmap" />
                <rosparam file="costmap_common_params.yaml" command="load" ns="local_costmap" />
                <rosparam file="local_costmap_params.yaml" command="load" />
                <rosparam file="global_costmap_params.yaml" command="load" />
                <rosparam file="base_local_planner_params.yaml" command="load" />
                <param name="base_global_planner" type="string" value="NavfnROS" />
                <param name="conservative_reset_dist" type="double" value="3.0" />
                <param name="controller_frequency" type="double" value="15.0" />
        </node>
</launch>

Run map server with the following command:

rosrun map_server map.yaml

Then do:

roslaunch p3dx_configuration.launch

followed by:

roslaunch move_base.launch

Run the P2OS Dashboard:

rosrun p2os_dashboard p2os_dashboard

and enable the motors of the P3DX.

Now fire up rviz to set goals via GUI:

rosrun rviz rviz

Load the .vcg file move_base.vcg from the package called PR2 navigation

Now, click on the "Set 2D Nav Goal" in the tool bar and then create a vector on the map in the display window.

Watch the robot move towards its goals while avoiding obstacles.

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.