Problem with global and local costmap

[Mercedes]

Hello,

We are working with navigation using a ZED (stereo camera). We managed to perform the part of Stereo Mapping Outdoor but we are in trouble with the navigation.

While proj_map shows well the obstacles, the global_costmap shows nothing. In turn, the local_costmap shows things that do not appear in the cloud of obstacles.

Any idea what could be happening?

Let .bag and the file we use to run this bag.

Thanks,
cheers.
Files: files.zip
Bag:  https://drive.google.com/file/d/0B4ep285vH9c8Y0R1bm1faldIcGc/view

[matlabbe]

Hi,

The local costmap should not subscribe to cloud_map. The global costmap should use static map plugin and subscribe to /proj_map. obstacles_detection plugin should subscribe to points from the camera, not cloud_map. Please compare carefully with configurations from the Stereo Outdoor Navigation tutorial.

Here some modifications I've done:

<common_costmap_params.yaml>
obstacle_range: 2.5 #actualiza info con obstaculos hasta 2.5 m
raytrace_range: 3.0 
footprint: [[0.67, 0.51], [0.67, -0.51 ], [-0.67, -0.51], [-0.67,0.51]] #tamaño del robot
#robot_radius: ir_of_robot
inflation_radius: 0.20

<global_costmap_params.yaml>
global_costmap:
  global_frame: /map
  robot_base_frame: /base_footprint
  update_frequency: 1.0
  static_map: true

<local_costmap_params.yaml>
local_costmap:
  global_frame: /odom
  robot_base_frame: /base_footprint
  update_frequency: 5
  publish_frequency: 2
  static_map: false
  rolling_window: true
  width: 6.0
  height: 6.0
  resolution: 0.05
  origin_x: -3
  origin_y: -3
  transform_tolerance: 10
  observation_sources: point_cloud_sensor
  point_cloud_sensor: {sensor_frame: base_zed, data_type: PointCloud2, topic: /obstacles_cloud, marking: true, clearing: true, min_obstacle_height: -99999, max_obstacle_height: 99999} 

And the launch file:

<launch>

    <param name="use_sim_time" value="true"/>

    <arg name="rtabmap_args"  default="--delete_db_on_start"/>              <!-- delete_db_on_start, udebug -->
    
    <group ns="rtabmap">

        <!-- Visual Odometry -->
	    <node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen" >
            <remap from="left/image_rect"       to="/zed/left/image_rect_throttle"/>
            <remap from="right/image_rect"      to="/zed/right/image_rect_throttle"/>
            <remap from="left/camera_info"      to="/zed/left/camera_info_throttle"/>
            <remap from="right/camera_info"     to="/zed/right/camera_info_throttle"/>
            <remap from="odom"                  to="/odometry"/>
            <param name="Stereo/MaxDisparity"   type="int" value="200"/>

            <param name="frame_id" type="string" value="base_footprint"/>
            <param name="odom_frame_id" type="string" value="odom"/>
            <param name="wait_for_transform_duration" type="double"   value="1"/>

            <param name="Vis/InlierDistance" type="string" value="0.5"/>
            <param name="Vis/MinInliers" type="string" value="8"/>
            <param name="Vis/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
            <param name="Vis/MaxDepth" type="string" value="0"/>

            <param name="Vis/MaxCorners" type="string" value="500"/>
            <param name="Vis/MinDistance" type="string" value="5"/>
            <param name="queue_size" type="int" value="30"/>
	    </node>

        <!-- Visual SLAM: args: "delete_db_on_start" and "udebug" -->
        <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
            <param name="frame_id" type="string" value="base_footprint"/>
            <param name="subscribe_stereo" type="bool" value="true"/>
            <param name="subscribe_depth" type="bool" value="false"/>
	        <param name="wait_for_transform_duration" type="double" value="1"/>
            <remap from="left/image_rect" to="/zed/left/image_rect_throttle"/>
            <remap from="right/image_rect" to="/zed/right/image_rect_throttle"/>
            <remap from="left/camera_info" to="/zed/left/camera_info_throttle"/>
            <remap from="right/camera_info" to="/zed/right/camera_info_throttle"/>

            <remap from="odom" to="/odometry"/>

            <param name="queue_size" type="int" value="30"/>

            <!-- RTAB-Map's parameters -->
            <param name="Rtabmap/TimeThr" type="string" value="700"/>
            <param name="Rtabmap/DetectionRate" type="string" value="1"/>

            <param name="Kp/MaxFeatures" type="string" value="200"/>
            <param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>

            <param name="SURF/HessianThreshold" type="string" value="1000"/>

            <param name="Vis/MaxDepth" type="string" value="0"/>
            <param name="Vis/MinInliers" type="string" value="5"/>
            <param name="Vis/InlierDistance" type="string" value="0.1"/>

            <param name="RGBD/LoopClosureReextractFeatures" type="string" value="true"/>
            <param name="Vis/MaxFeatures" type="string" value="500"/>
        </node>        
	
    </group> 

      <node pkg="nodelet" type="nodelet" name="stereo_nodelet"  args="manager"/>
      <group ns="zed">
         <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
            <remap from="left/image_raw"    to="left/image_rect_throttle"/>
            <remap from="left/camera_info"  to="left/camera_info_throttle"/>
            <remap from="right/image_raw"   to="right/image_rect_throttle"/>
            <remap from="right/camera_info" to="right/camera_info_throttle"/>
            <param name="disparity_range" value="200"/>
         </node>
      </group>

      <!-- to debug disparity -->
      <node pkg="image_view" type="stereo_view" name="stereo_view" output="screen">
         <remap from="stereo"       to="zed"/>
         <remap from="image"        to="image_rect_throttle"/>
      </node>
      rosrun image_view stereo_view stereo:=zed image:=image_rect_throttle

      <!-- 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="/zed/disparity"/>
         <remap from="disparity/camera_info" to="/zed/right/camera_info_throttle"/>
         <remap from="cloud"                to="cloudXYZ"/>
         
         <param name="voxel_size" type="double" value="0.05"/>
         <param name="decimation" type="int" value="1"/>
         <param name="max_depth" type="double" value="4"/>
      </node>

      <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection stereo_nodelet" output="screen">
          <remap from="cloud"     to="/cloudXYZ"/>
          <remap from="obstacles" to="/obstacles_cloud"/>
          <remap from="ground"    to="/ground_cloud"/>

          <param name="frame_id"             type="string" value="base_footprint"/>		
          <param name="wait_for_transform"   type="bool" value="true"/>
          <param name="min_cluster_size"     type="int" value="20"/>
          <param name="max_obstacles_height" type="double" value="0.4"/>
      </node>  


    <group ns="planner">
        <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">
         <param name="controller_frequency" value="5" />  
         <rosparam file="$(find zed)/launch/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
         <rosparam file="$(find zed)/launch/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
         <rosparam file="$(find zed)/launch/config/local_costmap_params.yaml" command="load" />
         <rosparam file="$(find zed)/launch/config/global_costmap_params.yaml" command="load" />
         <rosparam file="$(find zed)/launch/config/base_local_planner_params.yaml" command="load" />  

      </node>
    </group>
    
