Keepout Filter: Reduce lethal to high cost when robot is already in the zone

[SteveMacenski]

Its possible due to localization jumps or other behavioral servers taking control that the robot may enter a keepout zone. If the behavior isn't changed, the robot will be deadlocked in that zone.

So we can:

• Toggle the zones on/off - but that gives the robot free reign to hang out in the zones that the user really wasn't want it in
• Reinterpret lethal cost to simply very high cost to allow the robot to leave the zone, once out of the zone, re-enable lethality
• Adjust the costmap APIs to have transitions from A->B rather than simply cell costs, whereas if the starting point A is already in a keepout zone, then it doesn't set it as lethal. That would allow the robot to leave keepout zones but not enter them intentionally
• Likely others

[leander-dsouza]

Yes, I think if the robot is in a keepout zone, the costmap could be updated to a value less than the lethal cost for escaping.

I guess that this can be done by getting the robot's position in the process() function of the keepout filter, and updating the master_array accordingly.

[SteveMacenski]

I think so too!

[BCKSELFDRIVEWORLD]

Absolutely a very reasonable idea @leander-dsouza , but I believe this should be implemented not as part of the keepout layer but rather as a separate layer or filter. The reason robots end up inside keepout zones is often due to localization errors or other unexpected situations. Allowing movement within these zones could pose significant safety risks depending on the context.

We could make this behavior parameterized; however, doing so would activate it for all defined keepout areas, which might not be desirable in every case. I believe users should have more flexibility and control in deciding where this behavior applies. Therefore, implementing it as a standalone layer would be cleaner and safer.

[SteveMacenski]

We could make this behavior parameterized;

I think regardless of implementation details, it would be parameterized. I agree that it should be hard enforcable if users want it to be (even if it means a potential deadlock condition that they need to otherwise manually handle themselves).

doing so would activate it for all defined keepout areas, which might not be desirable in every case

I did think of this, but thought that the risk / reward was worth it as long as the cost set of the keepout zones was still very high (i.e. 200+) so planning / control would still be incredibly incentivized not to go onto those areas anyway. Then once out of the keepout zone it was violating, it would go right back to being lethal space so the very next planning / control iteration would find a new solution. I thought that sounded OK since the behavior would immediately revert.

