-
Notifications
You must be signed in to change notification settings - Fork 1.2k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
[Collision monitor] feature discussion: Stop zone with allowed rotation or inverted direction #3313
Comments
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. |
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 |
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:
First and second options have the disadvantages described by @SteveMacenski, in the above message:
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. |
Thanks you both for your inputs. That fueled a nice brainstorming with @kaichie and we debated many different options.
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:
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 |
I'm not sure I quite understand your meaning there or its specific intent. Also, that should be possible now (?)
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.
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 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. |
@doisyg thoughts? |
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) |
Let's return back to the discussion after some NY break.
The idea about dynamic However, can't we do something even simpler to cover these 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. |
Is this basically polygon-scheduling based on the robot velocity? Or is the actual velocity being taken into account beyond just
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 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".
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 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 |
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 |
Any word @kaichie? |
Currently it is just basic polygon switching based on both the
When the robot is stopped (
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.
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.
I think underlying we are trying to solve the same issue which is when the 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 |
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
Ah, got it! |
Hmm... I'm being a bit late, but let's think about adding two options into collision monitor (CM): 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:
|
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? |
I'll say What do you think on that front? |
Yep, the |
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. |
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. |
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 |
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. |
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 The selection of polygons is done in the I am also planning to add a new parameter called I would appreciate any feedback or suggestions on this initial draft. Are there any concerns or issues that I may have overlooked? |
@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. |
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 I think overall naming needs a review ( I don't understand the I don't think this implementation is what we discussed, embedding the feature into the 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.
Can be simplified to But overall, this is more slickly put together than my original concept of how it might be possible. Good job! |
@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:
|
|
|
|
Sounds reasonable, especially when velocities could change dramatically in a time 👍
As I understand from ToDo comment,
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 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 By the way I might not correctly understand the physical meaning of 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? |
Regarding the This also means that the current definition of Thank you for explaining the use of I will update the items that we have addressed so far. |
@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?
Great. Waiting for the next update to be ready for review. |
Hi! What's the status here? I'm back on the ball |
Hi there!
|
At Pixel Robotics we also use CM and have some tricks in the pipeline waiting to be contributed back. @jplapp FYI |
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. |
I have made the following updates to the draft implementation:
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. |
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. |
Created the PR for review and testing based on the changes above.
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). |
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) |
@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. |
If you have a polygon of some
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 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? |
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( 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:
Good guideline to start with and will include in the documentation. |
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. |
To be merged imminently! |
Hi, |
No, it cannot be backported as it breaks ABI/API stability policies. |
Oh, thanks @SteveMacenski for your fast reply. Do you think it's possible (for example, for me) to backport this functionality into a similar package that would not be part of the Nav2 stack but would work independently? To avoid this stability policies issue. |
Nothing prevents you from forking Nav2 and backporting the feature in your fork |
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.
The text was updated successfully, but these errors were encountered: