ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Clear obstacles of obstacle layer of costmap

asked 2020-03-17 07:21:27 -0500

Alessandro Melino gravatar image

updated 2020-05-25 03:54:27 -0500

Hello everyone.

I have a Realsense D435 camera on my application, and I use its depth_image to generate a laser scan topic with the package depthimage_to_laserscan. This works allright.

The problem I want to solve is when I generate obstacles with this laser topic, it marks me the obstacle but when I move and the obstacle get closer it updates the position of the obstacle but it does not lcear the previus marks of the obstacle.

Here you can see screenshots of the problems:

Far obstacle

In the image above the obstacle layer is just generating marks where the sensor sees an obstacle (green line). But in the image below the obstacle gets close and it generates the new marks, but the old marks are not being cleared.

Obstacle getting closer

I think is an error of configuration parameters of local_costmap, so here it is the code:


footprint: [[-0.30 , 0.38], [0.70, 0.38], [0.70, -0.38], [-0.30, -0.38]]

laser_layer: #Laser
  #track_unknown_space: false
  #footprint_clearing_enabled: true
  #combination_method: 1
  observation_sources:  laser
    topic: /scan
    sensor_frame: camera_depth_frame
    #observation_persistance: 0.0
    expected_update_rate: 0
    data_type: LaserScan
    clearing: true
    marking: true
    #max_obstacle_height: 2.0
    #min_obstacle_height: 0.0
    obstacle_range: 3.0
    raytrace_range: 5.0
    #inf_is_invalid: false

pointcloud_layer: #Nube de puntos
  #origin_z: 0.0
  #z_resolution: 0.2
  #z_voxels: 10
  #unknown_threshold: pointcloud_layer/z_voxels
  #mark_threshold: 0
  #publish_voxel_map: false
  #footprint_clearing_enabled: true
  observation_sources: pointcloud
    topic: /camera/depth/color/points
    sensor_frame: camera_depth_frame
    #observation_persistance: 0.0
    expected_update_rate: 0
    data_type: PointCloud2
    clearing: true
    marking: true
    #max_obstacle_height: 2.0
    #min_obstacle_height: 0.0
    obstacle_range: 3.0
    raytrace_range: 5.0
    #inf_is_invalid: false

  inflation_radius: 0.75
  #cost_scaling_factor: 10.0


    - {name: static_map,       type: "costmap_2d::StaticLayer"}
    - {name: laser_layer, type: "costmap_2d::ObstacleLayer"} #Laser sensors
    #- {name: pointcloud_layer, type: "costmap_2d::VoxelLayer"} #Pointcloud sensors
    #- {name: ultrasonic,       type: "range_sensor_layer::RangeSensorLayer"}
    - {name: inflation_layer,  type: "costmap_2d::InflationLayer"}

  update_frequency: 2.0 #HIGH CPU usage with sensors
  publish_frequency: 50.0 #Reducir para aligerar CPU

  global_frame: "odom" #To inflate obstacles
  robot_base_frame: "base_link"

  #static_map: false
  rolling_window: true
  width: 6.0 #6
  height: 6.0 #6
  resolution: 0.05 #0.01

  #always_send_full_costmap: true



    - {name: static_map,       type: "costmap_2d::StaticLayer"}
    #- {name: ultrasonic,   type: "range_sensor_layer::RangeSensorLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

  global_frame: "map"
  robot_base_frame: "base_link"

  update_frequency: 2.0 #HIGH CPU usage with sensors
  publish_frequency: 50.0 #Reducir para aligerar CPU

  resolution: 0.5 #0.01 #The resolution of the map in meters/cell.
  transform_tolerance: 0.2 #Specifies the delay in transform (tf) data that is tolerable in seconds
  map_type: costmap

  #always_send_full_costmap: true


#recovery_behavior_enabled: false
#clearing_rotation_allowed: false
controller_frequency: 10 #Default 20 took many time


  max_vel_x: 0.4 #meters/sec #0.6
  min_vel_x: -0.1
  max_vel_y: 0.0  # zero for a differential drive robot
  min_vel_y: 0.0  #radians/sec
  max_vel_theta: 1.0 #3
  min_vel_theta: -1.0
  min_in_place_vel_theta: 0.1 #radians/sec, in-place rotations
  escape_vel: -0.1 #0.1
  acc_lim_x: 0.4 #meters/sec^2
  acc_lim_y: 0.0  # zero for a differential drive robot
  acc_lim_theta: 1.0 #radians/sec^2

  holonomic_robot: false

   #####Trajectory Scoring Parameters#####

  meter_scoring: true #goal_distance and path_distance are expressed in units ...
edit retag flag offensive close merge delete


Please upload your images to the answer so they are retained.

stevemacenski gravatar image stevemacenski  ( 2020-03-17 11:37:10 -0500 )edit