A solution to that would be to have the different polygons that make up the zones be enumerated and we only modify the one that is currently being violated. That would complicate the implementation details a bit (store polygons, check which is violated, only modify the costs of that one polygon's representation on the costmap), but might be nice feature.

Thoughts?

[BCKSELFDRIVEWORLD]

I think we’re missing the core question here: Why is the robot entering the keepout area ?
If users have properly defined keepout areas in both local and global costmaps, and the robot still goes into them, that likely points to a bug

In cases where the robot enters a keepout area because of small localization jumps or tight environments, we already have a known workaround: the footprint_clearing mechanism. This is used in other layers to clear costs near the robot's footprint and could be extended here to help the robot get unstuck safely

In my experience working with robots in real-world environments, temporarily lowering the cost of a keepout area, even in a limited and reactive way, can be risky. This is especially true in areas where people or sensitive equipment are present.

For this reason, I believe this feature should be implemented as new layer or filter but not as part of the existing keepout layer #:)

A solution to that would be to have the different polygons that make up the zones be enumerated and we only modify the one that is currently being violated. That would complicate the implementation details a bit (store polygons, check which is violated, only modify the costs of that one polygon's representation on the costmap), but might be nice feature.

It's definitely an interesting idea to update only the specific polygon the robot is currently inside If the approach is to draw separate masks for each keepout area using PGMs per layer, I'm not sure that's going to be scalable. It would quickly become difficult to manage if each polygon requires its own file. I assume the idea behind this would eventually lead to some sort of GUI-based editor to make it easier to define and manage these areas.

Honestly, just writing this out gets me excited. The idea of Nav2 having its own native GUI editor for costmap layers sounds amazing. Definitely something we need, Steve. Please make it happen :)

[SteveMacenski]

Why is the robot entering the keepout area ?

A couple of thoughts:

• Localization corrections
• Zones are dynamic by a scheduler independent of any individual robot's state
• Zones for controllers are different from zones for planners

that likely points to a bug

I don't necessarily agree, though I would generally agree. There are applications where I think it may happen, especially in prototyping. Having things that work well even when localization is not 100% perfect is important too for different phases of development and deployment. I'm not saying that its good that it happens, but I'm just taking the practical position that this is an obvious and foreseeable deadlock condition that there needs to be some solution to get out of -- otherwise alot of people are going to be in for a bad time (and I know for example from discussions with other companies things like this exist in commercial deployments; whether it be A->B cost traversal including keepout knowledge, toggling on/off, etc). We should have some workflow or method for getting out of the deadlock without human intervention as a practical matter.

the footprint_clearing mechanism

This is good if you're barely straddling the keepout zone, but not suitable if you're in it by > 1 cell since no expansion from that pose can find a non-lethal pose. See my thoughts above for situations that this could happen and have non-trivial inclusion into zones for.

But, that is a direction I hadn't considered, maybe there's a more aggressive clearing that could be made to open up a bubble of keepout zone around the robot to allow it to get out of the zone. That feels complex though - but worth exploring if you had a design outline! Sizing the bubble and the policy behavior that creates would need to be flushed out (i.e. force the robot to take the shortest path to leave the keepout zone immediately, consider the task and take the shortest path that gets back into the route while going through the zone, etc). Push comes to shove, I think that's very similar to just setting the cost from 254 to 250 and letting the planner's tuned behavior decide how it wants to handle the situation: get out immediately if cost penalties are high or get out very soon, but consider the task at hand to weight the path in high cost vs progress towards the goal trade off.

I believe this feature should be implemented as new layer or filter but not as part of the existing keepout layer

What's the concern if its in the existing layer but it is parameterized if you want to use it or not? You'd have the option of not allowing the deadlock override to happen and it always staying lethal. I'm not suggesting you have to use it, only that its an option.

I'm not sure that's going to be scalable

There was some partially completed work on a Vector Object Server that would make this actually very reasonable and scalable, but that is not yet completed and perhaps something I'd actually prefer to stay dead until we do a larger costmap refactor / rewrite to include that in the core feature since it feels like we keep strapping things onto Costmap2D to let it keep limping along. I agree though that this isn't scalable for costmap 2d right now and my thought process there was tainted about knowing about the vector object server work (and forgot that we never merged it).

The idea of Nav2 having its own native GUI editor for costmap layers sounds amazing.

There's actually been alot of work in that direction in the not so distant past: #2113. We just never got it into the stack but it was very close. If someone was interested in picking it up and bringing it to fruition, I'd be happy to give it a detailed look again to see the gaps or things needed to be added so it can be in the stack: https://github.com/TomasHricina/navigation2/tree/main/nav2_map_editor.

These typically aren't the things I personally work on. Most commercial users will have their own GUI for robot operations and fleet management so hour-for-hour its not a great use of my time when I can be working on things that all users can / will benefit from. I agree that these are great ideas and very helpful utilities indeed though! I just need community support / help to make them happen 😄

[BCKSELFDRIVEWORLD]

What's the concern if its in the existing layer but it is parameterized if you want to use it or not? You'd have the option of not allowing the deadlock override to happen and it always staying lethal. I'm not suggesting you have to use it, only that its an option.

If this feature is implemented only as a parameter within the existing layer, I personally wouldn’t be able to use it effectively. While it might seem like a personal request, I believe this highlights a broader and objective issue that affects many users.

In real-world deployments, many companies build robots without ground-facing sensors to reduce costs, often relying on the cheapest available options. Most use a single, upward-facing depth camera, and radar-like sensors are usually not practical due to their cost and integration complexity. As a result, robots often assume the ground is flat and safe and are unable to locally detect hazardous areas like stairwells.

If the robot experiences a localization jump and realizes that it has ended up inside a keepout zone, forcing it to autonomously exit that area can be extremely dangerous.

In such scenarios, the safest approach is not to let the robot recover by itself, but to trigger an emergency support mode configured by the developer or company. For example, the system could send an SMS or another type of alert to notify an operator or responsible personnel. (Nav2 should not be responsible for handling this logic.)

Yes, disabling this behavior through a parameter is one possible solution. However, I personally use this feature as a separate costmap layer, but only outside the areas where the keepout layer is active. In other words, I use both layers simultaneously, but in different regions. This allows me to strictly enforce keepout behavior in high-risk zones such as stair edges, while still allowing the robot to exit from lower-risk, high-traffic areas where remaining stuck would impact performance and cause disruption.

I believe all Nav2 users should be able to benefit from both options at the same time. Rather than enforcing a single rigid usage model, offering support for an alternative layer gives users the flexibility to choose the approach that best fits their environment.

Of course, adding a new layer brings some CPU overhead. But in this case, I strongly believe that the flexibility and safety benefits far outweigh the cost.

These typically aren't the things I personally work on. Most commercial users will have their own GUI for robot operations and fleet management so hour-for-hour its not a great use of my time when I can be working on things that all users can / will benefit from. I agree that these are great ideas and very helpful utilities indeed though! I just need community support / help to make them happen

I'm still a senior-year engineering student, and for the past year I've been actively working on Nav2-related developments. I just wanted to say — you’ve played a big role in making open source exciting and inspiring for me, and for many of my friends as well.
Following your architectural insights and design decisions in issues and discussions has been both enjoyable and incredibly educational for me. I genuinely read every message with great excitement.

When I said "Please make it happen", I meant it as a way to express my hope that Nav2 developers might prioritize that part of the stack. I know there are some amazing contributors in the Nav2 repo, and I really admire the community around it.

Thanks again for all the work you do — it means a lot to people like me who are learning and growing through this project.

[SteveMacenski]

I guess to try to shorten responses, brass tacks, what do you think we should do specifically & its deployed workflow? I understand that you wouldn't want the robot to automatically recover for your application, but what do you suggest then for others' applications where they cannot send an SMS, the robot has to recover itself.

I personally use this feature as a separate costmap layer, but only outside the areas where the keepout layer is active. In other words, I use both layers simultaneously, but in different regions. This allows me to strictly enforce keepout behavior in high-risk zones such as stair edges, while still allowing the robot to exit from lower-risk, high-traffic areas where remaining stuck would impact performance and cause disruption.

I don't understand this fully. I'm not sure what "this feature" or "both layers simultaneously, but in different regions" are to mean -- or how they tie together when 'only outside the areas where the keepout layer is active' 😄

I'm suggesting the following:

• A new parameter to allow the 'I'm in a keepout zone and I shouldn't be, reduce LETHAL to very high (another parameter, but lets say 220 for argument's sake)'
• If you don't want to use it, you can disable it and not have it do anything, which gives you the same behavior as today
• That lets a tuned planner/controller combo decide how it wants to leave the zone it shouldn't have been in, whether it decides to weight leaving high cost areas immediately or weights the task at hand within it. Up to the controller/planner's tunings so the users can create their own optimal behavior
• This removes a foreseeable deadlock condition in a reasonably approachable way, certainly with room for potential future improvement, but gets past the core issue 'good enough' for many

