Skip to content

[InflationLayer] inflation artifacts behind update bounds #2051

@AlexeyMerzlyakov

Description

@AlexeyMerzlyakov

The problem appears when apply some layer which is going after inflation_layer.
In my example, it is keepout_filter layer from this tutorial applied after inflation_layer in a global costmap as follows:

@@ -206,7 +210,7 @@ global_costmap:
-      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer", "keepout_filter"]

The costmap artifacts look like follows:
Screenshot_2020-10-20_18-51-39

This will look even worse when using two near keepout zones e.g. in a following warehouse example:
Screenshot_2020-10-20_19-02-41

In last case robot can not plan between two keepouts because of these artifacts.

The problem appears because of inflation layer in updateCosts() routine employs points out of costmap window (set in updateBounds()) in the following part of code:

void InflationLayer::updateCosts()
...
  // We need to include in the inflation cells outside the bounding
  // box min_i...max_j, by the amount cell_inflation_radius_.  Cells
  // up to that distance outside the box can still influence the costs
  // stored in cells inside the box.
  min_i -= static_cast<int>(cell_inflation_radius_);
  min_j -= static_cast<int>(cell_inflation_radius_);
  max_i += static_cast<int>(cell_inflation_radius_);
  max_j += static_cast<int>(cell_inflation_radius_);

Since LayeredCostmap does not know anything about it, it resets only bounds window between each iteration set by updateBounds() leaving untouched out-bounds-window. And, inflation layer re-inflates unnecessary points remaining from previous iteration which causes the artifacts from above.

I've started to check the history of the above part of code and found that corresponding borders change was made in:
ros-planning/navigation@7ef4d92
In this commit bounds expansion was removed from updateBounds() and added to updateCosts().
However, it was moved back later in:
ros-planning/navigation@9ee8580
and
ros-planning/navigation@e731bbf
Which looks strange because of the window shifting code in updateBounds() (where it actually should be) initially was removed, then was restored back, but another window shifting in updateCosts() is still in master. Since, window shift in updateBounds() restored back, it looks like the above part of code in updateCosts() is unnecessary. Moreover, I think by overall costmaps concept, updateCosts() should operate only inside bounds window without getting out.

Removing this part of code from updateCosts() seems to resolve the problem for me.
Any ideas or suggestion, could we remove it in master?

Required Info:

  • Operating System:
    • Ubuntu 20.04
  • ROS2 Version:
    • Foxy
  • Version or commit hash:

Metadata

Metadata

Assignees

No one assigned

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions