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

costmap not clearing obstacles

asked 2020-06-07 01:14:17 -0600

mizba gravatar image

updated 2020-06-07 03:01:33 -0600

Hi All,

I am using a stereo camera to detect obstacles but I am having issues with the costmap. The costmap sometimes doesn't clear the local obstacles on the costmap even after it leaves. Please find below the screenshot and also my costmap parameters yaml file. It would be great if someone can help me resolve this issue.

image description


global_frame: camera_baselink
robot_base_frame: left_camera_frame

footprint:  [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] #robot coordinates
footprint_padding: 0.1

update_frequency: 10.0
publish_frequency: 10.0

static_map: false
rolling_window: true
width: 20.0
height: 20.0
resolution: 0.05
track_unknown_space: false
always_send_full_costmap: true

  inflation_radius: 0.0
  cost_scaling_factor: 10.0

  observation_sources: obstacles
  obstacles: {
    data_type: PointCloud2,
    topic: /PointCloud_Publisher/pcl4,
    clearing: true,
    marking: true,
    observation_persistence: 0.0,
    max_obstacle_height: 0.5,
    min_obstacle_height: -0.5,
    obstacle_range: 10.0,
    raytrace_range: 11.0

- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflater_layer, type: "costmap_2d::InflationLayer"}
edit retag flag offensive close merge delete


Please attach your screenshot directly to your question. Do not use google drive/docs or something else.

gvdhoorn gravatar image gvdhoorn  ( 2020-06-07 02:26:42 -0600 )edit

I did not have enough karma so I couldn't upload the screenshot. I updated the post with the screenshot.

mizba gravatar image mizba  ( 2020-06-07 02:57:20 -0600 )edit

Hi, "doesn't clear the local obstacles on the costmap even after it leaves", the robot is stationary and the obstacle leaves or the other way arround? The costmap keeps obstacles that are out of sight of the sensor, when the sensor moved. Depending on your setup this is also true in 3d space. As your scan has sort of a "pyramid" shape. If there is tall obstacle in the wide base of this pyramid and the robot drives closer, part of the object fall out of the sensors pyramid/"cone", the obstacle parts that are now "out of sight" will persist even if the parts that can be seen by the sensor moved away. Ros doesnt know that a Human is a Human and that its head moves with the torse, for Ros its a bunch of unrelated obstacles. So if the bunch of obstacles in sight move ros ...(more)

Dragonslayer gravatar image Dragonslayer  ( 2020-06-07 10:34:19 -0600 )edit

Also this might help: link text

Dragonslayer gravatar image Dragonslayer  ( 2020-06-07 10:35:21 -0600 )edit

Hi, in my case the robot is stationary but the obstacles are moving in front of the robot.

if the costmap doesn't clear the obstacle from the map unless it comes in sight again or if there is another object behind it, wouldn't that make the path planning difficult since the planner sees obstacles that are actually not present at that place in the real world?

mizba gravatar image mizba  ( 2020-06-07 21:08:50 -0600 )edit

Iam not sure I understand you, or if I descibed the issue properly. If the camera frame stays the same, this effect isnt relevant as it marks and clears the same space all the time.

The navigation stack is a simple 2d approach, for 360° lidar, and research in mind. With a 360° sensor and neglecting obstacle and robot hight it makes perfect sense, as this kind of setup would "see" everything relevant all the time.

Dragonslayer gravatar image Dragonslayer  ( 2020-06-08 05:30:59 -0600 )edit

Moved to answer.

Dragonslayer gravatar image Dragonslayer  ( 2020-06-08 05:37:02 -0600 )edit

2 Answers

Sort by » oldest newest most voted

answered 2020-07-14 06:11:56 -0600

marting gravatar image

I had a similar issue where it doesn't clear the local obstacles on the costmap even after it leaves:

edit flag offensive delete link more

answered 2020-06-08 06:08:09 -0600

Dragonslayer gravatar image

Whats likely going on here is that costmap2d gets non valid data for clearing. This means that costmap gets valid data for the obstacle that is near, but if the obstacle moves away raytracing doesnt give back a valid (as costmap would like it to be formated) value for max range. If it only ocures sometimes it could be about relative reflectivity, which gets more of an issue with distance. In your picture it looks like there is no wall or similar behind the obstacles. You could try if costmap works with obstacles moving towards and backing up from the sensor and the issue only arrises if max_range comes into play.

You could try: - set paramter: inf_is_valid: true (put in line below raytrace_range:... in costmap_param.yaml) - next step would be to print the pointcloud and see if it fills "0.0" values for max range. You would then have to filter the pointcloud and transformint the 0.0´s to >obstacle_range and <raytrace_range. <a="" href="http://href="">href=" is a ros package for this out there, should be easy fo find. </raytrace_range.>

edit flag offensive delete link more


I tried adding the inf_is_valid: true to the costmap_params.yaml but costmap still doesn't clear the obstacles.

Also, I checked the values in the point cloud. There are no '0.0' values or 'inf' values.

The max_range of the point cloud values is 10 and I set the obstacle distance to 11, ray trace distance to 13, and tested it. still the same issue.

mizba gravatar image mizba  ( 2020-06-09 19:21:40 -0600 )edit

OK, I would set the obstacles range to 9 and raytrace range to 9.9 then. Is the costmap updating at all? If you bring in another obstacle does it register, what happens if the obstacles moves further away from the sensor does this work, or does it just "save" the nearest obstacle position/values and then thats it?

Dragonslayer gravatar image Dragonslayer  ( 2020-06-10 10:07:42 -0600 )edit

Also, move the plugins section in your yaml above the plugin layers definitions!

Dragonslayer gravatar image Dragonslayer  ( 2020-06-10 10:34:30 -0600 )edit

Hi as you suggested, I set the obstacles range to 9 and ray trace range to 9.9. I can see the costmap updating. If I bring in another obstacle, I can see it on the costmap. When the obstacle moves away from the sensor, the costmap updates but when all the obstacles leave from the field of view of the sensor costmap doesn't clear them.

So I converted point cloud to laser scan and set obstacle_range: 9.0, raytrace_range: 10.0, inf_is_valid: true, the costmap cleared obstacles on the map after they left the fov of the sensor. This wasn't the case with just point cloud as input to costmap.

mizba gravatar image mizba  ( 2020-06-11 11:51:21 -0600 )edit

So, now it works with laserscantopointcloud -> laserscan as obstacle layer source? Did you try with the plugins section moved before the layers definitions? I dont know if I asked before but did you ever try with positive obstacle_hight_min/max values? Did all this happen when the obstacles left fov sideways as well as by "walking away"? What I thought about what might have been an issue, is that because of distance the "rays" for raytracing might have become to sparse to clear all available cells, thus not clearing all obstalce cells, dont know why they would have marked in a different way though, maybe due to inflation layer.

Dragonslayer gravatar image Dragonslayer  ( 2020-06-11 12:14:39 -0600 )edit

UPDATE: Although its about navigation2 (ROS2) the issue seems true for ROS1. link text

Dragonslayer gravatar image Dragonslayer  ( 2020-06-13 14:20:27 -0600 )edit

Question Tools

1 follower


Asked: 2020-06-07 01:13:40 -0600

Seen: 2,619 times

Last updated: Jul 14 '20