---

On the GUI, I think you're in a great spot to potentially take that on and bring it to completion if you were so interested! That's a really nice resume item and a major contribution to the stack with lasting impacts 😉

[BCKSELFDRIVEWORLD]

Sorry if I took your time by explaining in detail. Let me put it simply:

The parameter-based solution you suggested forces the user to choose only one behavior—either the original keepout or the new, lower-cost recovery option.But I believe it would be more useful if users could use both behaviors at the same time when needed. In other words, instead of forcing a choice through a single parameter, users should be able to benefit from both behaviors in different situations as needed.If a more flexible solution is considered in the future, I’d be happy to contribute.

[SteveMacenski]

either the original keepout or the new, lower-cost recovery option

Either
a. keepouts = lethal always or
b. keepouts = lethal always except if the robot is within one, then set to just under lethal (or a value of your choice)

How can you use both behaviors at once? How would separating a layer make it easier to switch between one and another that a parameter wouldn't (i.e. set the Costmap2D parameter to include second layer or set a bool parameter within the layer about allowing options A/B or not). It seems like setting a toggle is easier to track than including a second layer sometimes and other times not -- and doesn't seem as dynamically reconfigurable.

What exactly do you see the second layer doing differently and/or situations you'd toggle on/off that a boolean parameter wouldn't allow for just as easily?

Logistical question: How would you know in the other layer which LETHAL zones are keepouts versus lethal from sensed obstacles, other costmap filters, or other layers to make sure you don't set real LETHAL obstacles as non-lethal for this mode?

