Feature Request: Custom/Virtual Obstacle Layer

[tonynajjar]

Hey, in our use case, we would like to use https://github.com/GMahmoud/virtual_costmap_layer with Nav2. Would you consider it a generic costmap layer that could be part of Nav2? Could it also be an extension of the Obstacle Layer?
We could work on contributing it.

@GMahmoud FYI

[SteveMacenski]

I would indeed, I haven't looked in excessive detail how this works exactly, but the concept seems good to me to have in Nav2 and would be actually very convenient for testing purposes (mainly). Curious though for a more production reason what you might be doing with this?

I'd want to know

• How the polygons/shapes are being filled in, and how the cost value is chosen / specified
• How the polygons / shapes are being specified (service, topic, etc) and how to add / remove them
• How it handles rolling costmaps or costmap updates efficiently

But the general principle seems good.

[GMahmoud]

Thanks @tonynajjar.

My virtual layer repo is a bit outdated demo package. Sorry about that 🙋🏻
Lately I was working on ros2 migration for all of my ros packages and the work 🚧 🏗️ 👷🏻 is still in progress.

I will let you know when I'll get some free time to upgrade the virtual layer by adding some other features that might help:

• Support of non convex obstacles.
• Add RPC instead of published msgs in order to have the possibility of deleting obstacles with id identification,
• Add additional parameters to obstacles (lifetime, cost, random position, ...).
• Better documentation and samples.
• ROS2 compatibility.

Regarding the production use @SteveMacenski , I think people are using this plugin differently. Personally, I am using it (not with demo project) to define no-access-zone or to help the robot to avoid some undetectable obstacles via a web application.

I have developed also some navigation plugins (global and local planning) that I want to make them compatible with ROS2 and Nav2 🤖 🚀. I will let you know in slack or in another Github issue/thread about this work.

[SteveMacenski]

Add RPC instead of published msgs in order to have the possibility of deleting obstacles with id identification,

I might recommend just adding a "add" and "remove" service and part of the request includes some string or int index for the objects. Not using pub/sub.

I have developed also some navigation plugins (global and local planning) that I want to make them compatible with ROS2 and Nav2 robot rocket. I will let you know in slack or in another Github issue/thread about this work.

Great to hear it and look forward to hearing about it!

[SteveMacenski]

@tonynajjar so what's your plan here? Remake something using that as a reference or wait for it to be ported and we can go through some design reviews to patch it up? Or should we start design reviews here now with the intention of including the final results into Nav2 itself?

Its also totally fine if you keep it separate, no need to put it into nav2 if you don't want. We can always add it to our Navigation Plugins list in our docs so that its searchable for users (and possible a tutorial if you're interested in that?)

[jplapp]

I work on the same project as @tonynajjar and can give some context on our use:

We use multiple robots in a shared environment. There are some cases in which the onboard-sensors of robot A cannot fully see robot B. Thus, robots share their location (and planned path) with each other. Now, to help with local and global planning, we want to put the footprint of robot B on the costmap of robot A.

For this, robot A would publish the received footprint of robot B on a topic, and this new plugin would then add it to the costmap. I think services would add too much latency here, in case robot B is moving.

[GMahmoud]

For this, robot A would publish the received footprint of robot B on a topic, and this new plugin would then add it to the costmap. I think services would add too much latency here, in case robot B is moving.

@jplapp I will make sure that there is no incompatibility with current version when integrating new updates to the demo package. 😀

[SteveMacenski]

I think services would add too much latency here, in case robot B is moving.

At the end of the day, services are just a paired request and response, over the same network interfaces and can be run with QoS settings like topics. I don't think that would be problematic at all and I think it is entirely necessary so you can (1) verify that an obstacle was successfully added to the scene and (2) you can verify deletion or moving of obstacles in the scene.

Especially in the case of multi-robot networking where things could drop out, it is very important you know what was successfully added and what wasn't. Maybe for the long-range planning application it isn't critical, but we need to act like any user of this work if in Nav2 has critical information that could prevent a dangerous situation. Otherwise, you could stream topics that are missed and never know that you're not going to avoid and obstacle / zone until its too late. Also if a service call fails, you know as a fact that Robot A didn't get Robot B's footprint, so you can try to retransmit. You'd never know that if you didn't have a service.

I think services are the right choice for the interface design.

[GMahmoud]

What do you think of having a stream from the client side for this case ?
We could make available a ros action for example in addition to request-reply for addition and remove of elements.
This action will deal with dynamic elements and client could supervisor the process by listening to the feedback topic.

[jplapp]

Thanks a lot for your insights @SteveMacenski . I have so far mostly taken inspiration from the costmap_converter often used with the TEB planner, which converts a costmap into polygons and publishes them as a topic - and thought of what we want to do here as the opposite direction.

Using services will allow the sender (robot B) to know that the communication is broken. However, it might not suffice to prevent the dangerous situation: If communication is broken, what can robot B do with this information? It can't tell robot A to stop.

So maybe there are two usecases here:

1. some mostly static object, like an obstacle zone, which is set once and then stays. Here, the node that configures the obstacle zone wants a confirmation that it was actually set.
2. moving objects, such as other robots, that constantly change. Here, we need to verify that the information about their footprint is always up-to-date, as it's received over network.

In case 2), the sender (robot B) constantly produces new information. So, over the (unstable) network I don't think there's a need for retransmitting, as new information will be available soon anyway.
Now, the "up-to-date verification" needs to be on the receiver side (robot A). I think it makes sense to do it in a separate node. The communication between this verification node and the virtual costmap layer then needs to be reliable.

