| 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
Contents
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.
|
|
|
|
|
... <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>