Points on costmap layer after inflation layer might get inflated in edge cases

[jonipol]

Bug report

Points from layers defined after inflation_layer can be inflated in edge cases.

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • Humble (Docker image ros:humble)
• Version or commit hash:
  • d04de4e1d1f1b922c63b59133b8bb19d20e30cf3 (branch based on that hash to make Nav2 rolling to work on Humble with minimal changes <!-- from source: output of `git -C navigation2 rev-parse HEAD apt binaries: output of: dpkg-query --show "ros-$ROS_DISTRO-navigation2" or: dpkg-query --show "ros-$ROS_DISTRO-nav2-*" -->
  • ead48a9039c80cadb18dcfbb21fff0bdd496ef03 (A pure Humble branch which I can confirm this happening as well)
• DDS implementation:
  • CycloneDDS

Steps to reproduce issue

1. Set a layer to be after inflation_layer (tested obstacle_layer and "extra" static_layer) Global costmap params:

   plugins: [
   "static_layer",
   "inflation_layer",
   "obstacle_layer"
   ]

2. Drive close to an obstacle which is not marked on the bottom static_map but is visible on the layer below inflation (Detects only high cost points and no inflation is happening around those)

3. Back out slowly to about distance of raytrace_max_range

4. On the right distance the points might get inflated even tho they should not.

https://github.com/ros-planning/navigation2/assets/23654009/2e3446e0-cd35-44c7-b577-6703e0b32ecf

Expected behavior

Points of the layer below inflation should not be inflated

Actual behavior

Points at the distance of raytrace_max_range sometimes get inflated

Additional information

Reason for having a layer after the inflation_layer is that we have our own implementation of the keepout zones and we have wanted to inflate those areas bit differently.

Here is a image of the robot in middle of many keepout lines. (For visualization purposes) Here it is easy to see how the points on the "edge" are being inflated by inflation layer even tho they should not be. The cyan area is what our keepout lines/zones do not usually have in Rviz cost visualization.

Tried to test this on the turtlebot sim but could not get the sim to load the world on the provided devcontainer.

[SteveMacenski]

Have you looked into why this is happening, codewise?

@AlexeyMerzlyakov was poking around here in the last 2 years, maybe he has some insight?

[jonipol]

I took a quick look into the code but sadly could not find anything yet. I will see if I can find some time to dive deeper later this week.

[SteveMacenski]

From a quick scan: https://github.com/ros-planning/navigation2/blob/main/nav2_costmap_2d/plugins/inflation_layer.cpp#L216-L242

We find the costs in the master grid (i.e. all of them) adding in a margin for the inflation radius so that obstacles outside of the costmap have their inflations show up before being fully visible. That master grid includes all values, not just those below it.

This is actually one of the main reasons we have the costmap filters API separate from the costmap layers.

Reason for having a layer after the inflation_layer is that we have our own implementation of the keepout zones and we have wanted to inflate those areas bit differently.

It seems like that would meet you need as well.

[jonipol]

Thanks for quick responses and the suggestion. I was looking into this a bit today. Couldn't get much out of it yet, as I was bombarded with other things the whole day.

I believe we are resetting the combined_costmap before passing it to the plugins at https://github.com/ros-planning/navigation2/blob/main/nav2_costmap_2d/src/layered_costmap.cpp#L217. Assuming that I have not misunderstood how the iterator on the L218 works we should be going through the plugins in the order we added to the plugins_ array and that order is the order which they are listed on the config. Based on that we should not have the data of the layer defined after inflation when we are on updating costs on inflation_layer. Or am I missing something?

I also gave a try to the filters as you suggested. I added the Keepout Filter to my global costmap and added publishing of the info message to our implementation and got it to work. Looks that we should able to use the filters instead. Have to do a bit more testing on that. And maybe write a implementation without the FilterInfoCallback layer so we do not need to add dependency to nav_msgs to the piece of our code which handles the "keepout" zones.

[SteveMacenski]

Based on that we should not have the data of the layer defined after inflation when we are on updating costs on inflation_layer. Or am I missing something?

Look at the link to the inflation layer. It looks at the master_grid - ie the combined costmap. You're right to say that for a given iteration, the inflation layer wouldn't see data above it -- but the very next iteration then it would. What you're seeing is the inflation of obstacles marked in the global map the cycle after they're marked, not the same one. I believe.

Looks that we should able to use the filters instead.

Good :smile: I think that's actually the reason why we added the costmap filters as a separate object: to overcome this exact issue.

And maybe write a implementation without the FilterInfoCallback layer so we do not need to add dependency to nav_msgs to the piece of our code which handles the "keepout" zones.

As is your right, note that the actual costmap filter API does not require it. Our implementations happen to use it in order to generalize the keepout/speed/binary/etc information to be encoded arbitrarily. If you know your exact encoding and its never going to change, no need to overcomplicate your life, I suppose.

This good to close?

[jonipol]

Right, seems that I have misunderstood the ResetMap function. Thank you for the explanations and tips!