For such constantly updating data as in case 2), other obstacle layers also receive their (critical) input via topics, so what would be different here? There are some usability advantages of topics, such as logging with rosbags, that would be quite helpful for us.

[SteveMacenski]

However, it might not suffice to prevent the dangerous situation: If communication is broken, what can robot B do with this information? It can't tell robot A to stop.

The client knows it failed to send it, so it can (1) retry so that you make genuine attempts to get the message over (2) log warnings to debug later problems in the fleet. There are many things that knowing that Robot A failed to send the message enables for reliable systems. Its not about what the other robot's server is doing with that information (because it didn't get the request, so it knows nothing) its about what the client can do to improve the chances of it working or introspecting the results. And with a service, you can know which IDs were successfully added so you can remove them later and not waste networking resources on trying to delete some polygon from a costmap layer that was never added in the first place due to previous networking problems.

logging with rosbags

Introspection of services is on it way soon actually https://discourse.ros.org/t/rep-2012-service-introspection/26079/4. But like I said above, you could at least log failures with services that you couldn't with topics. Also, if you don't finely handle your QoS on the polygons topic, you could end up backfilling and having a huge amount of memory overhead and/or instant network traffic flooding the pipes when a connection is re-established for old messages trying to pass through - since I imagine you would want to have some kind of transient-local properties if just streaming a topic on separate wireless devices of this nature.

For your examples

1. If its some static 1-time set zones, services make the most sense to make sure you know they were set or to retry since they're probably important semantic information about the environment and your robot's system behavior that would be unacceptable to just ignore (e.g. keepout zones)
2. For time variant I can see the appeal of topics, but then especially if we're talking about communication between robots, you'd want to know what information is actually in each robot's costmap versus guessing based on the current state of the network. I'd actually argue that streaming internally to a single robot would be more appropriate since we could make a pretty solid assumption that messages published are probably processed. When we go off device, that assumption is no longer solid and I would think it would be very important that some thing somewhere has the logs about what robots have what other robot's information actually processed and represented in their environmental models (in case of issue) which you wouldn't get if you just published the messages. In retrospect after a collision you could tell that it was that way from the logs, but you couldn't at the time know that since Robot A has no awareness Robot B didn't get the message.

But for either 1/2, deleting is also an important thing to get feedback on, not just additions. So services again seem to me to make sense as the interface to get acks back that 'Yes, Object 43432 is removed'.

I suppose we could support both services and topics, though not my ideal outcome. I would want clear warnings to users informing them that topics are not recommended, but may be useful in certain circumstances.

Separate of this topic, but I also wonder if it would be nice to have a fill option, I think in some cases it wouldn't really matter to fill in the polygon with cost, rather just make a solid costed outline so that the planners / controllers cannot pass.

[tonynajjar]

@tonynajjar so what's your plan here? Remake something using that as a reference or wait for it to be ported and we can go through some design reviews to patch it up?

I'd like to avoid doubling the work with @GMahmoud. To decide on a way forward @GMahmoud can you let us know:

1- When do you expect to have the ROS2 port ready?
2- Are you planning on making a PR to Nav2 yourself?

[GMahmoud]

Hello @tonynajjar

I will start doing some update this weekend.
For ROS2 migration, I can do something for mid August if it's good for you.
At first, I will do the migration in my repo and then we can discuss about creating the Nav2 PR.

Let me know 😃

[GMahmoud]

Hello !
Sorry it took me sometime to update the project but the work is still in progress link

• Support of non convex obstacles.
• Add RPC instead of published msgs in order to have the possibility of deleting obstacles with uuid identification
• Add additional parameters to obstacles (lifetime, cost, random position, ...).
• Better documentation and samples.
• ROS2 compatibility.

If you want to take a look at it. @tonynajjar I will let you know when I start to work on Nav2

Cheers

[SteveMacenski]

Add RPC instead of published msgs in order to have the possibility of deleting obstacles with uuid identification

Via services? Yes, that was my thought as well 😄

I looked very briefly at the code, there's alot going on there. If you were to define a Polygon as a series of connected line segments, I'm not entirely sure why the difference between convex/concave is important (or using so many external polygon libraries). Obviously I'm not in the middle of this development, so I'm sure you have good reasons! Though, I suspect some of this may be able to be simplified (hopefully?)

But I very look forward to this, this is a good compliment to the keepout filters. Sometimes a mask makes sense, but often a polygon or vector shape makes sense too! If we add the services to the Python3 Simple Commander, it'll be stupidly easy to make testing demos with randomly generated obstacles in a costmap, without having to manually create a map. Just open a blank costmap and generate random stuff in random spots to insert.

[GMahmoud]

If you were to define a Polygon as a series of connected line segments, I'm not entirely sure why the difference between convex/concave is important (or using so many external polygon libraries). Obviously I'm not in the middle of this development, so I'm sure you have good reasons!

I agree with you about defining only outerline of a polygon (which I call a ring here) or as you said a series of connected line segments. This is sufficient for mobile robot to take only the boundary of an obstacle and add an inflation layer to it.
About the polygon concept I am using the boost.geomerty one. Here is a link for better understanding.

So I want to make the virtual_layer more generic and support also polygon with holes.
As an example I am using this notion for UAVs to define a flying zone (outer ring / boundary) with obstacles (inner rings / holes). For some failsafe scenarios, landing on obstacle is permitted that's why I need to fill the inner ring costmap pixels in order to get the exact cost. (To fill the obstacle pixels we face the convex/concave issue)
Hope my explanation was clear