[BCKSELFDRIVEWORLD]

From my perspective, as I’ve mentioned several times above, there can be dangerous areas where, if the robot enters (for example, due to a localization jump or a similar error) and gets stuck inside by mistake, even trying to leave that area can be more dangerous.

What exactly do you see the second layer doing differently and/or situations you'd toggle on/off that a boolean parameter wouldn't allow for just as easily?

This is because there could be obstacles or stairs in the robot’s blind spots, and any sudden movement the robot makes to exit as quickly as possible might actually increase the risk of an accident.
In some cases, it’s actually safer for the robot to stay put rather than attempt to leave immediately.

However, there are also other areas where I absolutely don’t want the robot to enter, as these might slow down human traffic or disrupt some critical operations. But if the robot does somehow enter such an area, I would want it to exit as quickly and safely as possible.

I believe these two different requirements can exist for different regions on the same map, and we encounter such situations in practice. Therefore, it’s necessary to provide the user with the flexibility to apply both behaviors simultaneously.

For this reason, forcing a single behavior for the whole map using just one parameter does not seem like a flexible or user-friendly approach to me.

Logistical question: How would you know in the other layer which LETHAL zones are keepouts versus lethal from sensed obstacles, other costmap filters, or other layers to make sure you don't set real LETHAL obstacles as non-lethal for this mode?

I might be missing some technical details, but as far as I understand, each costmap layer handles its own data separately. So, for example, even if I lower the cost in the KeepoutLayer, if there is still a static or obstacle layer marking that cell as lethal, the master costmap will keep it as lethal. That’s why I assume real obstacles and keepout areas can be distinguished based on which layer set the cost. Is this correct, or is there something I’m missing?

[SteveMacenski]

@BCKSELFDRIVEWORLD I believe I understand what you're saying. This is a parameter that you can set off so that the robot will not attempt to leave. What I don't understand is the issue you're addressing.

If you have Layer1 that is keepout only with no lethal overrides and Layer2 that performs lethal overrides, you have 2 layers.

If you have Layer1 that is a keepout only with no lethal overrides that represents zones that should not be automatically moved and Layer2 that is keepout with lethal overrides with a map that contains zones that can override -- that is also 2 layers.

These seem, to my eye, completely equivalent in complexity of user configuration. There are 2 layers and 1 layer has to have a special map that contains the zones that can be overridden. In my proposal you have 2 layers and both layers just have their zone types (overridable or not) which is still 2 maps. So far largely the same.

What I think my proposal does have going for it is (1) reduced complexity for the user to not have to understand the subtlety of the two layers (and possible configuration mistakes by misordering them) and (2) we have direct knowledge in the layer itself about whether we apply the overriding behavior with the data itself. See below why that's important.

That’s why I assume real obstacles and keepout areas can be distinguished based on which layer set the cost. Is this correct, or is there something I’m missing?

That is incorrect. The master grid has no knowledge about the nature of who added it where or what other costs were. That would be very difficult to manage because multiple layers could set it as lethal or as any arbitrary cost, but we only maintain the highest and/or the last depending on the combination policy that each layer sets itself as a configuration.

Its also alot easier to add below in a few lines than it is to get the master grid, try to understand if it came from the keepout layer, modify every lethal value again to reduce. This is ~10-15 lines of code and we know the costs we're modifying are from the keepout layer without effort.

        if (override_lethal_cost_ && is_pose_lethal) {
          master_array[index] = lethal_override_cost_;  // logical default being 252 for very high, but technically OK
        }

---

So tl;dr:

• If we created a layer with a parameterized override for whether to allow zones to escape,
• you could just have 2 instances of the keepout zone: one with it true and one with it false where each have their own maps that represent zones that can or cannot be automatically recovered
• It sounds like you have 2 maps already for how the second layer would work (to know which zones can be escaped), so there's no additional overhead
• But this method simplifies the implementation vastly since we have the cost information within the keepout layer to know its from the keepout layer to modify.
• Thus, I think this is the better technical and end-user configuration solution :-) I think perhaps you didn't consider having multiple of these layers 😄

[SteveMacenski]

See my implementation in #5187 can you review @leander-dsouza @BCKSELFDRIVEWORLD