[Collision monitor] feature discussion: Stop zone with allowed rotation or inverted direction

[doisyg]

First of all, thanks a lot to @AlexeyMerzlyakov and @SteveMacenski for the collision monitor. I am finding myself in a new company and having yet again to implement something similar to the collision monitor. Just started experimenting with it. It is great to be able to find this brick part of nav2.

A bit of context about my use case: a robot with a full certified low-level functional safety where we are using nav2 on top and with the collision monitor configured with slightly more conservative zones in order to never trigger the low level safety.

Feature description

A stop zone with optional allowed degree of freedoms in order to avoid having a robot stuck.
For instance:

For a circular robot

When obstacle in the zone, forbid linear speeds, but allow rotations / angular z speeds

For a rectangular bi-directional robot

When obstacle in the zone, forbid angular speeds, forbid linear speed in one direction but allow in the inverted direction (i.e. when an obstacle is in front of the robot, allow to go backward)

Implementation considerations

TBD if this feature is seen as useful. I will happily propose an implementation and a PR.

[SteveMacenski]

Any other bricks we should add? 😉 I only know what to add based on my personal experiences and what people tell me they want!

Consideration: there is the collision monitor approach mode which uses the velocity of the robot in accounting for collision within immediate future. If you use that, you can back out or move laterally, no problem, since it won't be in the directions of collision. But I assume you know that and want to still use the polygon zones?

One thing I'm concerned by here is generality. Right now, there are just zones without any specificity about their positioning relative to the robot base (they could be in front, behind, off to the right by 2000 meters, enveloping the robot etc) and knowing which of these its sensible to apply the conditioning to may be non-trivial. Or the policies that would be relevant for each (e.g. omni, diff, ackermann). Though, I think the policies are an easier thing to embed by what you describe as "degrees of freedom" as configurations.

Additionally, how do you deal with conflicts if there are multiple zones? Lets say you have a forward and a reverse zone setup: frontal says stop, backwards is OK. So you start going backwards, then the backwards one triggers because you're running near a wall. Or if you have a polygon enveloping the entire robot so forward has points for collision so you start reversing and now there are points in the reverse direction. Since its part of the same polygon, that could cause a collision.

The secondary problem set is providing the tooling (if not simply the recovery behaviors?) to perform the actions described as admissible by the policy. What were you thinking there? Just the default recovery behaviors or something else?

This seems very sensible at face value, but which (all?) polygons to apply the policy to allow certain actions to seems challenging.

[SteveMacenski]

I'd like to have something like this around + that it works well out of the box with recovery behaviors, but the selection of zones to apply it to + deconflicting problem needs to be sorted

[AlexeyMerzlyakov]

Hi! Thank you for the interest taken in the Collision Monitor!

From the use-cases you've initially proposed, I might guess that the motivation of them - is to have an automatic recoveries. Since it its current state, Collision Monitor is laying "under" the Nav2 stack, indeed any Nav2's changes could not affect CM behavior. Only the thing, laying over Nav2 and CM (like teleop/assistive teleop/etc...) could move robot out of danger area. This was initially designed for the safety: to allow operator only manually move out the robot. But such questions, as "how to pull out robot automatically" still have a place.

From the first glance, I see 3 global strategies to approach this situation:

1. Global CM design change: add new degree of freedoms or behavior models to the CM, like something along with existing STOP/SLOWDOWN and APPROACH
2. Pinpoint workarounds for the cases @doisyg described:

• allow_rotation parameter for "CIRCLE" shapes
• allow_reverse parameter for STOP model

3. Add the recovery option to the CM. This option might pass as a message or by service request from BT, when the robot is going to the recovery state. If this option enabled, CM will "forget" all its zones, and will allow recovery behaviors do their job. As soon as the robot comes out from recovery state, CM will take the control again.

First and second options have the disadvantages described by @SteveMacenski, in the above message:

• Zones could be not around the robot, but anywhere you want. Moreover (despite the official documentation), CM is designed to publish these zones in any frame you want, not only relative to the robot. So, it is unclear how to move robot back if it is already in the STOP zone, and what "move back" will mean in this case.
• We support all kinds of robots, so CM is universal for all types of drives, and it seems not a suitable idea to break this logic.
• How to handle multiple zones? Currently CM is pretty straightforward, and adding any changes like described in option 1 or 2 will complicate its logic and will make it hard to understand what he's doing in the end for developers and us as well.