I will surely adapt my integration to nav2 use cases 😺, if you need to use only a simplified polygon.

If we add the services to the Python3 Simple Commander, it'll be stupidly easy to make testing demos with randomly generated obstacles in a costmap, without having to manually create a map. Just open a blank costmap and generate random stuff in random spots to insert.

Agreed 👍🏻 (Added to task list)

[AlexeyMerzlyakov]

Please check the following design considerations how this feature could be added into Nav2 stack.
Since, not all Costmap2D are working with nav2_msgs/msg/Costmap.msg, but also with nav_msgs/msg/OccupancyGrid.msg as well (e.g. Speed Limiting Filter), we need to support adding vector objects to both Costmap2D and OccupancyGrid maps.
Having the node, able to working with OccupancyGrids, it is worth to enhance its influence for all occupancy grid maps, rather that on costmaps and filter masks only. However, the update rate from local or global costmap node makes this idea to be almost meaningless.
Thus, it was considered to make a separate VectorObject server, able to work with Costmap2D layers, Costmap Filters, maps published by Map Server, SLAM, and many other scenarios, which seems to be more universal. VectorObjects Layer (or Virtual Obstacle Layer) in Costmap2D to be a wrapper accessing VectorObject server by using service calls API. This also is having the potential to be expanded to the Semantic Map server (the has been discussed many times) some day in the future.

My initial idea was to maximum re-use the existing messages (like PolygonStamped.msg or shape_msgs/msg/SolidPrimitive.msg). However, having such attributes as fill value, shape uuid, overlay type, makes me think that we need to have some universal Shape.msg (or Form.msg as it made in virtual_costmap_layer) with all these fields inside. However, the possibility of future expansion of types of shapes (e.g. ellipse, 3D-bar, etc...) makes it to be a super-message with all sub-messages inside and leading to unnecessary memory usage. Thus, it seems to more reasonable to follow in the footsteps of PCL and make some universal message structure, with:

• array of shape key-points (it could be vertexes for polygon, edges for broken line, or center of circle) and
• some data[], stored in the bytes array, and representing whenever-you-want depending on shape type. This might be the radius for the circle, or [size_x, size_y, size_z, angle_yaw, angle_pitch, angle_roll] array for 3D-object.

The design seems to cover all use-cases from the ticket discussions, and I do hope, it might be suitable for future utilization by other scenarios.

The proposed initial feature design in attached as VO_design_20230131.pdf. Any comments/feedback are welcoming.

[SteveMacenski]

I don't think any external server is required, but is certainly an option. We could have the layer simply take in a file itself which populates the information. A centralized server for the maps are helpful because there are many consumers of the map (costmaps, localization, collision checkers, visualization, etc). If we think there will be many consumers of the virtual obstacle information, then a centralized server is sensible. Else, loading from file is fine. The layer should be the one managing the set of polygons and operating upon them, the only role of a server should be to serve that information up to the layer from a file. Think of the obstacle layer: sensor data is sent to populate in the layer itself (I think the idea of having more processing in the external server was why you mentioned the OccupancyGrid / Costmap message stuff?).

