Feature Request: Virtual Costmap Layer for Dynamic Zones

[SherifFathey]

Feature request

Feature description

I would like to propose a virtual costmap layer plugin that enables adding and managing dynamic zones and obstacles at runtime.

The plugin allows defining shapes such as polygons, circles, and lines directly in the costmap, with configurable cost values for each shape.

This makes it easier to represent temporary obstacles, safety zones, or restricted areas without requiring map changes or restarting Nav2.

Repository:
https://github.com/sherif1152/nav2-virtual-layer

Implementation considerations

The feature can be implemented as a standard Nav2 costmap layer plugin.

• Supports geometric shapes: circles, lines, and polygons (filled or open)
• Assigns configurable cost values per shape
• Provides runtime management via services (add/remove/clear)
• Supports time-based expiration of shapes
• Uses unique shape IDs for tracking and updates
• Applies updates directly to the layered costmap in real-time
• Handles coordinate transforms using TF
• Works with both global and local costmaps

The plugin follows Nav2’s existing plugin architecture and integrates without requiring core changes.

[tonynajjar]

Are you aware of https://docs.nav2.org/configuration/packages/map_server/configuring-vector-object-server.html? It probably doesn't support all the fetaures you mentioned but it's a start

[SherifFathey]

Thanks for pointing this out!

Yes, I’m aware of the Vector Object Server. My understanding is that it generates an OccupancyGrid from vector data.

The approach in this plugin is a bit different, as it works directly as a costmap layer and focuses on runtime interaction (e.g., adding/removing/updating shapes dynamically with immediate effect on the costmap).

I think both approaches can complement each other depending on the use case.

I’d really appreciate your thoughts on whether something like this would still be useful within the Nav2 ecosystem or as an external plugin.

[SteveMacenski]

Check out the vector object server, you can do all of those same elements (I believe) using it with a layer in the costmap that accepts the occupancy grid data. What gaps does this provide what we currently have does not? We can already add/remove/clear/etc shapes that are polygons, filled, etc. It seems to me from glancing over your code that perhaps you've read the vector object server in its implementation in the creation of your layer 😉

[SherifFathey]

Thanks @SteveMacenski for the detailed feedback!

You're right that the main goal aligns with the vector object server approach.

The focus here was more on adding flexibility at runtime and usability features, such as:

• Each shape can have its own cost level instead of using a single uniform cost
• Ability to restore all shapes after a full clear or just one using service-based management patterns without restarting Nav2
• Shapes can automatically disappear after a set time without manual removal
• Easy RViz visualization using unique IDs for tracking and debugging
• Supports reloading shapes from YAML without restarting Nav2

So the idea wasn’t to reinvent the core concept, but to make dynamic usage a bit more flexible and operator friendly in real-time scenarios.

Haha fair enough 😄
I’ve just spent quite a bit of time going through Nav2 internals, so any similarities are probably just natural overlap in thinking.

[SteveMacenski]

The focus here was more on adding flexibility at runtime and usability features

Always appreciate more features! Would it make sense perhaps though to have those as members of the vector object server to build on what already exists in instead of replacing it with a largely duplicate set of features? I think that would make it quite confusing for users as to which to use and why.

The benefit of the VO server is that its independent of the costmap layer so it can be used to represent information outside of the context of simply a costmap integration to add costs. So this feels more abstract and generalized to me to keep the VO server and just add the missing features to it.

Each shape can have its own cost level instead of using a single uniform cost
Easy RViz visualization using unique IDs for tracking and debugging

That is already the case :-) Maybe we just need to expose the unique IDs but there are unique IDs (and that code seems 1:1 from the VO server).

Shapes can automatically disappear after a set time without manual removal

That does sound nice! We should be able to add that to the VO server though using timers?

Supports reloading shapes from YAML without restarting Nav2

Can that not be done right now with lifecycle transitions and/or adding a service to do so? Seems like that could be a feature addition to the VO server as well to meet that need

[tonynajjar]

While I agree that the independent VO server has its benefits - a native costmap layer also has its own, mostly in performance:

• Zero-copy rasterisation — Writes directly into the costmap's internal buffer. No OccupancyGrid serialization and deserialization.
• No inter-process overhead — Data stays in-process. No topic bandwidth consumed by full grid messages when only a few polygons exist.
• Smarter update region management — Because the layer works with polygon data directly rather than a rasterized grid, it can track previous bounding boxes for stale-cell clearing and detect unchanged data trivially (a simple flag) to report no bounds at all — so the master never resets that region and cells persist for free, avoiding unnecessary InflationLayer re-processing. A consuming layer for an OccupancyGrid would need an expensive full-grid diff to achieve the same.
• Subclass extensibility — A base class can centralise all costmap mechanics (bounds, rolling support, TF, stale-cell clearing). Subclasses become trivially small — they just acquire data and hand off polygons.
• No service-call latency — Subclasses can subscribe directly to data sources and update polygons in-callback. No per-cycle service round-trip.
• current_ flag — Can signal to planners that the costmap is stale when polygon data changes, something an OccupancyGrid intermediary can't express.

