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.