If you have a vector of points making up a polygon stored in a layer, it can be used to fill in the costmap2D entries of the master grid on request (or retain an internal grid structure of the polygons, I suppose, depending on what's more optimal from an update cycle perspective). There's no real need to be thinking about ROS interfaces like OccupancyGrids or nav2_msgs/Costmaps. That's all handled for you in the backend for publication and visualization in rviz.

I don't see a strong need to support multiple subclass shapes (eg squares, bars) separately as types in the message from just having arbitrary polygons defined as a set of points. Any non-arcing shape (e.g. circles) can just be defined as a set of points. Additional information of would be fill, or similar is good. That would tell us to fill the shape with cost rather than just the outline. If not fill then its just the outline. Maybe also connect_ends or similarly named if the start and end nodes should be connected so that we close the shape (required for fill) to allow for lines. I don't understand priority is_bigendian or separate data in this context.

For circles, I think we'd need some special case for sure. Another question is about pose of the shape. If we want to set a box of points making up a 1m box, do I need to transform them to the pose already or can I have a pose of the center of the shape. I think that's more relevent for the add shape service than the actual shape message stored in file for a particular map that was semantically marked. Those marked in a map are probably already just at the coordinates.

The design I would have in mind is a server to read the polygons from file, if provided. That publishes to a topic that the costmap layer receives. The costmap layer either stores the vectorized information to populate in the costmap each cycle, or populates it immediately into a rastered map for use (whatever's more efficient). Services are added to get/set/remove the polygons from the costmap layer.

---

An aside, but the nav2_msgs/Costmap was made at a time long ago back when there were other design plans which were abandoned for the world models. Doing a brief survey, I cannot find a reason why we still even have that message around or use it for anything. We can probably throw that away entirely and replace it with the OccupancyGrid messages, which as far as I can tell provide the same information. Its actually quite annoying that we have that extra type around which serves little functional purpose.

[SteveMacenski]

The last branch @GMahmoud posted looked overall pretty good to me from a quick scan. I can't speak to the efficiency and certainly the parsing from file we discussed having done potentially by an outside server, but that's overall a good starting point. I wouldn't reinvent what's done well for us, if after some analysis you agree there are lessons to be taken from it.

[AlexeyMerzlyakov]

Regarding the server vs layer model: the main question was in that - do we plan to vectorize Costmap2D-s, or OccupancyGrid maps as well? My idea is to have vector objects applied not only to costmap layers, but to costmap filters as well. Costmap Filters are reading input OccupancyGrid filter mask from the topic, and some of them are not plotting it to the master costmap (for example, Speed Filter, which reads and stores OccupancyGrid map inside, but resulting costmap won't be merged with incoming filter mask). Therefore, adding vector objects only to costmaps may not enough. We need to work with both Costmaps and OccupancyGrid-s as well.

Going a little further, I might suggest that OccupancyGrid-s "vectorizing" might be useful in other areas, like intercepting maps going form Map Server and dynamically adding static of moving objects on it, e.g. for testing purposes. For this, having all the logic running on costmap layers won't look right: costmap layers are working with some restricted update-rate, which might be a design pitfall in the case. Therefore I thought to have a separate vector server that will do:

• Load vector objects from YAML-file (e.g. as ROS-parameters)
• Waiting for service requests and load vector objects from incoming services data
• Adding/updating vector object from topics, as the discussion result:

I suppose we could support both services and topics, though not my ideal outcome. I would want clear warnings to users informing them that topics are not recommended, but may be useful in certain circumstances.

• Reading non-vectorized OccupancyGrid map format from input topic and plotting added vectors on it. After it:
  • Publishing vectorized OccupancyGrid map to the output topic
  • Returning back vectorized Costmap2D to the VectorObject costmap layer, by service request

However, this only fair if we have right model of usage/usecases for that.
So, it depends on what do we want:

• If we want to have costmap layer, vectorizing only costmaps and (probably) filter masks, the design could be done on Costmap Layer level. I would like agree, that it is easier, than making separate vector server.
• If there will be some options to add vector object on maps, produced by SLAM for example, or something else, we might think about vector server today.

I actually, I don't mind any of both options, so the question is what model do we plan to have in Nav2: vector objects in costmaps, or vector object in costmaps + vectorized Nav2 maps everywhere?

---

Next item to discuss - is shapes design.
Do we plan to support only polygons (+maybe circle), or have an ability of expansion to other vector objects in the future (e.g. ellipse, arc, sector, ...)?

Any non-arcing shape (e.g. circles) can just be defined as a set of points.

In this case we to utilize only polygons, the design will be really simple, and we do not need to have that universal Shape.msg. Pretty good option, if nobody minds.

If we will decide have to support some other objects in the future, I see two major options there:

• O1. Support each new shape in the separate object and having each service for each shape:
  Add(Remove)Polygon.srv, Add(Remove)Circle.srv, AddEllipse.srv, AddArc.srv, etc...
  • Proc: Straightforward design and maximum re-usage of what have done
  • Cons: Boilerplate works and a bunch of services after many types of objects to be added
• O2. Try to add some universal object, that could absorb any of possible in foreseen future vector objects, like I've proposed in Shape.msg in previous comment.
  • Proc: One service calls, one topic, one objects per each Vector Object: AddShape.srv/RemoveShape.srv
  • Cons: A bit confusing design of data and points, or what we go in to be there; and therefore might be non-obvious to the end user.

I've selected the second one, to have an ability to expand the number of types of vector objects supported. If we do not need it (we'd like to leave only polygons and circles as it was made for Collision Monitor, or even only polygons w/o circles), the best option is O1.

---

An aside, but the nav2_msgs/Costmap was made at a time long ago back when there were other design plans which were abandoned for the world models. Doing a brief survey, I cannot find a reason why we still even have that message around or use it for anything. We can probably throw that away entirely and replace it with the OccupancyGrid messages, which as far as I can tell provide the same information. Its actually quite annoying that we have that extra type around which serves little functional purpose.

Hm... that is really interesting. I thought, it actively used in Costmap2D-s. But anyway, no one forbids us to revive this format, if we will really have a needs for it.

[SteveMacenski]

My idea is to have vector objects applied not only to costmap layers, but to costmap filters as well

I think that's out of scope of what we define as what a "costmap filter" is in the documentation. Its a rastered map of cost. The vectorized information is not a rastered map and there isn't the same kind of costmap filter info conversion to be done to convert costs into actions. But we could adjust that definition if required.

This can be thought of more as a normal costmap layer as a vector option instead of rastered maps, which that is a more sensible option. I had not at least initially considered the use of this feature for anything other than obstacles / costs in the costmap for navigation (e.g. Speed limit zones as polygons). How would that analog for the costmap filter info be send here for the multiplier, base, and action to use for the polygons? I don't think that was initially in the presentation.

like intercepting maps going form Map Server and dynamically adding static of moving objects on it, e.g. for testing purposes

But who would be the downstream consumer of that? I would think it would be the costmap, so having a layer that adds the geometries is no different of a workflow (its just who's hosting that particular service). Being able to just create a costmap and add some random shaped objects is inherently useful for benchmarking and analysis instead of having to create rastered maps of shapes to store offline.

The major reasons why I'm thinking this design:

• This has analogs elsewhere in the stack so it fits into the general workflow established (e.g. inflation, obstacle)
• It doesn't result in smaller, kb sized messages from file resulting in larger mb sized messages communicated at a more regular frequency for updates at runtime. Large messages are a bit of an issue in ROS 2 still in out-of-the-box DDS configs (and even after fixing, much higher CPU requirements) and likely in the foreseeable future, so as much as that can be minimized is good. Running a service call to update or remove a shape is a relatively small operation to other things happening in costmap layers like transforming dense pointclouds or performing hundreds of raycasting runs on laser scans.
• I'm not 100% sure how the server would know what resolution of occupancy grid to send over to the costmap layer for use. What would happen if the global/local costmaps used different resolutions?

Do we plan to support only polygons (+maybe circle), or have an ability of expansion to other vector objects in the future (e.g. ellipse, arc, sector, ...)?

I'd start with arbitrary polygons, lines, and circles. Honestly the most common use here is going to be boxes drawn on screens for keepout zones or speed zones or something by a non-trained user on a web GUI that's going to be propagated down here during robot setup.

Hm... that is really interesting. I thought, it actively used in Costmap2D-s

It is being used, but mostly with the details hidden in the costmap subscriber and costmap publisher objects. There's no need for it at all, the OccupancyGrid messages do the same. They added in a layer string name for distributed world model use but I quickly vetoed that for safety concerns but that work was already done.

---

So tl;dr

• You're probably right that we should have something costmap filter-y here for having the polygons do other tasks than simply adding costs. So maybe this should fall under the categorization of costmap filters.
• I think the costmap layers themselves should have the actual rastered maps only, the host server should only be reading from file and providing the vector shape messages to the costmap layer for use.

Edit: Ahhh, I think I understand a little bit better now your thinking, You want to use the default costmap filter plugin layers, don't you? You're seeing this as doing the pre-work for the layers so that the same speed and keepout zone costmap filters can be used, but the rastered costmaps are coming from this external server? That was unclear since the presentation shows a new VectorLayer. OK. Doing all this pre-work in a server makes alot of sense if we're not making any new layers. If we're making new layers, lets do it in the layer.

[tonynajjar]

Sorry for interjecting in the lovely design discussions, I would just like to mention that we found another application of such a costmap layer: We have a pallet-picking forklift-looking AMR; to be able to pick up a pallet aka driving into a pallet, we need to clear the region on the costmap (not centered at the robot) where the pallet is. I want to point out that even if we were to add the functionality to clear any custom region in nav2_costmap_2d, that would not be enough because the pallet is still there so the occupied costs will reappear. Therefore we actually need an extra layer to make the pallet disappear from the costmap.
We will workaround the issue for now and hope to re-implement the logic with this layer once it's ready, so we could be your testers 😄

P.S: Feel free to let me know if I missed another solution Nav2 offers for this problem

[SteveMacenski]

You have the option in a costmap layer to "override" cost instead of taking the maximum. So you can have the detector layer of your pallet that you'd be using to clear the costmap as the highest layer and it would wipe out all the sensor measurements as long as you're detecting the pallet there (or whatever your logic). That's probably the easiest way and only ~200 lines of code.

[SteveMacenski]

Per our discussion in the WG meeting:

• If we have the costmap filters that are the speed / keepout zones, and then we have speed / keepout zones as vectorized options, we probably need some way either in 'branding' and documentation/communication to bin these ideas together if we don't call the vectorized things also "costmap filters" so we can clearly communicate the capabilities to use either rastered maps or vectorized representations
• Ideally which would make our lives easier in technical communication and less redundant code would be to use the costmap filters themselves and have the polygon "stuff" happen in the separate server which publishes out a OccupancyGrid message that the costmap filter can ingest so we don't have to make any new costmap layers
• To make that plausible though, we should see the latency and CPU usage of regular sending of fullsized costmaps at 1-5hz from a server (in the component container) to a costmap layer to make sure that its "reasonable". This is the best option for cleanliness, but maybe not the best technical solution possible for resource utilization. Everything in life is a trade off though.

[AlexeyMerzlyakov]

@tonynajjar, thank you for the example provided above. No matter even if you will hide the data from sensor as Steve, suggested, I see here an additional use-case application of this feature: it is dynamically hiding some underlying objects in costmaps. This, BTW even more strongly speaks to the side of standalone Costmap2D layer running there for performance reasons.

---

So, regarding the design discussions, to continue the ideas from WG meeting, that @SteveMacenski pointed out, we have a tradeoff between two designs:
D1. Universal Vector Object server + StaticLayer (tuned for VO-Server output maps)

• Proc. Design versatility, allowing not only to add vector objects on costmaps, but everywhere, incl. Nav2 maps and Costmap Filters
• Cons1. Performance. The whole map is being stored, updated with VO and then trasferred to the costmaps. VO-Server has no knowledge about rolling Costmap window, so it could operate only with whole maps. This might be a real issue for huge outdoor/warehouses maps and high costmap update frequencies.
• Cons2. Concept issue: initially we are talking about static/dynamic VO affecting only costmaps. OccupancyGrid maps - are intentionally raster maps, used in Nav2 and in Costmap Filters as well. So, there is no clear reason (from the use-cases), why do we need to change these concepts?

D2. Vector Object layer on Costmap2D operating only with costmaps

• Proc1. Best performance for this kind of task
• Proc2. Feature intention clarity and simplicity
• Cons. Can not cover OccupancyGrid maps

Collecting all approach proc and cons, and if we are no requiring in OccupancyGrid maps "vectorizing" as of today intention, it is more reasonable to prefer "D2" over it. Otherwise, in the future, if we need to have to add VO to OccupancyGrid maps someday, the logic could be moved-out to operate separately as of VO-server. It seems not a big issue in that case.

Later, I will prepare design PPT update. with more details and attach there.

[AlexeyMerzlyakov]

Updated according to latest discussions, feature design is attached as VO_layer_design_20230203.pdf

[tonynajjar]

You have the option in a costmap layer to "override" cost instead of taking the maximum. So you can have the detector layer of your pallet that you'd be using to clear the costmap as the highest layer and it would wipe out all the sensor measurements as long as you're detecting the pallet there (or whatever your logic). That's probably the easiest way and only ~200 lines of code.

Thanks for the proposition. The detector layer is actually plain STVL, which also detects other obstacles which we do not want to clear. Additionally, we obviously can't detect the whole pallet with STVL so we do need to generate a custom polygon.
So if I understood your proposition correctly, that's not something we can use unfortunately.

[SteveMacenski]

StaticLayer

You mean the costmap filters (keepout + speed), right? To be able to do other non-just-cost tasks with the polygon's values

The whole map is being stored

It would likely be stored either way (e.g. in the costmap), so its more about the transport.

Cons. Can not cover OccupancyGrid maps

But isn't that what the costmap filters are for? I don't think that's a con, that's just a feature that already exists.

Cons2. Concept issue

I think this was miscategorized. That's a con for D2. D1 is far more simple conceptually and enables re-use of existing costmap filters just with the input being from the vector object server, not the map file.

[AlexeyMerzlyakov]

You mean the costmap filters (keepout + speed), right? To be able to do other non-just-cost tasks with the polygon's values

Not certainly in that way. In case of StaticLayer, we will re-use one of the existing costmap layer to make and apply (to master grid) raster costmap made from obtained input OccupancyGrid map, which is made by VO server from vector objects.
For costmap filters (keepout, speed, binary, etc...): they are already reading OccupancyGrid-s, so costmap filters could be connected to VO-server tuned to add vector objects to filter masks, directly; w/o any additional middle wrapper or layer.

It would likely be stored either way (e.g. in the costmap)

Yes, I was not entirely accurate. The main performance pitfall we may get when we have a large OccGrid map with large vector polygon (occupying almost all the space) on it, and small costmap rolling window. If we on each costmap update cycle are calculating only inside small window (int min_i, int min_j, int max_i, int max_j) , it will be much better (in terms of performance), than re-calculate whole OccupancyGrid map by VO-server. The explanation behind it is: separate VO-server have no knowledge about dynamic costmap window.

But isn't that what the costmap filters are for? I don't think that's a con, that's just a feature that already exists.

Yes, but as I understand costmaps are using OccupancyGrid-s to produce Costmap2D maps by it. Everything here is tied to costmap update rate.
By this cons, I mean the case, if we want to add vector objects on OccupancyGrid map generally. w/o referencing costmaps at all. An example might be produced by AMCL map with some missed by lidar objects (like glass wall); the map we want to save to a file (by map server) for further re-use. Costmaps are not designed for that.
But anyway, w/o thinking about examples; having something operating with OccGrids (and Costmaps through a StaticLayer) separately, is conceptually have a wider ability of applications, than costmap-based layer only. That was the meaning of the cons for D2.

The other question - do we really need these abilities?

I think this was miscategorized. That's a con for D2. D1 is far more simple conceptually and enables re-use of existing costmap filters just with the input being from the vector object server, not the map file.

D1 is more simple conceptually, but as I recall we've discussed, that this approach might change the concept/meaning/understanding of OccupancyGrid map itself, as a raster map.

[SteveMacenski]

Not certainly in that way. In case of StaticLayer, we will re-use one of the existing costmap layer to make and apply (to master grid) raster costmap made from obtained input OccupancyGrid map, which is made by VO server from vector objects.

Why? The keepout filter would be more flexible, and the static layer doesn't allow for shapes to represent other things like speed zones. Its in effect the same thing, but doesn't allow remapping of values potentially stored in the shape files. If we're going to allow the user to have shapes with other things like Speed or Triggering, why use the Static layer (not a costmap filter) instead of the Keepout filter -- when all others are costmap filters?

---

OK what I'm thinking, since I think we're generally in the first ballpark as each other but missing on the finer details:

• We have a server that reads some polygons from file and generates an OccupancyGrid from it
• Assuming the CPU or latency of regularly publishing that out isn't too high, then that occupancy grid is then subscribed to by the Costmap Filter plugins to interpret the polygons files' shape values to perform some action (e.g. speed, keepout, higher cost, lanes, whatever).
• That lets us not have to create any new costmap filters or costmap layers because there's no difference between an OccGrid like this and a map file.
• Each type of polygon (speed zone, keepout, etc) is in a separate file so they're subscribed to by different costmap filters on different topics

Everything in Nav2 is an Occupancy Grid, a costmap is an occupancy grid. There's not difference. The costmap message should be deleted and shouldn't be considered here in any differing nature.

[AlexeyMerzlyakov]

Why? The keepout filter would be more flexible, and the static layer doesn't allow for shapes to represent other things like speed zones.

Now I looks understand well your intention: to use Keepout Filter for resulting costmap updates.
Yes, this will work.
My intention was to use StaticLayer for this.
However, when we will use VirtualObject-server, both options (spreading VO through Keepout Filter or either through Static Layer) will work. So, it is not a problem in our case. Later, we could recommend developers to use Static Layer or Keepout Filter, depending on their tasks.

Each type of polygon (speed zone, keepout, etc) is in a separate file so they're subscribed to by different costmap filters on different topics

Not sure, why do we need to put each shape into separate file? If everything will be described as one ROS-param YAML, having all-shapes in one list (like we have for Collision Monitor), could it be an option?

Moving forward, we need to have separate input topic, each one per each type of shape: Polygon, BrokenLine and Circle. They e.g. could be automatically generated by user-defined prefix. Something, if we have name of VO-server to be equal to /vo_server, name of input topics could be:

"/vo_server/in_polygon" <- with Polygon.msg type
"/vo_server/in_broken_line" <- with BrokenLine.msg type
"/vo_server/in_circle" <- with Circle.msg type

Each topic will have uuid per each shape. If uuid will be the same, VO will update the shape. If uuid will be new, or empty, VO will create a new shape with data obtained from the corresponding message.

Similar situation will be for shapes adding/removing services. We need to have each type of service per each shape to add, but we can get by with one removing service for all shapes (removing by uuid), as I've mentioned in latest design PDF update from prev.message.

Assuming the CPU or latency of regularly publishing that out isn't too high, then that occupancy grid is then subscribed to by the Costmap Filter plugins to interpret the polygons files' shape values to perform some action (e.g. speed, keepout, higher cost, lanes, whatever).

I have some doubts about the case when we have large map with (large) restricting polygon + dynamically moving/updating another polygon and small rolling window for resulting costmap, like depicted below:

Isn't idea with centralized VO-server, publisging whole rasterized OccGrid map for the polygons (D1 architecture), will have a performance pitfalls comparing to separate VO-layer (w/o VO-server architecture D2) in the costmap?

[SteveMacenski]

Not sure, why do we need to put each shape into separate file?

How would you communicate to the client which shape should be used for which application to interpret the costs?

Moving forward, we need to have separate input topic, each one per each type of shape

Why? We should use a single interface for the shapes. Also I think you mean services so that you can get a request/reply to know the update / removal / addition was made. I'm not sure why messages for the polygons would need to exist if the server is reading from file and populating the occupancy grid.

I have some doubts about the case when we have large map

As well, its worth doing an experiment. I don't think it matters at all what the size of the polygons the changes have, because either way we need to recompute the occ grid and then publish out the entire costmap again (unless we use the updates API - which I'm not sure the costmap filter implement?)

[AlexeyMerzlyakov]

Performance estimation results of using centralized VO-server (duplicating from Slack, just for future use):

I've created some kind of simple VO-Server node in Nav2 preparing required size of mask and publishing it regularly. Then enabling KeepoutFilter in local costmap with published mask.
There were measured the following items:

• mask preparation time on VO-server
• OccupancyGrid mask publish time on VO-server side
• OccupancyGrid mask receive time on KeepoutFilter side (with Improve KeepoutFilter mask receiving performance #3420 enabled)
• KeepoutFilter process time

---

Both mask publishing and receiving times are directly proportional to mask size. However, map publish time is ~40-100x times higher than map receive time, so let's focus on it.
This linear dependency is being presented at the picture attached below:

Effectively, maximum mask size for having suitable dynamic update rate at ~30Hz - is 20-25 Mpixels (which is 5000x5000 pixels mask size, or 250x250 meters with 0.05 m resolution)

---

As was expected, KeepoutFilter process time was not dependent on mask size, as we are processing area inside rolling window.
When we have different mask and global frames, same mask and global frame, the average mask processing time differs on the ~ similar values:

KeepoutFilter::process() time, us
mask in "map" frame, global_frame = "odom": 213.76 us
mask and global_frame are both "odom": 176.97 us
difference: 36.79 us

which is insignificant in our case

---

So, the timings are more than suitable for dynamic objects support on centralized VO-server side.

[AlexeyMerzlyakov]

Next update of feature HLD is here: VO_design_20230222.pdf.
New Costmap2D layer is not required in this case: the VO-server to be used with KeepoutFilter in Costmap2D. It can not be used with StaticLayer for the following reasons:

• StaticLayer reads input OccupancyGrid map once, so we could not implement dynamic updates on it
• StaticLayer converts whole incoming OccupancyGrid map by-item to Costmap2D, which will cause significant performance slowdown (KeepoutFilter won't do this since Improve KeepoutFilter mask receiving performance #3420)
• StaticLayer works only with the same as main map OccupancyGrids, and thus, VO-server needs to read the input map from map-server to define the parameters (origin, resolution and size), which unnecessary complicating the architecture.

So, it could be used a KeepoutFilter. However, currently KeepoutFilter updates the maximum from mask_value and master_costmap. So, to take everything from VO-server, we need to add use_maximum option support to KeepoutFilter (which is trivial but anyway).

[SteveMacenski]

For the API:

• Priority: I think a unit8 would be fine, 32 bits seems excessive. Though, I'm trying to think logically about when this wouldn't always be a "use highest" or "use lowest". If this is a keepout layer, then we always want to use the highest value of a region. If its a speed zone, we'd always want to use the lowest value. But either way, that's actually more up the use of the values within the layer. I think priority could be removed with a bool for use_max or similar to indicate if we should use the maximum or minimum value for adding the shape to the map.
• Transparency: How would this enact transparency? How would the vector server know how to interpret the values for the application to know what value means "do nothing" if we have the base+multiplier going on in the costmap filter layer? I don't think this is something of practical need and could probably be removed. If transparency (e.g. invert behavior) is necessary, they could just add a shape with value that corresponds to that?
• Fill: I think needs to be in circle and polygon to indicate if we want to fill the shape, or just outline the shape. Closed is whether to connect the start/end points in the polygon, but doesn't say if we want to fill the interior or not.

[AlexeyMerzlyakov]

I think a unit8 would be fine, 32 bits seems excessive.

Agree, 8-bit value should be OK, since we won't use each own unique priority per each vector object

priority could be removed with a bool for use_max or similar to indicate if we should use the maximum or minimum value for adding the shape to the map.

The priority is made for the case when we intentionally want to make the order of intersections of the vector objects. This is the case if we want to some objects with lower value to intentionally to lay over the ones with higher values while using use_max = True indicator. Or this is required for the example where we will use the transparency, described a little bit below.

Indeed, use_max indicator is also needed, to define how two or more objects would intersect in case of both of them are having equal priority. use_max in this case could be a global ROS-parameter, since one VO-server instance is implied to be used per one selected filter.

How would the vector server know how to interpret the values for the application to know what value means "do nothing" if we have the base+multiplier going on in the costmap filter layer?

Initially, I thought about making some kind of "holes" inside vector objects, and filling them with FREE_SPACE values, allowing to underlying objects with lower priority to be on the resulting map.
The example depicted below could not be drawn only by utilizing only use_max and priority values:

Polygons "A" and "B" having the same priority = 1, while circles "C" and "D" are having lower priority = 2. Circle "C" in this case should be laid always under the border "A&B", which seems to be impossible w/o additional attribute like transparency allowing to show vector objects laid under the current one.

This could be resolved by adding some new essences like "rings", initially proposed in the "virtual_costmap_layer". But I'd here rather more inclined to an extra transparency indicator than to make new essences instead.

Fill: I think needs to be in circle and polygon to indicate if we want to fill the shape, or just outline the shape.

Thanks for pointing this out! fill would be also a useful attribute for the vector object.

Closed is whether to connect the start/end points in the polygon, but doesn't say if we want to fill the interior or not.

Yep. Moreover, if closed is set to true, we will check the fill value set or not. If closed := false, talking about filling makes no sense in this case.

Also, closed is valid only for Polygon types of shapes. For circles we do no need this parameter, unless we won't introduce the arcs with begin_angle and end_angle. However, I think it is over-complicating the design in our case: we could use circle + polygons to do that.

[AlexeyMerzlyakov]

One more thing about the design.
Since one VO-server node is used to be launched per each filter (e.g. if we are using KeepoutFilter and SpeedFilter, we need to have 2 instances of VO-server running), all vector shapes could be described in one launch-file, as ROS-parameters' array:

vo_server:
  ros__parameters:
    frame_id: "map"
    map_topic_name: "vectorized_map"
    use_max: True
    circles: ["CircleA"]
    polygons: ["PolyA", "PolyB"]
    CircleA:
      center: [0.1, 0.1]
      radius: 0.2
      priority: 1
      fill: true
      value: 0 # FREE_SPACE
      UUID:  78985aa8-a353-4164-9fb1-0219795ec176
    ...
    PolyA:
      points: [1.0, 1.0, 1.0, -1.0, -1.0, -1.0, -1.0, 1.0]
      priority: 2
      closed: true
      fill: true
      value: 127 # somewhere in the middle
      UUID: c785690c-286b-4030-b9c4-cc4474b840f0
    PolyB:
      points: [0.5, 0.5, 0.5, 1.5, 1.5, 1.5, 1.5, 0.5]
      priority: 2
      closed: true
      fill: false
      value: 254 # LETHAL_OBSTACLE
      UUID: 3ad2b866-bd7f-4e21-af35-bb79d1e9c507

I remember there were some points why each shape is better to describe in its own file, were on the call. But I can not recall exactly is there any specific point why we should do it, if we are using one VO-server per specific costmap layer/filter?

[SteveMacenski]

I don't think any priority is needed. If you have intersecting shapes, use_max or so would tell you the policy to deconflict (use the max or min values). I don't see how any other policy would be sensible logically. If you have 3 shapes overlapping of values 1, 2, and 3: what's a situation where you'd want 2 to be the "right" answer? It wouldn't be for speed limits (you'd want minimum) or keepout (you'd want maximum) or anything else we've previously discussed having as a costmap filter / vector layer. I don't think there are generally priorities in shapes. If I as a user draw 20 shapes on a map to correspond to different zones, none have more priority or interest over the others. They're all just keepout zones or speed restricted zones. If however there are overlaps the values themselves correspond to what the deconflicting policy should be: for keepout, use maximum value; for speed, use minimum value.

A very common workflow when deploying robots is to map the space and then have a facility manager (or someone) sit with an ipad and just drag boxes onto the map to mark docks, safety zones, aisles, etc for semantic information to use in navigation later. In fact, I'd really like as part of this after we get done with the MVP of the server is to develop the tools in rviz to essentially allow for that workflow and save to a file for use.

But I'd here rather more inclined to an extra transparency indicator than to make new essences instead.

But what are the specific mechanics at which you'd make transparency? I don't think its possible with the information you have available. You don't know what value of cost 0-255 would remap to "do nothing". 0 could be 0% speed or 0 could be trigger the binary filter -- we simply cannot know.

I remember there were some points why each shape is better to describe in its own file, were on the call.

I figured their own files would be most sensible because of the fact that if you're deploying N robots across M facilities in the world, each of the M facilities is going to have its own set of keepout zones / vector files. But perhaps that can be a step after we have a prototype to look over the param file and see if we can easily separate out nodes that are (1) robot specific vs (2) facility specific to see if we can pre-break-up the files corresponding to those criteria for deployed users. Off the top of my head, only the map server would be the "other" big thing.

[tonynajjar]

just following up, is this actively being worked on or on pause at the moment?

[AlexeyMerzlyakov]

Working on the feature is in active progress. There is ~90% of main algorithms code done. After combing-up stage, I plan to open draft PR on it.

[PranavShevkar]

Hello @AlexeyMerzlyakov and the whole maintainers team.
i just want to say that I had been tracking this feature request since two years because this is something I need in my uni project. Today I tested it and it is working perfectly and it is very well documented too.
Thank you