Given that we work with very big maps and thus need to prioritize performance, we do internally have implemented a native "Polygon costmap layer". We'd be happy to contribute it back.

FYI @doisyg

[SherifFathey]

Thanks @SteveMacenski , this is really helpful , I appreciate your experience and insights here.

Always appreciate more features! Would it make sense perhaps though to have those as members of the vector object server to build on what already exists in instead of replacing it with a largely duplicate set of features? I think that would make it quite confusing for users as to which to use and why.

I agree with your point about avoiding duplication and building on top of the Vector Object Server ,that’s definitely a cleaner direction overall. At the same time, I was thinking that in some cases having alternative plugins with slightly different interaction models might give users flexibility depending on their use case.

That is already the case :-) Maybe we just need to expose the unique IDs but there are unique IDs (and that code seems 1:1 from the VO server).

Regarding the unique IDs — yes, I’m aware they already exist. My thought was more about usability: working with UUIDs can sometimes be error-prone when manually interacting with objects, so having a simpler numeric ID option (e.g. 1, 20, 50) could make operations like removing or updating shapes easier in practice.

For time-based expiration, I agree it could fit within the VO server , it’s just something I found useful in dynamic scenarios.

These are just some practical needs I encountered while working with dynamic environments ,really appreciate the detailed feedback.

[SteveMacenski]

@tonynajjar I think you have that because it pre-dates the VO server 😉 The inter-process overhead is easily resolved by composition which is the default bringup anyway :-)

@sherif1152 what do you think is the path here you'd like to explore with all this considered? I would like to make sure we're concise and not having largely duplicate features which confuse users about which to use when/why. If we can add the missing features into the VO server, then I think that we should go that direction. With that said, I appreciate the simplicity of a costmap layer that just does it but I don't know how to rectify those two things without perhaps creating some abstract objects which both use to share key parts of the implementations to remove the redundancy. I just don't know how realistic that is off hand.

[tonynajjar]

@tonynajjar I think you have that because it pre-dates the VO server 😉 The inter-process overhead is easily resolved by composition which is the default bringup anyway :-)

Nope, we made the conscious decision a couple of months ago to implement it as a native costmap layer knowing that the VO server exists for the reasons cited above + the fact that we use OpenCV rasterisation which is much much faster.

You're probably right that the inter-process overhead is resolved by composition but 1- we are overdue but don't use composition yet and 2- Last time I checked, it's not so simple to achieve zero-copy, there are more prerequisites than just "using composition"

[SteveMacenski]

I don't want to distract from the initial point of this ticket - so I'll chat with you on those in our other Slack threads. But suffice it to say in summary - actually I've done this hard work for you, it is ready/done/easy but there are caveats

the fact that we use OpenCV rasterisation which is much much faster

Sounds like a good addition :-) If we can pull in some of those so we don't have to have the opencv dependency, that would be ideal, but I think for something like this that seems reasonable.

[SherifFathey]

@SteveMacenski I agree with you on avoiding duplicated features and keeping things clear for users. If we can clearly define the distinction between the two approaches, users should be able to easily understand which one fits their use case without introducing confusion. This would allow us to keep both systems without forcing a merge, while still maintaining a clean and intuitive separation of responsibilities.

That said, I think both the VO Server and a virtual costmap layer address similar objectives, but with different architectural designs.

The VO Server uses a decoupled pipeline where vector data is rasterized into an intermediate OccupancyGrid, which can be reused by multiple systems beyond Nav2. This introduces an external processing and messaging layer, but provides better generality and reuse.

In contrast, a virtual costmap layer is an in-process Nav2 plugin that applies vector data directly to the costmap without an intermediate representation. This reduces overhead and improves real-time performance, but results in tighter coupling to the costmap system.

Architecturally, this represents a trade-off between a reusable representation-based pipeline (VO Server) and a tightly integrated execution-focused layer (Virtual Layer).

As highlighted by @tonynajjar,

While I agree that the independent VO server has its benefits - a native costmap layer also has its own, mostly in performance:

the virtual costmap layer has notable advantages in performance, update efficiency, and in-process execution, which become increasingly important for large maps and real-time scenarios.

I’d be interested to hear your thoughts on whether this separation of responsibilities is the right direction for the Nav2 ecosystem.