</launch>

The biggest issue here is the ZED images, I've added stereo_view node above to debug it. Note how the disparity on the ground is sparse and noisy (not sure if it is a calibration issue). If the ground cannot be correctly computed, you may have false obstacles appearing in the map or in obstacles_detection.



There are some image artifacts on the top, and some images are just wrong ( which cause odometry lost, note how no disparity values can be computed):



Did you try using depth images computed by ZED SDK for a RGB-D setup instead of a stereo setup?

cheers

[Mercedes]

Hello, thanks a lot for your Reply.

Now it's better but we don't understand this things:
1) The global map use proj_map, but since proj_map is ok, global_map put black spots where doesn't exist obstacles in proj_map. That is possible?

2) The local_map use /obstacles_cloud to mark obstacles, but if we see where local_map put  spots like obstacles in the point cloud "/obstacles_cloud" there isn't, so we don't understand why are there.

Thanks a lot!

Some images:
- Proj_map
- Global_map
- Local_map

[matlabbe]

Hi,

1) If the global map uses an obstacle_layer plugin, it will add obstacles from the sensor on the map created by the static map plugin.

2) You can show up in RVIZ the /obstacles_cloud and the local costmap at the same time to debug this. The local map accumulates obstacles. Note that the ground cloud is very sparse and noisy (in comparison to stereo outdoor demo), so obstacles may accumulate in the local costmap as the robot moves. Having a better ground cloud, it would be possible to clear them, by adding the /ground_cloud in local costmap parameters:

observation_sources: point_cloud_sensor point_cloud_sensor2
point_cloud_sensor: {sensor_frame: base_zed, data_type: PointCloud2, topic: /obstacles_cloud, marking: true, clearing: true, min_obstacle_height: -99999, max_obstacle_height: 99999} 
point_cloud_sensor2: {sensor_frame: base_zed, data_type: PointCloud2, topic: /ground_cloud, marking: false, clearing: true, min_obstacle_height: -99999, max_obstacle_height: 99999} 

cheers

[Mercedes]

Thanks a lot! Sorry for so many questions.

Nos the global map is great.

The local map is empty, even seeing the obstacles_cloud well.
Include the following piece of code in local_costmap_params.yaml:

observation_sources: point_cloud_sensor
point_cloud_sensor: {sensor_frame: base_zed, data_type: PointCloud2, topic: /obstacles_cloud, marking: true, clearing: true, min_obstacle_height: -99999, max_obstacle_height: 99999}
And the launch is the same as you sent.
Any ideas?

Thank you!

[matlabbe]

Hi,

Make sure the indentation in local_costmap_params.yaml is ok, I just corrected two-spaces missing before observation_sources above:

<local_costmap_params.yaml>
local_costmap:
  global_frame: /odom
  robot_base_frame: /base_footprint
  update_frequency: 5
  publish_frequency: 2
  static_map: false
  rolling_window: true
  width: 6.0
  height: 6.0
  resolution: 0.05
  origin_x: -3
  origin_y: -3
  transform_tolerance: 10
  observation_sources: point_cloud_sensor
  point_cloud_sensor: {sensor_frame: base_zed, data_type: PointCloud2, topic: /obstacles_cloud, marking: true, clearing: true, min_obstacle_height: -99999, max_obstacle_height: 99999} 

Then you can look if /obstacles_cloud is connected to move_base through rqt_graph (after move_base has started to publish the costmaps). Note that obstacles over 3 meters won't appear in the local costmap (which is size 6x6).

cheers