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

• [X] Support of non convex obstacles.
• [X] 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, ...).
• [X] 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 :smile:

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)