The platform doesn't let me upload images because I am less than 5 points. Can't you see them using the links?

Obstacle far

Obstacle near

Here you are another links to see the images.

Thank you for your response.

Alessandro Melino gravatar image Alessandro Melino  ( 2020-03-18 04:44:05 -0500 )edit

Please attach the images to your question now. I've given you sufficient karma.

gvdhoorn gravatar image gvdhoorn  ( 2020-03-18 04:47:01 -0500 )edit

Thank you! It's done.

Alessandro Melino gravatar image Alessandro Melino  ( 2020-03-18 04:49:51 -0500 )edit

3 Answers

Sort by ยป oldest newest most voted

answered 2020-05-25 04:06:31 -0500

Alessandro Melino gravatar image

Finally I solved it using other type of layer (Spatio Temporal Voxel Layer) and generating a mark layer with the LaserScan topic with a voxel decay of 0.75 seconds. The configuration of the layer is the following:

  enabled:               true
  voxel_decay:           0.75     #seconds if linear, e^n if exponential
  decay_model:           0      #0=linear, 1=exponential, -1=persistent
  voxel_size:            0.05   #meters
  track_unknown_space:   true   #default space is unknown
  observation_persistence: 0.0  #seconds
  max_obstacle_height:   2.0    #meters
  unknown_threshold:     15     #voxel height
  mark_threshold:        0      #voxel height
  update_footprint_enabled: true
  combination_method:    1      #1=max, 0=override
  obstacle_range:        3.0    #meters
  origin_z:              0.0    #meters
  publish_voxel_map:     true   # default off
  transform_tolerance:   0.2    # seconds
  mapping_mode:          false  # default off, saves map not for navigation
  map_save_duration:     60     #default 60s, how often to autosave
  observation_sources:    rgbd1_mark
    data_type: LaserScan
    topic: /scan
    marking: true
    clearing: false
    #min_obstacle_height: 0.3     #default 0, meters
    #max_obstacle_height: 2.0     #defaule 3, meters
    expected_update_rate: 0.0    #default 0, if not updating at this rate at least, remove from buffer
    observation_persistence: 0.0 #default 0, use all measurements taken during now-value, 0=latest
    inf_is_valid: false          #default false, for laser scans
    clear_after_reading: true    #default false, clear the buffer after the layer gets readings from it
    voxel_filter: true           #default off, apply voxel filter to sensor, recommend on
    voxel_min_points: 0          #default 0, minimum points per voxel for voxel filter

Best regards. Alessandro

edit flag offensive delete link more

answered 2020-03-18 12:11:04 -0500

The old marks aren't being cleared

behind new observations.

The object moves toward the sensor in the 2nd frame. Since the object now obscures the sensor from seeing behind it, there is no simple way for the costmap to be updated to clear those occupied cells. The way for the sensor to update cells as "not occupied" is to have a sensing beam sent through the cells and not reflected back. This is what the clearing: true parameter sets in the costmap configuration files.

In the 2nd frame, the object moves closer so the sensing beams are reflected back but haven't had a chance to fly through the previously occupied cells. This may not be ideal for your use case, but in general its safer because if you cannot perceive what's behind an object, you can't make guarantees about whether or not that space is occupied.

If you want to track moving obstacles like that, it will require more work- recognizing it as a dynamic obstacle and updating the costmap cells directly.

edit flag offensive delete link more

answered 2020-03-18 12:20:08 -0500

Dragonslayer gravatar image

As it seems in the end this is about navigation, here is a link to a similar question, where the move_base clear_obstacles function comes into play, usualy used as recovery behavior. link text

edit flag offensive delete link more


Hello. Thanks for your answer.

Yes, I want to do that, call the service of move_base to clear the old layers. I tried it from terminal and it worked, but i want to do it every iteration. How can achieve it?

Alessandro Melino gravatar image Alessandro Melino  ( 2020-03-20 08:04:53 -0500 )edit

Iam not an expert on this, but as far as I understand it clear_costmap can be called as a service. Writing a simple node that calls the service on a fixed interval (does it really need to be EVERY iteration?) should work, or directly fidle with the move_base package itself, to call this behavior at controller frequency itself (might work better as you dont have to use services). But latency ROS thread priorities etc might mess things up. Depending on the speed of your robot, calling the service every several controller intervals might be good enough. If this is anything more than experimenting you should go for additional sensors and sensor fusion as clearing uncertain space in the robots path is hazardous.

Dragonslayer gravatar image Dragonslayer  ( 2020-03-20 13:00:04 -0500 )edit

Thank you. I understand it very well. I will try it and see if it works. Best regards.

Alessandro Melino gravatar image Alessandro Melino  ( 2020-03-23 03:20:04 -0500 )edit

Question Tools



Asked: 2020-03-17 07:21:27 -0500

Seen: 1,991 times

Last updated: May 25 '20