[InflationLayer] inflation artifacts behind update bounds

[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:

This will look even worse when using two near keepout zones e.g. in a following warehouse example:

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: https://github.com/ros-planning/navigation/commit/7ef4d926d97e001e5697e4de9c64433c3c5684b6 In this commit bounds expansion was removed from updateBounds() and added to updateCosts(). However, it was moved back later in: https://github.com/ros-planning/navigation/commit/9ee8580ce7f473f46456d8e9604aa7fde24116a8 and https://github.com/ros-planning/navigation/commit/e731bbf647cfaf68a4a4b8df664fc391d444ab62 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:
  • e1e1be4880a703c9fba44bb0ed68f4463d31eb95

[SteveMacenski]

The problem appears when apply some layer which is going after inflation_layer.

I'm a little confused on that one. I thought we made it so that the costmap filters can't impact the update window size? And this only happens when you have something after the inflation layer, no other time?

inflation layer in updateCosts() routine employs points out of costmap window

I think that's good behavior for the same reasons the comment points out.

it resets only bounds window between each iteration set by updateBounds() leaving untouched out-bounds-window.

I don't understand this comment, where is there a reset happening?

What's your suggested action to fix this? Move the min/max i/j to the other function? @mikeferguson maybe you can look at those Nav1 PRs and let us know if that was just an oversight reverting them.

[SteveMacenski]

@AlexeyMerzlyakov is this something we should address? I know you're busy with other tasks, so this shouldn't take priority, but just wanting to understand how critical this is to fix soon or if it can be pushed off?

[AlexeyMerzlyakov]

Currently, it is not burning problem or something that greatly restricts us. I think, it could be postponed for better times.

[SteveMacenski]

Makes sense, thanks!

[AlexeyMerzlyakov]

After some digging into nav_costmap_2d, I've found that artifacts could be divided into two types:

1. "out of bounds artifacts" caused by applying the inflation to out-of-boundary areas, which are not being cleaned between costmap_2d cycles.
2. "inbound artifacts" appeared on inner parts of boundaries and caused by previously added keepouts, or any other costmap information to the resulting master_grid, which at next cycle will be inflated inside the boundary (as mentioned above, inflation_layer take information from outside boundary areas to continuously inject in the inflated objects into boundary area).

The fist "out of bounds artifacts" is being fixed by the following patch:

diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp
index de60b342..1d2352bf 100644
--- a/nav2_costmap_2d/plugins/inflation_layer.cpp
+++ b/nav2_costmap_2d/plugins/inflation_layer.cpp
@@ -199,6 +199,10 @@ InflationLayer::updateCosts(
   // 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.
+  const int base_min_i = min_i;
+  const int base_min_j = min_j;
+  const int base_max_i = max_i;
+  const int base_max_j = max_j;
   min_i -= static_cast<int>(cell_inflation_radius_);
   min_j -= static_cast<int>(cell_inflation_radius_);
   max_i += static_cast<int>(cell_inflation_radius_);
@@ -251,12 +255,20 @@ InflationLayer::updateCosts(
       // assign the cost associated with the distance from an obstacle to the cell
       unsigned char cost = costLookup(mx, my, sx, sy);
       unsigned char old_cost = master_array[index];
-      if (old_cost == NO_INFORMATION &&
-        (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
+      // In order to avoid artifacts appeared out of boundary areas,
+      // we need to apply inflation layer only to inside of given bounds
+      if (static_cast<int>(mx) >= base_min_i &&
+          static_cast<int>(my) >= base_min_j &&
+          static_cast<int>(mx) < base_max_i &&
+          static_cast<int>(my) < base_max_j)
       {
-        master_array[index] = cost;
-      } else {
-        master_array[index] = std::max(old_cost, cost);
+        if (old_cost == NO_INFORMATION &&
+          (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
+        {
+          master_array[index] = cost;
+        } else {
+          master_array[index] = std::max(old_cost, cost);
+        }
       }

       // attempt to put the neighbors of the current cell onto the inflation list

Which eliminates all "out-of-bounds artifacts" as follows in the picture below:

The second type of "inbound artifacts" appeared if any costmap layer which have something new to add on and going after inflation_layer, will merge its costmap with master_grid. Between two costmap_2d cycles there is only boundary window being cleaned-up. Out-of-boundary information will be merged into master_grid as obstacles and will remain for next cycles. Inflation layer will perceive this new information also as obstacles on the next updateCosts() call and will inflate it inside the boundaries. This type of artifacts is a peculiarity inherent to such update cycles and could not be fixed in current costmap_2d design. We need to introduce a new (say "non-touch") attributes to costmaps and re-work costmap updating mechanism, which seems a bit hairy. In my opinion, this type of artifacts is rare and not common: it should appear only in cases when other layer bringing something new and not intended to be inflated, is going after inflation_layer.

So, I suppose to apply the fix for "out-of-bounds" inflation artifacts and leave "inbound" artifacts for now, until they won't be a real obstacle for something to do.

---

As an alternative "out-of-bounds artifacts" reproduction scenario, it might be a static_layer going after an inflation_layer for the global costmap:

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

This will produce the artifacts of first type as follows in the picture below (left part). The right part of image is how the global costmap looks after applying the patch from above:

[SteveMacenski]

So, I suppose to apply the fix for "out-of-bounds" inflation artifacts and leave "inbound" artifacts for now, until they won't be a real obstacle for something to do.

That seems like a reasonable starting point, I agree. You mention it was there in Nav1, removed, and readded. Can you look more into why they were readded and if those are going to also be issues for us here?

I understand the out of bound artifacts, but not the in-bound artifacts. I can follow your logic about why they happen, but its unclear to me why it applies to part of that upper block, but not at all inflated around the full block or lower block in future update cycles since they're now in the master costmap? It seems like if the inflation layer is reacting to the keepout zone now in the master costmap, wouldn't it all start to be inflated?

[AlexeyMerzlyakov]

That seems like a reasonable starting point, I agree. You mention it was there in Nav1, removed, and readded. Can you look more into why they were readded and if those are going to also be issues for us here?

Today I've looked through the history of change. It is pretty tangled. Initially, the bounds (or rather out-of-bounds) extension was added to inflation layer into updateCosts() in https://github.com/ros-planning/navigation/commit/37e734b9499a6f2ac5604872335ba2e367e92efb. However, as we already know this caused an artifacts appeared out-of-bounds which was described in https://github.com/ros-planning/navigation/pull/388 PR. That was fixed by removing the out-of-bounds extension, which was moved from updateCosts() to updateBounds() in order to cover maximum of previous and current window area plus inflation radius. This did help and artifacts disappeared. However, since the extension in updateCosts() was removed, the points (walls, obstacles) that are placing out-of-boundaries could not have an inflation influence to inside of window area. That caused inflated areas shears described in https://github.com/ros-planning/navigation/pull/469. This was fixed by returning back inflation extension into updateCosts() and decreasing the boundaries on inflation radius set by updateBounds(). The last caused again an artifacts, described later in https://github.com/ros-planning/navigation/pull/475. After that the boundaries extension was restored back for updateBounds().

However, it does not work for the example from above where something is placed after inflation layer. So, that is why I've suggested to still counting the out-of-bounds areas but not allowing updateCosts() to write there. If this will work as described, we do not need to keep the updateBounds() boundaries extension from previous step. As I understood, this was made to combat with artifacts from updateCosts() but for some cases it does not work.

[AlexeyMerzlyakov]

I understand the out of bound artifacts, but not the in-bound artifacts. I can follow your logic about why they happen, but its unclear to me why it applies to part of that upper block, but not at all inflated around the full block or lower block in future update cycles since they're now in the master costmap? It seems like if the inflation layer is reacting to the keepout zone now in the master costmap, wouldn't it all start to be inflated?

Yes, we can say that this is specific primarily for keepout_zones filter. After keepout zones went into master_costmap, they will be treated as usual obstacles on next cycle by inflation_layer. That is why in-boundaries artifacts will happen. As shown on picture above, the upper part of keepout zone is already went to master_costmap which is caused waste inflation into in-boundaries area. The lower part of keepout zone is placed inside boundaries window and being updated correctly (I mean not treated by inflation_layer as obstacle) since of keepout plugin is going after an inflation layer. The lower part of keepout zone won't be preserved at master_costmap since window area is being cleaned-up each time before for (plugin in plugins[]) { plugin->updateCosts(); } loop, which cannot be said about upper out-of-window-bounds area.

[SteveMacenski]

But in bound artifacts should impact the entire keepout block that's under the yellow line, why does it only show up on the "wings" of the block versus inflating the entire thing?

My understanding of why it happens in those 2 spots (or "wings") is that the keepout block is in the costmap on the next update. So the out-of-bounds part of the block makes the inbound on have to inflate on that boundary line. So why only those 2 points, why not the entire yellow line boundary (or is it and we only see the 2 points)? The in-boundary part of the block is still in the master grid the same way the out-of-bound is, why wouldn't that also inflate? There's no distinction anymore in the master grid where it came from.

That was fixed by removing the out-of-bounds extension, which was moved from updateCosts() to updateBounds() in order to cover maximum of previous and current window area plus inflation radius.

It also shows that it only updates that way when needs_reinflation_ is true, which only happens if the footprint changes or dynamic reconfigure occurs, which both don't typically happen at runtime in any cases we're considering. It looks like future PRs probably fixed that again.

Regardless, from that paragraph, am I to read that the state of the static layer is fully working when layers are placed before the static layer? It's really unclear after the tangling if there's only an issue if we place afterwards. If it does work, wouldn't your reverting of one of those changes then break the primary behavior of layers before the static layer? If it breaks anything from layers before the static layer, please explicitly mention what.

[AlexeyMerzlyakov]

But in bound artifacts should impact the entire keepout block that's under the yellow line, why does it only show up on the "wings" of the block versus inflating the entire thing?

This is being overwritten by lower part of keepout zone, which values are higher than inflated (from upper part of keepout zone) values. Therefore, there are only remaining two "wings" at keepout & window boundaries intersection.

The in-boundary part of the block is still in the master grid the same way the out-of-bound is, why wouldn't that also inflate?

Boundary window area will be cleaned by LayeredCostmap::updateMap() -> costmap_.resetMap(x0, y0, xn, yn); just before calling plugin->updateCosts() cycle. So, the inner (lower) part of keepout zone won't exist at the inflation_layer execution stage. Inside boundary window it will appear at the keepout_filter execution stage which goes after inflation_layer.

[AlexeyMerzlyakov]

Regardless, from that paragraph, am I to read that the state of the static layer is fully working when layers are placed before the static layer? It's really unclear after the tangling if there's only an issue if we place afterwards. If it does work, wouldn't your reverting of one of those changes then break the primary behavior of layers before the static layer? If it breaks anything from layers before the static layer, please explicitly mention what.

That was the main question to me. For additional analysis I've reproduced the initial problem described in the PR338 for navigation1. These "ghost" artifacts appeared because of insufficient boundary window area clean-up from previous step when robot starting to move. The fix which enhancing the coverage of maximum from previous and current window area plus inflation radius just intended to do that. I've drawn the picture below explaining how this "ghosts" artifacts are appearing: The analysis shown that the "ghost" problem has another than "out of bounds artifacts" root cause. The "ghosts" appears because of insufficient boundary window cleaning while "out of bounds artifacts" artifacts related to the case when some plugin going after inflation layer. Testing confirms that: the reverting a part of original patch will cause "ghost" problem to return back again. So, the patch for the current issue won't have a previous code reverting parts.

---

Just for reference, how to reproduce the "ghost" artifacts problem today (suddenly it will be useful to someone in the future):

1. Using global_costmap. The plugins are going in standard sequence ("static_layer", "obstacle_layer", "inflation_layer")
2. Using a modified ObstacleLayer: reset master costmap master_grid.resetMap(min_i, min_j, max_i, max_j); in the beginning of ObstacleLayer::updateCosts() cycle
3. Bring InflationLayer to before-PR338 state: remove all logic related to last_* variables and boundaries extention in InflationLayer::updateBounds(), leaving InflationLayer::updateCosts() to be untouched.

[AlexeyMerzlyakov]

Regarding "wings" artifacts: I've started to experiment with priority layers. These layers that are always will lie on top of non-priority ones. In this case all normal layers' plugins are not applied on them. The code logic could look like below (this is not well-worked code snap, just made to show how this could be handled):

-  costmap_.resetMap(x0, y0, xn, yn);
+  normal_costmap_.resetMap(x0, y0, xn, yn);
   for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
     plugin != plugins_.end(); ++plugin)
   {
-    (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
+    if ((*plugin)->get_parameter("priority_layer") == "enabled") {
+      // 1. Save costmap_ into normal_costmap_
+      normal_costmap_ = costmap_;
+
+      // 2. updateWindow() at priority_costmap_ from normal_costmap_
+      updateWindow(priority_costmap_, normal_costmap_, x0, y0, xn, yn);
+
+      // 3. Do updateCosts() for this priority_costmap_
+      (*plugin)->updateCosts(priority_costmap_, x0, y0, xn, yn);
+
+      // 4. Set costmap_ as a priority one
+      costmap_ = priority_costmap_;
+    } else {
+      // 1. Do updateCosts() for normal_costmap_
+      (*plugin)->updateCosts(normal_costmap_, x0, y0, xn, yn);
+
+      // 2. Set costmap_ as normal_costmap_
+      costmap_ = normal_costmap_;
+    }
   }

This works for me (if set "keepout_filter" as a priority layer), but seems to be very ugly as it dramatically changes LayeredCostmap logic only for fixing "wings" artifacts. So here are just my thoughts about it. Are there any suggestions?

[AlexeyMerzlyakov]

Another workaround could be optionally (by ROS parameter, false by-default) refusing the boundaries extension in updateBounds() at inflation_layer. As discussed before, this will cause shears in costmaps. However, this will fix "wings" artifacts, as it will stop to using of out-of-boundaries merged keepouts. This workaround could be optionally enabled only in cases if someone's work will be getting affected by "wings". It seems more straight comparing to solution, described in previous message, but being rather a workaround.

[SteveMacenski]

I'd pull @DLu in at this point for a moment to get his thoughts.

David, the short of it is that we've found that if you add a costmap layer after the inflation layer, there are thing inflation artifacts across that layer even though its above the inflation layer. We're trying to find a solution to the 2 edge cases we've identified.

The sheering suggestion is probably not a good one, I don't want a parameter that enables someone to mess things up badly and blame the system.

From your suggestion, its unclear to me what updateWindow would be specifically doing and how it solves the issue (or why the else statement makes use of the normal_costmap_ at all when it could just use costmap_ in the call directly). normal_costmap_ = costmap_ and then you resize (?) the priority_costmap_ to match normal_costmap_ (which is just the costmap_), updateCosts() on it which is the same size (?) as the costmap_ and then just set it back to costmap.

So what does updateWindow() do and is that really a "priority" thing?

[AlexeyMerzlyakov]

updateWindow() method just copies {x0,y0}..{xn,yn} boundary window from normal_costmap_ into a priority_costmap_. This is something like shown in the picture below: The main idea here is that normal_costmap_ is the same as costmap_ was. priority_costmap_ on each cycle copies/overwrites {x0,y0}..{xn,yn} boundary window from normal_costmap_ (outside boundary window on a priority_costmap_ there will be priority objects applied from prev.cycles). At current cycle priority layer will be applied inside boundary window on a priority_costmap_. This will allow us to have a priority objects (such as keepout zones) only on priority layer. And yes, both normal_costmap_ and priority_costmap_ have the same sizes/to be resized as costmap_ did before. The costmap_ would be equal to normal_costmap_ or to priority_costmap_ depending on situation. When normal layers are applied, they are operating with normal_costmap_ having no priority information (which could cause "wings"). When all layers have been applied, the costmap_ to be set as priority_costmap_ but only for planner/controller usage, but not for next cycle. The snap above is very-early draft. I've putted it there just for reference to understand how it works. Currently it is under my estimation.

However, I personally do not like this new philosophy, as we need to change whole costmap update mechanism by introducing priority layers. So, the big question here: do we really need it in Nav2 stack or not (for now)?

[SteveMacenski]

I'd rather us think about this in terms of the a costmap_ which is the normal costmap-costmap and a filters_costmap_ containing the top level keepout / speed zones. Maybe rather than allowing users to specify the order of the keepout / speed zones, we ourselves specify they're always at the end and not included in that main layered costmap parameter set. I think thinking about it like that rather than a priority layer retains the layered costmaps as they are now, we're just adding a different set of costmaps that are stored separately for the purpose of combining for isolation of the costmap filters only (so basically 2 sets of layered costmaps running, but the second is only for the filters).

Either way, we'd be doing similar mechanics of copying one of them over the other for the master costmap to utilize. So before going into any of that in the previous paragraph, I think its valuable for us to profile what CPU impacts this has to have that extra copy around (or if we could still reduce to a single copy on the update logic somehow) for the global and local costmaps. If its super impactful, we shouldn't do this regardless so its moot to talk over the design. If its not significant or acceptable, then we should proceed with a design at least to fully flush out the options we have and if any make sense.

[AlexeyMerzlyakov]

@SteveMacenski, thanks for the idea! Seems good to me! I think this could be implementable by introducing new filters ROS2 array like plugins does. We could fill two arrays: plugins_ and filters_ in Costmap2DROS. Then call in LayeredCostmap all plugins_ first and then filters_ applied above. The code snap (main part) may look like that (very preliminary and non-optimized, say updateWindow() call could be avoided if filters_ array has 0-length):

void LayeredCostmap::updateMap()
...
  normal_costmap_.resetMap(x0, y0, xn, yn);
  for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
    plugin != plugins_.end(); ++plugin)
  {
    (*plugin)->updateCosts(normal_costmap_, x0, y0, xn, yn);
  }

  // Copy normal_costmap_ boundary window -> into costmap_
  updateWindow(costmap_, normal_costmap_, x0, y0, xn, yn);

  for (vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin();
    filter != filters_.end(); ++filter)
  {
      (*filter)->updateCosts(costmap_, x0, y0, xn, yn);
  }

Something similar I've draft today locally and checked CPU and memory consumption for local and global costmaps with enabled "keepout_filter". There are no notable changes in CPU or memory consumption overhead I've detected on https://github.com/aws-robotics/aws-robomaker-small-warehouse-world.git Gazebo simulation. I've estimated the performance by simply using ps -o %cpu,%mem command stick to planner_server and controller_server processes. Please let me know, if more deepen analysis is necessary.

[SteveMacenski]

That updateWindow has got to have some impact on CPU if we're copying over a large window in the global costmap and some known amount in the local costmap (dim_x * dim_y for rolling costmap). For a local costmap of 5x5 at 5cm, that would be 100x100 cells copied, which is going to have some impact on compute time. I'd also measure the compute time using chrono clocks to make a timer to print update times

[AlexeyMerzlyakov]

OK. I've measured using chrono the total LayeredCostmap::updateMap() time and costmap window copy time (actually updateWindow() is a cycle of memcpy-s). Costmap boundary window is being copied in average eating ~0.57% of total updateMap() execution time (max overhead reached to 2%). The experiments were proceeded on Amazon small warehouse Gazebo model for global costmaps where average window boundary size exceeded 100x100 cells. The code snap similar to listed above in previous comment was used for the experiments. You can refer to the detailed results in attached table perf.xlsx.

One principal question about design. Do we better to add new ROS2 filters field (like plugins):

plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
filters: ["keepout_filter"]
...
keepout_filter:
  plugin: "nav2_costmap_2d::KeepoutFilter"
  enabled: True
  filter_info_topic: "/costmap_filter_info"

Or is it better to just adding a filter attribute into a plugin' parameters:

plugins: ["static_layer", "obstacle_layer", "inflation_layer", "keepout_filter"]
...
keepout_filter:
 plugin: "nav2_costmap_2d::KeepoutFilter"
 enabled: True
 filter: True
 filter_info_topic: "/costmap_filter_info"

First approach seems to be more logical in terms of parameters API. However, actually costmap filters are plugins. This could cause some confusion. Both approaches are OK for me, but the second seems more reasonable in terms of understanding of plugins and filters nature by developers they will use them.

[SteveMacenski]

Do we better to add new ROS2 filters field (like plugins)

Yes, probably, since this would also be a parallel layered track. Putting them all in the same plugins field would be semantically incorrect since the keepout, if put anywhere other than last, would be layered last.

Can you test warehouse world with the laser parameters set to 25 meters? That's a pretty typical number for an industrial robot. That would increase the global update window size and give us a real indication of the CPU overhead in a practical system.

[DLu]

My apologies for the delay in responding, I've been on paternity leave for the past two weeks.

I haven't read through the entire thread, but think I get the basic gist. One key point that its worth restating is that the original layered costmap code was trying to achieve two similar but different goals simultaneously.

1. Creating a more modular version of the costmap_2d package that matches previous iterations of the costmap_2d package.
2. Creating a clean definition of the algorithm that was robust and extensible.

The inflation layer (and occasionally the footprint layer) drove a lot of the edge cases in how the algorithm worked. A single pass version (simultaneous updateBounds and updateValues) wouldn't work for the inflation layer.

I always meant to rewrite the costmap module for the robot_navigation stack, but never got around to it, partially because I got a little bogged down in writing tests for edge cases like this.

After writing all the above, I realized that this might have actually already been closed by @AlexeyMerzlyakov in #2259 ? So maybe you're all set.

[SteveMacenski]

We fixed one of the two edge cases in that situation, stopping inflation out of bounds, but we still have the issue of inflating layers after the inflation_layer within bounds. We call this the "wings" problem just how it appears to the user, but in fact it is inflating basically the boundary of a rolling costmap where there are marked obstacles from the upper layers (but to the user, you just see the 2 sides inflated, e.g. "wings"). From both Alexey and my analysis, we don't see a way around it without literally having separate master grids for layers on top of the inflation layer and combining them separately. Since this is primarily a concern for the costmap filters (keepout, speed zones), our current thought is to separate the costmap filters from the "normal" costmap and combine them in a way that the "normal" costmap never sees the filters.

Its not a great solution in my mind, but its a decent one to get the behavior that we want for the keepout/speed zones, although it remains to be an issue for any user that chooses to put anything above the inflation layer in the layered costmap (albeit not common). The only way around this I can see that wouldn't involve maintaining somewhat isolated costmaps would be to have the costmap also store the layer number as well as the cost so that the inflation layer would only queue cells below its own value. Once we hit that point, I start to wonder if we don't rewrite costmap 2d altogether to allow for templated cell information like grid_maps or what you worked on in robot_navigation. This is a relatively small problem though that might be just considered a "known issue" if we have the keepout/speed zones covered.

[AlexeyMerzlyakov]

Can you test warehouse world with the laser parameters set to 25 meters? That's a pretty typical number for an industrial robot. That would increase the global update window size and give us a real indication of the CPU overhead in a practical system.

I've increased the lidar range up to 25m. However, this did't give a required effect, so I've forcibly patched updateMap() routine so its boundaries window will always take the map size. Therefore, boundary window size was ~120 KCells at all iterations. As always, I've measured the performance for global costmap with enabled keepout_filter and "wings" artifacts fix. During ~170 seconds run on Amazon small warehouse (~170 iterations), average costmap copy time was ~21 ms us (with standard deviation = ~20.5 ms us) which is ~0.36% (±0.34% std.dev) from total updateMap() execution time.

More details of analysis are attached in perf_full_window.xlsx file.

[AlexeyMerzlyakov]

Added a WIP PR with a proposed version of fix for "wings".

[SteveMacenski]

A 21ms increase being only 0.36% means that the update takes 5833ms, which can't possibly be correct, these costmaps update far faster than 1 hz, let alone 0.2hz

[AlexeyMerzlyakov]

Sorry, there was a misprint. 21mks, not 21ms.

[SteveMacenski]

What's mks?

[AlexeyMerzlyakov]

In microseconds. Initially all performance measurements were taken in nanoseconds, and average costmap copy time was 21292 nanoseconds which is ~21 microseconds. That was my confusion about abbreviations. "ms" was lead to a misunderstanding about milliseconds. Actual time is converted to microseconds (mks or more proper "us") which is correct.

[SteveMacenski]

OK, that's definitely fast enough this is OK - and that's for a global costmap update with a 25m lidar (should be a pretty decently sized window)?

I haven't seen microseconds in mks before, usually I think of it as us or μs, but live and learn :laughing:

[SteveMacenski]

Thanks for your diligent work on this, I think this finally puts the keepout / speed zones features into top support. Thanks!