Dealing with dynamic obstacles that get 'stuck' in costmap

[orduno]

Bug report

Required Info:

• Operating System: Ubuntu 16.04
• Installation type: source
• Version or commit hash:
• DDS implementation: FAST-RTPS`
• Client library (if applicable):

Steps to reproduce issue

Launch nav stack in simulation using your prefered method

Make sure obstacle layer is ON in `nav2_params.yaml'

global_costmap:
  global_costmap:
    ros__parameters:
      obstacle_layer:
        enabled: True
      always_send_full_costmap: True
      observation_sources: scan
      scan:
        topic: /scan
        max_obstacle_height: 2.0
        clearing: True
        marking: True

Make sure clearing and marking are True

Manually drop an object in gazebo, i.e. cardboard box, then manually remove the object.

Expected behavior

When the object is added, the obstacle layer in costmap should increase the cost of cells where the dynamic object was placed, likewise it should completely remove the cost once the object is removed.

Actual behavior

The cost is not completely clear once the object is removed. See video below.

Additional information

[orduno]

Here's a video showing the issue:

[SteveMacenski]

You found the issue with the voxel layer resulting in my STVL replacement package. I dont think you actually have any errors here, that's just how it works.

[orduno]

Thanks for the feedback. Is it because the laser beam separation prevents from detecting if a small region (between the beams) is unoccupied after the object is gone?

The object is placed fairly closed to the robot. Seems the laser density should be enough to detect that the region is free.

[SteveMacenski]

Yes, there are problems the discrete raycasting algorithm that the obstacle layer and voxel layer use under the hood. If you used a hokuyo or something dense enough for 2D you wouldn't have an issue but for 3D it becomes a real headache

[meiqua]

We met this problem in ros1 navigation using 2D laser too.
I think it's due to package laser_geomery. In obstacle layer, laser_geomery is called to transform scan to point cloud. However, when there is no obstacle in a scan line, the scan data will be set as range_max, which is dropped directly by laser_geomery. So raytraceFreespace can't clear previously existing obstacle in that line. You can notice that in the video when the box is removed, only the obstacle which has another obstacle at back are cleared.
In our project, we solve this by increasing sensor_msgs/LaserScan range_max(data left unchanged), so data won't be dropped by laser_geomery. Maybe customizing laser_geomery is a better way.

[HappySamuel]

We met this problem in ros1 navigation using 2D laser too. I think it's due to package laser_geomery. In obstacle layer, laser_geomery is called to transform scan to point cloud. However, when there is no obstacle in a scan line, the scan data will be set as range_max, which is dropped directly by laser_geomery. So raytraceFreespace can't clear previously existing obstacle in that line. You can notice that in the video when the box is removed, only the obstacle which has another obstacle at back are cleared. In our project, we solve this by increasing sensor_msgs/LaserScan range_max(data left unchanged), so data won't be dropped by laser_geomery. Maybe customizing laser_geomery is a better way.

Hi

I am facing this issue as well. Regarding the range_max workaround that you mentioned, where can i find this range_max setting?

Best,
Samuel

[NorrieF]

We met this problem in ros1 navigation using 2D laser too. I think it's due to package laser_geomery. In obstacle layer, laser_geomery is called to transform scan to point cloud. However, when there is no obstacle in a scan line, the scan data will be set as range_max, which is dropped directly by laser_geomery. So raytraceFreespace can't clear previously existing obstacle in that line. You can notice that in the video when the box is removed, only the obstacle which has another obstacle at back are cleared. In our project, we solve this by increasing sensor_msgs/LaserScan range_max(data left unchanged), so data won't be dropped by laser_geomery. Maybe customizing laser_geomery is a better way.

Hi

I am facing this issue as well. Regarding the range_max workaround that you mentioned, where can i find this range_max setting?

Best, Samuel

Hi @meiqua, @HappySamuel,

Did you solve this already? I'm not sure if the suggested solution works, because if laser_geometry drops the data set as range_max, how could increasing the latter make any difference (it would be dropped anyway, wouldn't it)?

I would appreciate your comments!

Best,
Norrie

[meiqua]

