[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? :wink: 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 :wink: 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!

[AlexeyMerzlyakov]

Hmm... I'm being a bit late, but let's think about adding two options into collision monitor (CM): allow_move_backward and allow_rotations in emergency states (when more than threshold points are inside Stop or Approach polygon). The point is that CM can remember the last cmd_in_vel with what the collision was appeared. The reverse velocity in this case is easy to calculate (as a scalar product of last-crash and desired cmd_in_vel vectors) and be allowed. In this case, we could add the recovery mode inside CM sources and switch which recoveries are allowed intentionally by user when the crash at least with one polygon appeared.

These options to be set in the parameters, so CM should not think about polygon's geometry. So, just allowing to reverse (or rotating) motion in some cases although might be not safe; but in case of user intention - we could allow this. Why not?

The proc. of this approach - is that it is much more light-weight solution rather than making a separate server over the monitor, dynamically changing the Stop/Approach polygon as was shown here. Although, this is also doable solution, let's consider the most simple (in terms of performance) solution first. I think that is what Steve meant by:

Would it be good to use a more dynamic adjusted polygon or do you prefer the static polygon for particular ranges of velocities?

[SteveMacenski]

let's think about adding two options into collision monitor (CM): allow_move_backward and allow_rotations in emergency states

I think we're past that idea at this point, what Dexory brings up is more general and (apparently) aligns with the logic that safety sensors already do which we're just mimicking in this node anyway. Seems like the right tech choice.

@kaichie do you have docs for the lidar system you're referencing for us to review?

[kaichie]

Yes there are some documents come together with the lidar system. For example, you can refer to this document section 4.3.7. Alternatively you can also download the user manual from here and refer to section 4.

[SteveMacenski]

I'll say 4.3.7 wasn't very specific beyond what I intuitively understood but got it hah. I was hoping for some conventions around what are standard stopping distances and how they select the ranges of velocities to change the polygon dynamically to be (e.g. still, < 0.2 m/s, <0.5 m/s, 1.0m/s+) to know if we just need to provide a basically static and a moving full speed dynamic polygon ability -- or if we need to provide N different possibilities at arbitrary user defined ranges.

What do you think on that front?

[AlexeyMerzlyakov]

Yep, the 4.3.7.1 is specific for S300, but this indeed showing the idea behind that we should operate with. This is +1 in favor of the polygon_generator idea. This idea is also is more general one, so in case if we want to bring-up new behavioral changes (related to the concrete laser model, or to any new use-case), this will be more straightforward in terms of implementation & architecture, as well.

[kaichie]

I agree with @AlexeyMerzlyakov , and I am more inclined towards having a more general feature where the user can define N polygons and their conditions. A single dynamic scaling polygon could get more complicated when we consider the rotation profile for different types of robots too.

[SteveMacenski]

Sure thing, makes sense to me! This sounds more or less in line with what you have shown in your demonstrations above. What do you think is the path forward to add this feature @kaichie ? Is this something that you'll take the lead on to adapt your work or potentially make those changes on a public branch that Alexey or I can use as a starting point?

I'm not sure when we'd be able to prioritize this task over others, but we'd probably be able to get to it sometime this year (maybe over the summer), so if this is more time sensitive, I definitely encourage you taking the lead on it and we're always happy to help bounce ideas off of or architecture reviews.

[kaichie]

I would like to take the lead on adding this feature and adapting the work to incorporate the changes we discussed. I will work on a public branch and keep you guys updated here. My plan is to first assess the collision_monitor package and identify the appropriate location to incorporate the changes, then propose the necessary modifications (or create an initial draft on a branch if this is more preferable) for further review.

[SteveMacenski]

Got it! Keep us apprised and I'm sure we'd be happy to review your ideas / thoughts to make sure you're on the right track for @AlexeyMerzlyakov 's design.

[kaichie]

Thank you @AlexeyMerzlyakov and @SteveMacenski for the time and attention on this. I've created an initial draft on a branch and wanted to share it for further review before continuing.

In this initial commit, I've updated the parameter file to include a new parameter for the polygon_generator(just for testing). This is to allow the user to define multiple polygons as the source of the generator and customise their points and thresholds for different scenarios. The polygon_generator will be the third priority option for setting the polygon, only if there are no static points (first priority) or polygon_sub_topic (second priority) defined.

The selection of polygons is done in the processStopSlowdown() function, where it will check whether the velocity is within the range with: min <= abs(velocity) <= max. The first source of polygon that matches the condition will be selected.

I am also planning to add a new parameter called use_odom that will allow the user to specify whether to use odom as the velocity comparison for each of the source, so that it is more analogous to safety sensor and hardware features.

I would appreciate any feedback or suggestions on this initial draft. Are there any concerns or issues that I may have overlooked?

[AlexeyMerzlyakov]

@kaichie, thank you for the update about upcoming contribution. I'll check it shortly on the days, and will figure-out the feedback on it.

[SteveMacenski]

The min/max velocities look like they're overlapping, isn't that not really permissible? Each specific polygon needs to be member to mutually exclusive ranges of velocities?

Also the params min_x and min_rot (etc) don't specifically mention its a velocity. If x is not just x but translational (e.g. y for omni too) then we should rename it. rot should be theta just by convention of other similar parameters. Nit-picking, but something to change which is quick.

I think overall naming needs a review (PolygonSource, polygon_sources_, isUsingPolygonGenerator, updatePolygonGenerator, etc). I like the name PolygonGenerator, but it needs something to indicate its selecting a polygon based on velocity -- so perhaps isUsingPolygonVelocitySelector and updatePolygonByVelocity.

I don't understand the follow_x_direction param or its use.

---

I don't think this implementation is what we discussed, embedding the feature into the Polygon class itself. But, it generally seems to work and might be a better choice, but I'll let @AlexeyMerzlyakov give us his thoughts. It does require close scrutinization to make sure we're not creating edge cases we're not handling and the initialization of the different features are proper (and test coverage).

You've got a couple of TODOs in there that need to be flushed out. I'm not sure what's the role of the L185-L188 block.

---

bool Polygon::isUsingPolygonGenerator()
{
  if (polygon_sources_.empty()) {
    return false;
  }
  return true;
}

Can be simplified to return !polygon_sources_.empty();

But overall, this is more slickly put together than my original concept of how it might be possible. Good job!

[AlexeyMerzlyakov]

@kaichie, I've returned back with the local branch review. I think, it is a good start point and we will resulting to be at the same page, putting this contribution to life.

I've wrote the review separately trying to look on changes from the scratch, so some items (now I see) might repeat review from above mentioned by @SteveMacenski. The main items I've noted when checked the local branch are as follows:

1. The idea to update polygon directly instead of using messages topic mechanism is better in terms of performance. However, as we've previously discussing, the separate PolygonGenerator module could be better in architectural terms. It could just dynamically generate and publish geometry_msgs::msg::PolygonStamped message by some external node, and then using Polygon::updatePolygon() mechanism that is already supported by CM's polygons. If there is no restriction on performance using messages, this is more straightforward solution.
2. PolygonSource -> to separate module (already mentioned in the branch)
3. The condition of using polygon generator might be added in the beginning inside updatePolygonGenerator() routine (like was made for updatePolygon(). isUsingPolygonGenerator() is not needed.
4. Why the updatePolygonGenerator() is not being called for CollisionMonitor::processApproach() as well?
5. Polygon::updatePolygonGenerator() is seems to use only OX-linear velocity to check that the polygon is within speed range. However, this is not always true, since some robots might have OY-component of velocities (e.g. omni-drives). I suppose, the proper way is to use std::hypot(cmd_vel_in.x, cmd_vel_in.y) instead.
6. I think, I do not understand well the meaning of follow_x_direction_ and its usage in Polygon::updatePolygonGenerator(). What will be, if we are, again, operating with omni-robots having either OY-component of linear velicty?
7. Regarding parameters: am I understood correctly, that there are might be multiple polygon_generator-s per each polygon (why it has PARAMETER_STRING_ARRAY type)? If so, how they would interfere between each other? Are there any intersection & priority rules for them?
8. When using polygon_generator parameter, polygon_sub_topic to be empty. This means, that polygon generator now will gurantee the polygons consistency and that it is not empty. In this case we need to check carefully, that everything is being received (polygon shape), before will start to process it. It seems the unnecessary complication of the logic, and again brings to the mind of re-using already existing polygon updating mechanism through the messages, published by external module.

[SteveMacenski]

1. I actually prefer the proposed design better. Instead of modifying the polygons externally, its just a feature internally to it and its pretty minimally invasive to add there. In terms of the parameterization files too, it seems to make sense to have the gain-scheduled polygons to be described in the polygon namespace.

2. This is just for the zones adjusted by speed, I don't think the footprint for approach is appropriate for this kind of model. If we were only using the footprint to slow down, that would be one thing to gain schedule the footprint by the speed so that you can leave a wider berth when moving fast. But since the approach model will straight-up stop the system, adding additional margin could stop the robot from actually being able to get through legit areas. But maybe I'm wrong and folks would actually like to have the robot slow like a stop and then the gain scheduling would use the smaller polygon to allow it to pass again (but then it would start to speed up again and have this circular issue).

3. Agreed, I made a related but different point with this as well. I think they need to be mutually exclusive and complete in the range of velocities used by the system if you're going to have gain-scheduling happening (e.g. everywhere from 0 - max speed needs to be covered by a single polygon)

4. :+1:

[AlexeyMerzlyakov]

@SteveMacenski,

1. The proposed approach certainly makes sense, as well. The main reason - is to add minimal invasion for the code to keep it simple as possible. This is mostly related to checks from my point No.8: we can not miss the case when the polygon will have non-shape before it will be set by PolygonGenerator. Embedding this logic into Polygon is OK.

2. Yes, my question has the same root cause: during high speeds robot's footprint might be also increased towards to the motion during APPROACH model, or changed other way. I suppose to wait for the @kaichie opinion there.

[kaichie]

Regarding of 1. and 8.

1. Just to add that if this feature is embedded internally, it would ensure that there is no race condition for the twists command and polygon update too.

Regarding of 2. and role of the L185-L188 block + the TODOs.

2. the TODOs are to move the PolygonSourceclass/function to a new, separate class. I was trying to minimise the changes in my initial draft to show where to incorporate the main logic before refactoring it. More of a design/architectural question, should PolygonSource be a new class or just a struct in types.hpp?

3. Good to know! :+1:

4.

4. I was under the impression that the footprint is kind of static, so I am not sure if this is suitable for the APPROACH model. As the APPROACH model keeps the robot x seconds away from collision, when the robot is traveling at a higher speed, the logic is kind of doing the same thing to slow down the robot. It can be duplicated for the processApproach() so that it can be more aggressive in slowing it down.

Regarding of 5. and the param naming.

5. Agree to consider for omni-directional type robot, the parameters can be simplified to translational: [x_min, y_min, x_max, y_max] and theta: [min, max]. I think we still need to check the condition of the x, y, and theta components individually, or is it equivalent to check with std::hypot(cmd_vel_in.x, cmd_vel_in.y)?

Regarding of 6. and the follow_x_direction param or its use.

6. This is to simplify the definition of a polygon for a bi-directional robot use-case, where the polygon will be flipped along the direction of travel for the x-axis, just like the GIF above. For example, we can avoid defining two different sets of polygons for the following scenario: x_min = 0.1, x_max = 0.2 and x_min = -0.1, x_max = -0.2. If this makes sense, we might want to flip it for y too for omni-directional types of robots.

Regarding of 7. and polygon needs to be member to mutually exclusive ranges of velocities.

7. I agree that the min/max velocities might overlap in my initial draft, as I didn't intend for each specific polygon to be limited to mutually exclusive ranges of velocities. Rather, it's up to the user to define the velocity conditions for each individual polygon, and the polygon will be selected based on the first polygon in polygon_generator(PARAMETER_STRING_ARRAY) parameter that matches its condition. Nonetheless, it would be a good idea to make them mutually exclusive to ranges of velocities. I am still thinking how do we enforce this by design, considering that there are three different velocities: translational (x, y) and rotational (theta) speed?

[AlexeyMerzlyakov]

Just to add that if this feature is embedded internally, it would ensure that there is no race condition for the twists command and polygon update too.

Sounds reasonable, especially when velocities could change dramatically in a time :+1:

More of a design/architectural question, should PolygonSource be a new class or just a struct in types.hpp?

As I understand from ToDo comment, PolygonSource class will embed the logic defining how polygon will be changed during the speed and time. If I understand this intention correctly, PolygonSource by its design should be in separate file/module: types.hpp is a generally used simple structs, as velocity, pose, etc, action type...

I think we still need to check the condition of the x, y, and theta components individually, or is it equivalent to check with std::hypot(cmd_vel_in.x, cmd_vel_in.y)?

The linear twist is being published towards to the robot base for diff-drives and Ackermann platforms. For the omni-drives, OY component of speed could be persist, which makes the situation as follows at the picture below:

For this, the vector of linear speed should be defined as std::hypot(cmd_vel_in.x, cmd_vel_in.y). I'd suppose to compare it within linear_min ... linear_max range. And rotation to have the same logic theta_min ... theta_max range of angular OZ-absolute value. If we will check x, y components individually, we could get into the problem, where each component is less than linear_max, while summary speed will be ~ 1.41 * linear_max, which is more more than maximum allowed speed.

That is the problem of defining robot direction in omni-case. The robot could move towards to obstacle having positive or either negetive OX and OY components of speeds. That is why, the logic written in https://github.com/kaichie/navigation2/blob/7961ea12d94895481dd579a73525299550e1b688/nav2_collision_monitor/src/polygon.cpp#L178-L179 won't work. I'd suppose to save the latest velocity, used when zone was "crashed" into obstacle points, and use reverse velocity by just scalar multiplying two linear vectors, having near -1.0 value (the same for rotation).

By the way I might not correctly understand the physical meaning of min_* and max_* values for speed and twists restriction. What will be, if robot is moving faster / or slower that allowed speed: robot's polygon will not be changed in this case? In this case, max_* restriction still has place (but robot's maximum speed was already restricted by a controller, so this could be an extra-safety measure). But why we won't allow to move backwards or rotate for slowly moving robots?

Regarding parameter 7. - it is still not clear for me. Could you please describe the case, when it is necessary for one polygon to have more that one polygon generator? I'd supposed the design that one polygon generator is looking over one polygon; but not the case, where one polygon will have 2 or more polygon generators?

[kaichie]

Regarding the polygon_generator defined with PARAMETER_STRING_ARRAY, I believe the naming may be causing some confusion, as pointed out by @SteveMacenski as well. Instead of PolygonGenerator, a more appropriate name could be VelocityBasedPolygon or ConditionalBasedPolygon. A Polygon (slowdown/stop) can contain multiple conditional-based-polygons for switching, for example: Condition 1 (low speed): 0 <= speed <= 0.2, use points[x1, y1, x2, y2, ...] Condition 2 (high speed): 0.2 <= speed <= 0.7, use points[x1, y1, x2, y2, ...]

This also means that the current definition of min_* and max_* is the expected velocity range for each velocity component in each conditional-based-polygon.

Thank you for explaining the use of std::hypot(cmd_vel_in.x, cmd_vel_in.y) to calculate the magnitude of the twists (linear) and compare with the linear_min and linear_max condition. I agree that the logic written here will not work. If that is the case, we could calculate the robot's direction using std::atan2(cmd_vel_in.y, cmd_vel_in.x) and use this as another condition for switching polygons, as well as updating the logic. So instead of comparing x, y, and theta component individually, we will be comparing twist(linear), direction, and theta, which can cover most of the model cases correctly(except for Ackermann steering type - which may have a different definition of twist?).

I will update the items that we have addressed so far.

[AlexeyMerzlyakov]

@kaichie, OK, I've finally got it after your explanation. It appears, there will be steppy discrete changes in polygons depnding on velocity (angular/linear). It should work for backwards linear moving when robot is touched the keepout zone. How will you allow rotations at-place when robot is being stuck in the keepout zone?

I will update the items that we have addressed so far.

Great. Waiting for the next update to be ready for review.

[SteveMacenski]

Hi! What's the status here? I'm back on the ball

[AlexeyMerzlyakov]

@doisyg, @kaichie, Hi! Are there any updates on this ticket?

[doisyg]

Hi there! No specific rush, but discussed with Steve over the ROSCON fr, it would be nice to organize a meeting/WG with people using the CM out there. We all use different tricks and we could unite. I know there are at least :

• Dexory (us with @kaichie )
• Robocc (@BriceRenaudeau @anaelle-sw https://indymotion.fr/w/2e284vtvCvLS3QxURDwAMP?start=35m45s)
• Polymath ? @SteveMacenski ?

[tonynajjar]

At Pixel Robotics we also use CM and have some tricks in the pipeline waiting to be contributed back. @jplapp FYI

[SteveMacenski]

We can certainly have a meeting, but if my memory serves, this PR is mostly waiting for the implementation submission, no? I think from ROSCon-FR talks this might be already done and just needs to be upstreamed :-)

I'm definitely happy to organize a meeting if folks want to come with topics! Alex Yuen from Polymath would be the right contact point. I just pinged him.

[kaichie]

I have made the following updates to the draft implementation:

• Updated the twist linear speed checking instead of per axis speed using (hypot(x, y)).
• Added the direction range check (min -> max) in the normalised range of [-π,π]
• Renamed polygon generator to PolygonVelocity
• Included PolygonVelocity in the APPROACH model as well.
• Removed the auto-flipping of polygon based on direction
• Moved PolygonVelocity to a new file.
• Can be tested with ros2 launch nav2_bringup tb3_simulation_launch.py and ros2 launch nav2_collision_monitor collision_monitor_node.launch.py use_composition:=False in the branch.

However, the checking of the PolygonVelocity to ensure they are mutually exclusive and cover the full range of velocities are not done yet. And also I am considering if we should introduce a default polygon (or using the first polygon) if the speed is not covered.

[SteveMacenski]

Great! When could we potentially expect something for us to review / test?

Shouldn't in the situation that you're specifying polygons per velocity range have a polygon for each valid velocity possible where you care about collision? I don't think there's a reason to have a "default" because it should always be covered. If we're going at a speed in that mode without a polygon, I think that's a error logging failure kind of situation -- or assume the user is smart enough to know that they don't want a polygon in that particular range. Applying something silently as a default feels dangerous if there's a range improperly covered.

[kaichie]

Created the PR for review and testing based on the changes above.

Shouldn't in the situation that you're specifying polygons per velocity range have a polygon for each valid velocity possible where you care about collision?

Yes it is, just that I am unsure about the approriate method to check if the polygons provided covered the full velocity range (in term of linear, direction and also rotational speed).

[SteveMacenski]

I suppose if we know the max velocity (perhaps parameters), we could loop over the different polygons and grab their velocity ranges and insert them into a vector or similar for sorting. Then, it should be easy to check if the minimum is less than or equal to the minimum velocities (and vise versa for max). It should also be equally easy to loop over the sorted ranges and make sure the max's of one line up with the min's of the next so that there is continuous coverage.

If we don't know the max/min velocities, we could still take the polygon ranges and check for gaps. That might be my suggestion and simply log errors when velocities are exceeding the last polygon as a run-time error (so we have less, redundant parameters)

Perhaps also a param for allowing gaps to exist, for the cases that is desirable (but probably just for prototyping / testing)

[kaichie]

@SteveMacenski Thanks for the suggestion. I have been trying to figure this out and if I understand correctly, this should be able to find the coverage range of a single parameter but I still could not see how this will works if we are checking 3 different ranges at once (linear, direction and rotational speed).

For example, let's consider only the linear + directional range for simplicity, and the minimum and maximum values of the ranges are provided in the config:

linear range : 0 -> 1
direction range: -pi to pi
polygon 1: linear_min:0, linear_max:0.5, direction_min: -pi, direction max: 0 
polygon 2: linear_min:0.5, linear_max:1.0, direction_min: 0, direction max: pi

If we are iterating through the polygons and evaluating the continuous coverage for each range individually, they should collectively cover the full range. However, in this example, the unprotected range is:

range 1: linear_min:0, linear_max:0.5, direction_min: 0, direction max: pi 
range 2: linear_min:0.5, linear_max:1, direction_min: -pi, direction max: 0 

Please let me know if I have intepreted this incorrectly.

[SteveMacenski]

If you have a polygon of some N points that is the new collision zone, it should have the range of linear and angular velocities from the Twist which assign when this particular zone should be used. I don't know what this means:

linear range : 0 -> 1
direction range: -pi to pi
polygon 1: linear_min:0, linear_max:0.5, direction_min: -pi, direction max: 0 
polygon 2: linear_min:0.5, linear_max:1.0, direction_min: 0, direction max: pi

since you have a "linear range" and "direction range" yet the polyon1/2 has the ranges in them. Plus, I don't understand the "direction" bit. Velocities are signed. Alot of that info seems unnecessary and confusing. Lets talk about polygons and twists - which is the fundamental structure of data incoming to us.

The mutually exclusive range works for the velocity ranges, I did not think much about having restrictions also including angular velocities as part of it. Either way, it just turns from a 1D range check (e.g. for each value in min_vel to max_vel, do we have coverage?) into a 2D range check (e.g. for each value in min_vel to max_vel, does each range of -rot vel to rot vel have coverage?).

That does revive the idea in my head of having a "default" polygon, as long as that default is explicitly set (not implicitly as just the first polygon) if we don't think folks will fully fill in the range of the fields of Twist. Or, just have documentation explaining an intended workflow for how to define these (eg start with making polygons for each linear velocity range covering all angular velocities -- then override certain ranges of angular velocities as needed for specialized behaviors). That way, everything is promised to be covered and some have additional specializations. The nonspecialized can be the "defaults" for particular velocity ranges if they don't meet any of the specific angular velocity ranges.

Or, we could just make it the user's problem not to do something "stupid" and assume what they put in is error free and they know exactly when they want polygons and when they don't. Some default param profiles (like for example what Brice did in his ROSConFR talk) could do some heavy lifting to make sure users start with a "reasonable" profile that they can adjust. Maybe we don't need to overcomplicate it, though if there are "easy" ways to protect users from themselves, that is always beneficial. Perhaps just some run-time throttled error logging if the robot's velocity ends up in some ranges without polygon support is enough?

[kaichie]

Lets talk about polygons and twists - which is the fundamental structure of data incoming to us.

For twists, we are aiming to cover both omni-directional(and Ackermann?) and diff-drive type. My primary reference for the current implementation was this comment. By using the x and y component of the twists, I obtain the unsigned magnitude(linear) and direction of moving. Other than that, we also factor in the theta component from the twists, resulting in the need to define three parameter ranges. Does this makes sense?

Allowing the user to create a default polygon (within a specific range) sounds suitable to cover the nonspecialized ranges. However, in scenarios where user intends to leave some ranges empty, we need to allow user to set empty polygon or set a new parameter(for example: ignore/enable) to bypass the collision check. And as you mentioned, if velocities still fall within the undefined range, we will have the runtime-throttled error logging. I think this method will provide users with fundamental protection while configuring the default polygon, effectively encompassing the robot's default velocity range as well.

eg start with making polygons for each linear velocity range covering all angular velocities -- then override certain ranges of angular velocities as needed for specialized behaviors

Good guideline to start with and will include in the documentation.

[SteveMacenski]

I didn't think about a separate X vs Y (vs XY) set of limits. OK, so that's 3D then or omni types - maybe a param for whether its holonomic or not so we know whether to consider that range or not.

That sounds reasonable. I'm just looking to protect users from themselves of misconfiguration, but maybe we can accomplish that via documentation instead. I suppose we don't need to check the full range completion then.

[SteveMacenski]

To be merged imminently!

[robotcopper]

To be merged imminently!

Hi, excuse me for digging up this topic, but will velocity_polygons be back-ported to humble? Because it seems that it is only available for iron for the moment if I'm not mistaken.

[SteveMacenski]

No, it cannot be backported as it breaks ABI/API stability policies.