The third option seems to be more universal approach for automatic recoveries problem: we could select any allowed behavior in the behavior tree at the stage of designing software for the robot and this will be utilized (if we want), when robot will went to the stuck situation.
If the motivation is only to have automatic behaviors, and no more, so how about this idea?

[doisyg]

Thanks you both for your inputs. That fueled a nice brainstorming with @kaichie and we debated many different options.

Consideration: there is the collision monitor approach mode which uses the velocity of the robot in accounting for collision within immediate future. If you use that, you can back out or move laterally, no problem, since it won't be in the directions of collision. But I assume you know that and want to still use the polygon zones?

Yes, polygon zones are ideal to replicate what a safety lidar is doing (not replacing). Plus, I think the footprint used for the approach mode should be inflated to the actual polygon of the stop zone + an epsilon in order to be sure that we don't get into it at very low speed. Which feels a bit hacky. @kaichie can give more details.

On the overall issue and how to solve it, we gave it a bit more thinking and reconsidering:

• The fact that the CM is not relying on anything higher level is elegant and we should keep it that way to maintain its simplicity and readability.
• We can probably achieve what we want without touching the code of the CM itself but using the dynamic polygon support:
  We already have an external node (let's call it polygon_generator) responsible for switching between multiple stop and slow zones configurations based on the measured speed of the robot (catched from /odom). This is actually mimicking what a safety controller/PLC will do to a safety lidar based on motor speed monitoring. I feel that we could avoid getting stuck if we add to our polygon_generator the possibility to have rules for swithing the zones based on the commanded twist. That will leave the CM clean of extra logic. However, if we base our decision of what zone to use on the input twist, we need to be sure that the polygon linked to that decision is applied in the CM before this input twist is proceeded by the CM. The only way that I see to ensure that for now without touching the CM, is to make our polygon_generator subscribe to the input twist and republish it to the CM input only after having switched the polygon.

To sum up: we are going to continue experimenting in the next days, and with the goal now of not touching the CM. We will report here and if there is an interest for our polygon_generator (don't have a better name yet), we will happily contribute it to the nav2_collision_monitor package.

[SteveMacenski]

Plus, I think the footprint used for the approach mode should be inflated to the actual polygon of the stop zone + an epsilon in order to be sure that we don't get into it at very low speed.

I'm not sure I quite understand your meaning there or its specific intent. Also, that should be possible now (?)

We already have an external node ... This is actually mimicking what a safety controller/PLC will do to a safety lidar based on motor speed monitoring. ... we will happily contribute it

No doubt - if you need dynamic updates, you need something to be dynamically updat...ing 😉 Is this sufficiently general (or could be made so) to potentially include release open-source? This sounds of utility.

I feel that we could avoid getting stuck if we add to our polygon_generator the possibility to have rules for swithing the zones based on the commanded twist.

Please explain the logic you have in mind for this, I don't have a good mental model to try to help come up with ideas. It doesn't sound though like the odom+Twist subscription for the node to not modify CM is a great choice (but we may also be in a problem of picking the least bad solution from the pile).

First question set: If your robot is stopped due to a collision zone, how is the selection of Twist made so that it will exit the collision situation? Are you just thrashing based on your local trajectory planner and hoping some twist will come in that will resolve the situation and that when that happens, changing the zones to allow it? Is it based on recovery behaviors? Is it based on custom recovery behaviors matching your zone requirements or the standard set of recoveries? These are important questions - because if its recovery related, that opens a huge box of far cleaner solutions. Especially if they are more standard.

Second set: Assuming we have some set of Twists and a collision-zone-publishing-node-thingy, how would the rules be setup? What are some examples of these rules?

Perhaps also rather than trying to cram absolutely everything into this base implementation, we could have something that derives from this and expands on it with additional capabilities that may be a little more specialized to keep the main "stuff" decoupled. But the more I think about this here, I don't think that may be required. I can see a path through this with some pretty simple changes.

In the ideal world, this is how I'd handle it:

• The Twists are coming from recovery behaviors executed either in the behavior tree as navigation failures (e.g. in collision zone, so robot goes no where, so that causes behaviors to trigger in the BT) OR as event triggers adopted within the Collision Monitor itself (e.g. I enter a collision zone, pause for a moment, then trigger a recovery action specific to that specific zone)
• Most ideally, the second option from above -- but the first would work as well. Option 2 would mean each zone should have a (set?) of recovery actions when the collision zone is triggered. That's a general solution to the "but my zones can be anywhere/anything!" problem. Option 1 would be basically to have each zone be parameterized whether it can be "disabled" when a certain kind of Twist comes in from the behavior tree corresponding to a recovery (e.g. forward polygon disabled if fully reversing). Either way, that deals with deconflicting when the selection of permissible actions align with the polygon's nature. Overlapping polygons would both need to have similar conditions.

The question basically is how to avoid the oscillatory condition whereas your robot drives into a zone, then it gets itself out of the jam, and basically just tries to do that exact thing again because the conditions that led to that decision have not changed. But that's a problem no matter what.

[SteveMacenski]

@doisyg thoughts?

[doisyg]

I will let @kaichie answer as he is the one who took the subject for us (he is in vacation for 2 more weeks though)

[kaichie]

Hi all! Sorry for the late response!

No doubt - if you need dynamic updates, you need something to be dynamically updat...ing wink Is this sufficiently general (or could be made so) to potentially include release open-source? This sounds of utility.

Would definitely happy to contribute, open for any discussion to make it more general to be a utility. For diff-drive bi-directional model, it looks something like this:

Please explain the logic you have in mind for this, I don't have a good mental model to try to help come up with ideas. It doesn't sound though like the odom+Twist subscription for the node to not modify CM is a great choice (but we may also be in a problem of picking the least bad solution from the pile).

With the collision_monitor package, we can nicely use the Approach model to keep the robot always M seconds from any collision and Stop model to stop the robot X distance away from the obstacles. As the collision_monitor is at the last layer of the nav stack, it can be conveniently used to filter any teleop operation before passing the cmd_vel to robot base too. However the robot might end up in the Stop polygon and stucked, so the idea here is similar as shown in the gif above to dynamically update the polygon based on the combination of twist. (For example a smaller polygon on the opposite of twist direction.)

First question set: If your robot is stopped due to a collision zone, how is the selection of Twist made so that it will exit the collision situation? Are you just thrashing based on your local trajectory planner and hoping some twist will come in that will resolve the situation and that when that happens, changing the zones to allow it? Is it based on recovery behaviors? Is it based on custom recovery behaviors matching your zone requirements or the standard set of recoveries? These are important questions - because if its recovery related, that opens a huge box of far cleaner solutions. Especially if they are more standard.

Ideally if changing the polygons based on Twist could resolve the issue then it would be the best, and this sounds like something we can do in the collision-zone-publishing-node-thingy/collision_monitor itself without touching the recovery behaviors.

Second set: Assuming we have some set of Twists and a collision-zone-publishing-node-thingy, how would the rules be setup? What are some examples of these rules?

The node probably should publish different sets of polygons based on the twist command, for example within certain range of translation speed and rotation speed.

The question basically is how to avoid the oscillatory condition whereas your robot drives into a zone, then it gets itself out of the jam, and basically just tries to do that exact thing again because the conditions that led to that decision have not changed. But that's a problem no matter what.

Totally agree and I think it is important to set the global/local planner correctly to avoid it driving to the stop zone at first place. Like inflate the footprint to match the size of Stop polygon.

[AlexeyMerzlyakov]

Let's return back to the discussion after some NY break.
To summarize-up, we have the following use-cases to extending on:

• Allow rotations (for circular robots)
• Allow reversing (for rectangular ackerman/omni robot)
  Plus it should:
• Allow external teleop operating by the scenarios from above, when robot is stucked in the stop/approach zone

The idea about dynamic polygon_generator node that will be the part of the nav2_collision_monitor seems workable for me. This will allow to cover all the current use-cases from above and won't do CM to be hairy with an infinite number of exceptions.

However, can't we do something even simpler to cover these use-cases?
It seems that both options that @SteveMacenski pointed out are workable for this:

• O1. Run recoveries triggered by Nav2 stack + CM disabling service.
  • This option will allow to make robot recovery rotation / reverse movement both using BT recovery algorithms and manually, e.g. through the teleop.
  • Since both use-cases are depending on robot type (circle omni, or rectangle Ackermann), the parametrizing by recovery behavior might belonging to the robot, which will allow to rotate circular robot in all stuck cases, as well as moving back rectangle robot for all polygons. Will it be enough? Or otherwise, each polygon might be parametrized by specific power-off / BT recovery trigger.
• O2. Add recovery parameter to the CM polygon attribute, and trigger BT recovery behavior back from CM node, when it is stucked.
  • This option is also could make the parametrization by polygon and its recovery behavior.
  • However, this solution makes CM to be independent node laying "under" Nav2 stack, and making positive feedback that might make oscillations that Steve mentioned about (in case of Option1. BT will decide where to switch back CM, that might be a buffer protecting from oscillations). So, I would like to prefer the Option1., if it suitable for all reported and possible use-cases.

And other thoughts/ideas? I am definitely open to accept further contributions into Nav2 CM that will help to extend its current scope of use.

[SteveMacenski]

Would definitely happy to contribute, open for any discussion to make it more general to be a utility. For diff-drive bi-directional model, it looks something like this:

Is this basically polygon-scheduling based on the robot velocity? Or is the actual velocity being taken into account beyond just if velocity in range, use this polygon (e.g. there's some base polygon that's adjusted/warped proportional to speed continously)

Ideally if changing the polygons based on Twist could resolve the issue then it would be the best

Ah, so the robot-is-stopped polygon being scheduled would contain essentially nothing except the robot so that it couldn't get "stuck" so to speak? I like that idea.

How would you like to proceed @kaichie? It sounds like you have something largely working already - do you want to contribute this separate server or build it into the collision_monitor directly? If it uses the odometric velocity, then a separate server is doable. If it uses the input velocity from Navigation, it should probably live in the collision monitor directly to shim that velocity. We could essentially have this be an object member of the main server polygon_scheduler that is given an input navigation requested velocity and modifies the polygon objects as needed. Or could continue to be a separate server that we call via a service call (although I don't love adding service/actions calls in a psuedo-safety system)

---

This is far more straight forward than as initially proposed or at least my understanding of the initial proposition. Going back over the discussion it seems like @AlexeyMerzlyakov and I latched onto @doisyg's description as quite literally regarding "if obstacles are in zone X, I want to execute behavior Y".

When obstacle in the zone, forbid linear speeds, but allow rotations / angular z speeds 
...
When obstacle in the zone, forbid angular speeds, forbid linear speed in one direction but allow in the inverted direction (i.e. when an obstacle is in front of the robot, allow to go backward)

Now you're really more proposing adjustment of polygons in proportion to speed which by their mere nature stops moving in certain directions. I'm not sure the stuff @doisyg mentioned about rotation really aligns with what we're discussing now? Has this idea evolved away from enacting particular behaviors or enabling particular behaviors when certain events happen in a particular zone?

That seems awfully similar to the approach model - why not just use that with the polygon set to your zone? Is it that you don't want it to continuously change (to instead have discrete zone states for ranges of velocities)?

If so that would invalidate my concerns regarding deconfliction of multiple overlapping zones, how to trigger or "enable" the appropriate behaviors when an event occurs in a zone -- if we're no longer talking about triggering behaviors and/or only "allowing" certain types of velocities

[SteveMacenski]

I just thought about it for a moment, you should definitely not use the odometric velocity, you should use the commanded velocity, so scheduling of polygons on velocity should be done in the server

[AlexeyMerzlyakov]

I just thought about it for a moment, you should definitely not use the odometric velocity, you should use the commanded velocity, so scheduling of polygons on velocity should be done in the server

Agree. When I've used odom velocity, it leaded to parasitic robot oscillations in APPROACH model due to positive timely feedback between speed decreased and interpolated time-to-collision. Which won't work well, so the best way - is to restrict intended cmd_vel speed rather than measure actual one for CM decisions making.

[SteveMacenski]

Any word @kaichie?

[kaichie]

Is this basically polygon-scheduling based on the robot velocity? Or is the actual velocity being taken into account beyond just if velocity in range, use this polygon (e.g. there's some base polygon that's adjusted/warped proportional to speed continously)

Currently it is just basic polygon switching based on both the odom velocity and commanded velocity. If the robot is stopped/very-slow, it will use the commanded velocity, else it will be using the odom velocity (this is due to the intention to mimick what a safety controller/PLC will do as mentioned earlier, but I agree we can just use the commanded velocity instead).

Ah, so the robot-is-stopped polygon being scheduled would contain essentially nothing except the robot so that it couldn't get "stuck" so to speak? I like that idea.

When the robot is stopped (odom 0 or commanded velocity 0), a default polygon(robot-is-stopped) is used. However, since the robot is not moving in this state, this polygon is not particularly important. What is more relevant is the polygon that is generated or adjusted when the robot starts to move. This polygon should exclude obstacles in the opposite direction of travel. In this case, if the Stop Model/Approach model uses this robot-is-stopped polygon, then it won't get stuck.

That seems awfully similar to the approach model - why not just use that with the polygon set to your zone? Is it that you don't want it to continuously change (to instead have discrete zone states for ranges of velocities)?

I tried this approach and found that, although it is possible, the robot will still move until the approach model polygon touches the obstacle (points in the polygon). When this happens, the robot is effectively 'stuck.' The reason for changing the zone is mainly to switch to an asymmetric polygon with a smaller area in the opposite direction of travel. You can think of the polygon as an offset rectangle along the direction of travel.

The idea about dynamic polygon_generator node that will be the part of the nav2_collision_monitor seems workable for me. This will allow to cover all the current use-cases from above and won't do CM to be hairy with an infinite number of exceptions.

I like the idea about dynamic polygon_generator node that will be the part of the nav2_collision_monitor, this will ensure generated polygon and active polygon are synced instead of setting them via topics/services. It prevents the CM allowing restricted command velocity due to race condition.

Now you're really more proposing adjustment of polygons in proportion to speed which by their mere nature stops moving in certain directions. I'm not sure the stuff @doisyg mentioned about rotation really aligns with what we're discussing now? Has this idea evolved away from enacting particular behaviors or enabling particular behaviors when certain events happen in a particular zone?

I think underlying we are trying to solve the same issue which is when the Approach Model or Stop Model's polygon comes into contact with an obstacle, the robot becomes permanently 'stuck' once there are points in the polygon. The goal is to enable the robot to escape in the opposite direction if the obstacle are just touching the edge of the zone/model.

Regarding the idea of discrete polygon switching based on speed and direction for CM Stop Model/Slow Model, I believe that if we incorporate this into CM, it would allow users to define the polygons and the conditions to switch (such as cmd_vel threshold/rotation threshold). For the Stop Model, this feature can be use to escape the "stuck" scenario, while for the Slow Model, user will be able to switch the zone according to the robot's speed. Nothing is set in stone, we can discuss whether it would be more effective to scale a "base" polygon, as previously mentioned.

[SteveMacenski]

Currently it is just basic polygon switching based on both the odom velocity and commanded velocity.

Would it be good to use a more dynamic adjusted polygon or do you prefer the static polygon for particular ranges of velocities? This is a relatively big thing that we should come to an agreement on since it vastly changes where the complexity lies (e.g. in the user configuration of scheduling the polyons or in the code in adjusting the polygon's different axes by speed).

I don't have a strong opinion. If the safety lidars say 0 < v < 0.4 use XYZ polygon, if 0.4 < v < 0.8 use ABC polygon, etc I'm fine with mimicking that behavior 1:1. However, we do have the ability to just have a polygon and warp it linearly to the velocity for a full range instead. But then, its always just that 1 polygon being warped around, not necessarily able to change in different velocity ranges so much.

I tried this approach and found that, although it is possible, the robot will still move until the approach model polygon touches the obstacle (points in the polygon). When this happens, the robot is effectively 'stuck.'

Ah, got it!