ROS与VSLAM入门教程-rtabmap_ros-双目户外导航

ROS与VSLAM入门教程-rtabmap_ros-双目户外导航

说明：

• 介绍RTAB-Map如何实现双目户外导航

github：

• 参考代码

2D Navigation/2D导航

• planner/规划器使用 move_base
• global costmap/全局代价地图通过"/map"(nav_msgs/OccupancyGrid)话题生成
• local costmap/本地代价地图通过"openni_points" (sensor_msgs/PointCloud2) 话题更新
• 一个配置好的例子az3_mapping_robot_stereo_nav.launch. 参考github
• 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_ros/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

• rtabmap节点通过3D点云图在地面的投影来生成2D栅栏图(名为"/rtabmap/proj_map"话题)
• 通过在xy平面上投影地面来填充空的单元格。通过在xy平面上投影障碍物来填充占用的单元格。
• 由于地面不均匀，地面通过正常滤波分割：所有在+ z方向（+-固定角度）正常的点都被标记为地面，所有其他标记为障碍物。
• 图例：

• 使用全局地图/map，需要映射话题

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

Local costmap/本地代码地图

• 对于本地代码地图更新，我们只提供障碍物的点云
• 使用rtabmap_ros/obstacles_detection nodelet节点，该节点使用与全局成本图相同的地面/障碍物分割方法。
• 在local_costmap_params.yaml中，因为机器人可以与其odom/map参考的可以是任何高度，我们将min_obstacle_height和max_obstacle_height设置为高值(-99999 and 99999).
• 用于分割的点云是使用rtabmap_ros/point_cloud_xyz的nodelet节点从stereo_image_proc的差异图像生成的。
• rtabmap_ros/point_cloud_xyz代码如下：

  <group ns="/stereo_camera" >
      
      <!-- Generate a point cloud from the disparity image -->
      <node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_ros/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_ros/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>

参考：

• http://wiki.ros.org/rtabmap_ros/Tutorials/StereoOutdoorNavigation