Note: This tutorial assumes that you have completed the previous tutorials: Stereo outdoor mapping.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Stereo Outdoor Navigation

Description: This tutorial shows how to integrate autonomous navigation with RTAB-Map in context of outdoor stereo mapping.

Tutorial Level: ADVANCED

2D Navigation

Example of navigation with only a stereo camera:

The planner used in the video is move_base. The global costmap is generated from the "/map" (nav_msgs/OccupancyGrid) topic and the local costmap is updated using the "openni_points" (sensor_msgs/PointCloud2) topic. Here an example of launch file with the related move_base config files. The sections below show how these topics come from.

For info and for a complete example launch file, all the snippets below were integrated together with 3D stereo mapping in az3_mapping_robot_stereo_nav.launch.

  • Planner:
    •    <group ns="planner">
            <remap from="openni_points" to="/planner_cloud"/>
            <remap from="map" to="/rtabmap/proj_map"/>
            <remap from="move_base_simple/goal" to="/planner_goal"/>
              
            <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" />
            </node>
         </group>
  • costmap_common_params.yaml:
    • obstacle_range: 2.5
      raytrace_range: 3.0
      footprint: [[ 0.3,  0.3], [-0.3,  0.3], [-0.3, -0.3], [ 0.3, -0.3]]
      footprint_padding: 0.03
      #robot_radius: ir_of_robot
      inflation_radius: 0.55
      transform_tolerance: 1
      
      controller_patience: 2.0
      
      NavfnROS:
          allow_unknown: true
      
      recovery_behaviors: [
          {name: conservative_clear, type: clear_costmap_recovery/ClearCostmapRecovery},
          {name: aggressive_clear, type: clear_costmap_recovery/ClearCostmapRecovery}
      ]
      
      conservative_clear: 
          reset_distance: 3.00
      aggressive_clear:
          reset_distance: 1.84
  • global_costmap_params.yaml:
    • global_costmap:
        global_frame: map
        robot_base_frame: base_footprint
        update_frequency: 0.5
        publish_frequency: 0.5
        static_map: true
  • local_costmap_params.yaml:
    • local_costmap:
        global_frame: odom
        robot_base_frame: base_footprint
        update_frequency: 2.0
        publish_frequency: 2.0
        static_map: false
        rolling_window: true
        width: 4.0
        height: 4.0
        resolution: 0.025
        origin_x: -2.0
        origin_y: -2.0
      
        observation_sources: point_cloud_sensor
      
        # assuming receiving a cloud from rtabmap_util/obstacles_detection node
        point_cloud_sensor: {
          sensor_frame: base_footprint,
          data_type: PointCloud2, 
          topic: openni_points, 
          expected_update_rate: 0.5, 
          marking: true, 
          clearing: true,
          min_obstacle_height: -99999.0,
          max_obstacle_height: 99999.0}
  • base_local_planner_params.yaml:
    • TrajectoryPlannerROS:
      
        # Current limits based on AZ3 standalone configuration.
        acc_lim_x:  0.75
        acc_lim_y:  0.75
        acc_lim_theta: 4.00
        max_vel_x:  0.500
        min_vel_x:  0.212
        max_rotational_vel: 0.550
        min_in_place_rotational_vel: 0.15
        escape_vel: -0.10
        holonomic_robot: false
      
        xy_goal_tolerance:  0.20
        yaw_goal_tolerance: 0.20
      
        sim_time: 1.7
        sim_granularity: 0.025
        vx_samples: 3
        vtheta_samples: 3
        vtheta_samples: 20
      
        goal_distance_bias: 0.8
        path_distance_bias: 0.6
        occdist_scale: 0.01
        heading_lookahead: 0.325
        dwa: true
      
        oscillation_reset_dist: 0.05
        meter_scoring: true

Global costmap

The rtabmap node can generate a 2D occupancy grid (named "/rtabmap/proj_map") from the projection of the 3D cloud map on the ground. The empty cells are filled by projecting the ground on the xy plane. The occupied cells are filled by projecting the obstacles on the xy plane. For each point cloud, the obstacles and the ground are segmented. Since the ground is not even, the ground is segmented by normal filtering: all points with normal in the +z direction (+- fixed angle) are labelled as ground, all the others are labelled as obstacles. The images below show an example.

obs0

obs1

obs2

obs3

obs4

   ...
   <remap from="map" to="/rtabmap/proj_map"/>
   ...

Local costmap

For the local costmap update, we feed it a point cloud with only the obstacles without the ground. The rtabmap_util/obstacles_detection nodelet is used. This nodelet uses the same ground/obstacles segmentation approach described for the global costmap. Note that in local_costmap_params.yaml, because the robot can be at any height from its odom/map referentials, we set the min_obstacle_height and max_obstacle_height to high values (-99999 and 99999). The cloud used for segmentation is generated from the disparity image of stereo_image_proc using the rtabmap_util/point_cloud_xyz nodelet.

   <group ns="/stereo_camera" >
      
      <node pkg="nodelet" type="nodelet" name="stereo_nodelet"  args="manager"/>

      <!-- Generate a point cloud from the disparity image -->
      <node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_util/point_cloud_xyz stereo_nodelet">
         <remap from="disparity/image"       to="disparity"/>
         <remap from="disparity/camera_info" to="right/camera_info_throttle"/>
         <remap from="cloud"                to="cloudXYZ"/>
         
         <param name="voxel_size" type="double" value="0.05"/>
         <param name="decimation" type="int" value="4"/>
         <param name="max_depth" type="double" value="4"/>
      </node>

      <!-- Create point cloud for the local planner -->
      <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_util/obstacles_detection stereo_nodelet">
         <remap from="cloud" to="cloudXYZ"/>
         <remap from="obstacles" to="/planner_cloud"/>

         <param name="frame_id" type="string" value="base_footprint"/>
         <param name="map_frame_id" type="string" value="map"/>
         <param name="min_cluster_size" type="int" value="20"/>
         <param name="max_obstacles_height" type="double" value="0.0"/>
       </node>
   </group>

Wiki: rtabmap_ros/Tutorials/StereoOutdoorNavigation (last edited 2023-04-19 05:10:24 by MathieuLabbe)