We met this problem in ros1 navigation using 2D laser too. I think it's due to package laser_geomery. In obstacle layer, laser_geomery is called to transform scan to point cloud. However, when there is no obstacle in a scan line, the scan data will be set as range_max, which is dropped directly by laser_geomery. So raytraceFreespace can't clear previously existing obstacle in that line. You can notice that in the video when the box is removed, only the obstacle which has another obstacle at back are cleared. In our project, we solve this by increasing sensor_msgs/LaserScan range_max(data left unchanged), so data won't be dropped by laser_geomery. Maybe customizing laser_geomery is a better way.

Hi
I am facing this issue as well. Regarding the range_max workaround that you mentioned, where can i find this range_max setting?
Best, Samuel

Hi @meiqua, @HappySamuel,

Did you solve this already? I'm not sure if the suggested solution works, because if laser_geometry drops the data set as range_max, how could increasing the latter make any difference (it would be dropped anyway, wouldn't it)?

I would appreciate your comments!

Best, Norrie

Yes, that's because the scan data is set as range_max(old), which is smaller. You can have a try.

[NorrieF]

We met this problem in ros1 navigation using 2D laser too. I think it's due to package laser_geomery. In obstacle layer, laser_geomery is called to transform scan to point cloud. However, when there is no obstacle in a scan line, the scan data will be set as range_max, which is dropped directly by laser_geomery. So raytraceFreespace can't clear previously existing obstacle in that line. You can notice that in the video when the box is removed, only the obstacle which has another obstacle at back are cleared. In our project, we solve this by increasing sensor_msgs/LaserScan range_max(data left unchanged), so data won't be dropped by laser_geomery. Maybe customizing laser_geomery is a better way.

Hi
I am facing this issue as well. Regarding the range_max workaround that you mentioned, where can i find this range_max setting?
Best, Samuel

Hi @meiqua, @HappySamuel,
Did you solve this already? I'm not sure if the suggested solution works, because if laser_geometry drops the data set as range_max, how could increasing the latter make any difference (it would be dropped anyway, wouldn't it)?
I would appreciate your comments!
Best, Norrie

Yes, that's because the scan data is set as range_max(old), which is smaller. You can have a try.

Thanks @meiqua for your quick reply!

Actually, this hearkens back to Samuel's question, which is "where is the range_max setting?"

In my case, the local_costmap subscribes to /scan topic published from depthimage_to_laserscan_node.

So I tried increasing the range_max parameter of the depthimage_to_laserscan package, but it didn't make any difference (as I anticipated in my post yesterday).

I suppose you are talking about a range_max parameter of some other place like a sub-package in navigation2.

I would appreciate if you could tell me where it is.

Thank you in advance for your help!!

Best,
Norrie

[NorrieF]

We met this problem in ros1 navigation using 2D laser too. I think it's due to package laser_geomery. In obstacle layer, laser_geomery is called to transform scan to point cloud. However, when there is no obstacle in a scan line, the scan data will be set as range_max, which is dropped directly by laser_geomery. So raytraceFreespace can't clear previously existing obstacle in that line. You can notice that in the video when the box is removed, only the obstacle which has another obstacle at back are cleared. In our project, we solve this by increasing sensor_msgs/LaserScan range_max(data left unchanged), so data won't be dropped by laser_geomery. Maybe customizing laser_geomery is a better way.

Hi
I am facing this issue as well. Regarding the range_max workaround that you mentioned, where can i find this range_max setting?
Best, Samuel

Hi @meiqua, @HappySamuel,
Did you solve this already? I'm not sure if the suggested solution works, because if laser_geometry drops the data set as range_max, how could increasing the latter make any difference (it would be dropped anyway, wouldn't it)?
I would appreciate your comments!
Best, Norrie

Yes, that's because the scan data is set as range_max(old), which is smaller. You can have a try.

Thanks @meiqua for your quick reply!

Actually, this hearkens back to Samuel's question, which is "where is the range_max setting?"

In my case, the local_costmap subscribes to /scan topic published from depthimage_to_laserscan_node.

So I tried increasing the range_max parameter of the depthimage_to_laserscan package, but it didn't make any difference (as I anticipated in my post yesterday).

I suppose you are talking about a range_max parameter of some other place like a sub-package in navigation2.

I would appreciate if you could tell me where it is.

Thank you in advance for your help!!

Best, Norrie

Hi @meiqua,

I am converting depth_image from realsense, so I changed the max range of that data and it worked!
Thanks for your help again!

Best,
Norrie

[meiqua]

Glad it was helpful!