Costmaps shows free space at grid_map as obstacles

[ignatov]

Hello.

I am trying to use rtabmap for navigation of quadcopter (from here) in Gazebo. Quadcopter is equipped with kinect. It hovers at a height of 1.5 m. Global and local costmaps both show that free space at grid_map in front of copter is occupied with obstacles. During the movement, these cells sometimes remain occupied, sometimes become free.
Have I made any mistakes in rtabmap node or in .yaml files?
grid_map without global_costmap:



grid_map with global_costmap:



Rtabmap node in launch-file:

<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
	<param name="frame_id" type="string" value="base_link"/>
        <param name="subscribe_stereo" type="bool" value="false"/>
        <param name="subscribe_depth" type="bool" value="true"/>
        <param name="odom_frame_id" type="string" value="world"/>

	<remap from="depth/image" to="/quad/camera_/depth/disparity"/>
	<remap from="rgb/image" to="/quad/camera_/camera/image_raw"/> 
	<remap from="rgb/camera_info" to="/quad/camera_/camera/camera_info"/> 
		
        <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="5"/>
        <param name="Vis/MinInliers" type="string" value="10"/>
        <param name="Vis/InlierDistance" type="string" value="0.05"/>

        <param name="RGBD/LoopClosureReextractFeatures" type="string" value="true"/>
        <param name="Vis/MaxFeatures" type="string" value="500"/>
        
        <param name="grid_size" type="double" value="10.0"/>
        
        <remap from="grid_map" to="map"/>
</node>

cosmtap_common_params.yaml:

footprint: [[-0.2,-0.2],[-0.2,0.2], [0.2, 0.2], [0.2,-0.2]]
inflation_radius: 0.65
cost_scaling_factor: 10.0

obstacle_layer:
  obstacle_range: 5.5
  raytrace_range: 6.0
  max_obstacle_height: 0.7
  track_unknown_space: true

  observation_sources: point_cloud_sensor
  
  point_cloud_sensor: {
    sensor_frame: quad/camera__link,
    data_type: PointCloud2,
    topic: /quad/camera_/depth/points,
    marking: true,
    clearing: true
  }

global_costmap_params.yaml:

global_costmap:
  global_frame: /map
  robot_base_frame: /base_link
  update_frequency: 1.0
  publish_frequency: 1.0
  static_map: false
  plugins:
     - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}

local_costmap_params:

local_costmap:
  global_frame: /map
  robot_base_frame: /base_link
  update_frequency: 5.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 5.0
  height: 5.0
  resolution: 0.02
  tranform_tolerance: 0.5
  planner_frequency: 1.0
  planner_patiente: 5.0
  plugins:
     - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}

[TOM_RB]

Hi
had your problem solved or not ?
I have a similar one and i need your help

Thanks

[matlabbe]

The problem on costmap side could be that the obstacle layer is not in right frame:
sensor_frame: quad/camera__link

You may try to set sensor_frame to your base frame.

Also you may look at that mavros / 2d nav example here: https://github.com/matlabbe/mavros_nav_2d

cheers,
Mathieu

[TOM_RB]

 Hi matlabbe
i tried this but the problem still appears
also i try your advice in this post to add a ground cloud :
http://official-rtab-map-forum.206.s1.nabble.com/Problem-with-global-and-local-costmap-td1867.html
also nothing change.

in this picture , upon rotating the robot there is obstacles in the local_costmap , then after a while it disappears :





this is the launch file of the local_costmap :

local_costmap_params.local_costmap_params

here are the grid parameters of the rtabmap :

      param name="Grid/FromDepth"                    type="string"  value="true"
      param name="Grid/NoiseFilteringRadius"         type="string"  value="0.1"
      param name="Grid/NoiseFilteringMinNeighbors"   type="string"  value="10"
      param name="Grid/3D"                           type="bool"    value="false"
      param name="Grid/RayTracing"                   type="bool"    value="true"
      param name="Grid/MaxGroundHeight"              type="string"  value="-0.3"
      param name="Grid/MaxObstacleHeight"            type="string"  value="0.3"
      param name="Grid/NormalsSegmentation"          type="string"  value="true"
      param name="Grid/FlatObstacleDetected"         type="bool"    value="false"
      param name="Grid/GroundIsObstacle"             type="bool"    value="false"
      param name="Grid/RangeMax"                     type="string"  value="5.0"
      param name="Grid/RangeMin"                     type="string"  value="0"
      param name="Grid/MapFrameProjection"           type="bool"    value="true"


cheers

[matlabbe]

Is the yellow point the cloud an obstacle cloud? If so, it seems it failed to correctly segment the ground, and labelled the ground as an obstacle. From the top view it is difficult to see the height of the yellow point cloud, is it at the rviz grid level? Can you show a side view of the yellow point cloud? Because if the ground cloud is outside those ranges:

param name="Grid/MaxGroundHeight"              type="string"  value="-0.3"
param name="Grid/MaxObstacleHeight"            type="string"  value="0.3" 

it will be labelled as obstacle. A max obstacle height max ground height of -0.3 means that if the point cloud is over -30 cm from the base link of the robot, it will automatically labelled as obstacle.

[TOM_RB]

Hi Matthieu,
Apologies for the late reply.

1 - yes , the yellow point the cloud as an obstacle cloud. alse the green (one for the right and the other for the left camera) - the red points to the ground cloud
2 - I did not understand the mean of " ground segmentation" , i think you mean considering a ground level ?
     is this parameter :  Grid/NormalsSegmentation    with   value="true"
     enough for segmentation?
3 - From the top view it is difficult to see the height of the yellow point cloud, is it at the rviz grid level?  
     yes the yellow point cloud is of height 0.6 ( 0.3 above the grid level and 0.3 below it - the grid level is at the same level of the map frame.
4 - param name="Grid/MaxGroundHeight"              type="string"  value="-0.3"
     param name="Grid/MaxObstacleHeight"            type="string"  value="0.3"
       about these two parameter ,
     the base link is at a height of 0.36 from the floor (ground)
     the 3 camera (D435)  are at the same level of the base_link
     I choose MaxObstacleHeight with a value 0.3 ( i think you mean 0.3 not -0.3 as you mention in the previous reply ) and the Grid/MaxObstacleHeight with -0.3 to consider obstacles 30 cm below the base link and 30 cm above it.
so points below than 6 cm i did not want them to appear as obstacles in order to ignore small obstacles on the ground.
that is how I understand these parameters.






Thanks

[matlabbe]

Hi,

I corrected my message, I meant "A  max ground height of -0.3 means that if the point cloud is over -30 cm from the base link of the robot, it will automatically labelled as obstacle."

You could set Grid/NormalsSegmentation to false if you are only interested to filter the cloud +-30 cm around the xy plane at the height of map frame.

Note also that if you are looking for a local costmap at the height of the drone, you may check this example: https://github.com/matlabbe/mavros_nav_2d

cheers,
Mathieu