Add exact path following in rpp for feasible plans #5496
Conversation
Signed-off-by: mini-1235 <[email protected]>
Signed-off-by: mini-1235 <[email protected]>
Signed-off-by: mini-1235 <[email protected]>
|
One thing I am not sure here: is I will show my results here Before this PR: After this PR: |
|
@mini-1235 if we're talking about moving the path handler into the controller server in #5446, why is this action necessary? |
| if (dist_to_cusp < lookahead_dist) { | ||
| lookahead_dist = dist_to_cusp; | ||
| } | ||
| if (dist_to_cusp < curv_lookahead_dist) { |
There was a problem hiding this comment.
I think this is a subtle point worth narrowing in on a little. The curvature lookahead is updated on approach to the goal that is otherwise left unchecked on L191 above. I think we should still check for this case or is that implicitly handled correctly now from your other updates?
There was a problem hiding this comment.
I remember that in my experiments, when I set enforce_path_inversion to true, both the condition here
if (dist_to_cusp < lookahead_dist) {
lookahead_dist = dist_to_cusp;
}
if (dist_to_cusp < curv_lookahead_dist) {
curv_lookahead_dist = dist_to_cusp;
}
is never true, I will double check again tomorrow
I agree, but I thought in #4763 (comment) we discussed that we are going to implement/verify the missing logic in RPP/Graceful/DWB 's path handler, and then finally moving the path_handler into controller server if possible in #5446, no? IMO, we can just keep it temporarily in nav2 utils, and after we have tested in all controllers, we can remove them. If you are worrying about the code coverage, perhaps I can add something like smachybrid + RPP to the system tests(I guess what we have currently is Navfn + dwb/mppi?) |
Signed-off-by: mini-1235 <[email protected]>
|
@SteveMacenski, I have done more experiments, at the time I raised this PR, I have only tested with ackermann. Recently,I have tested with differential model as well. Sharing my results here:
if (dist_to_cusp < lookahead_dist) {
lookahead_dist = dist_to_cusp;
}
if (dist_to_cusp < curv_lookahead_dist) {
curv_lookahead_dist = dist_to_cusp;
}I think this is reasonable, since we are pruning the path containing cusp points in the path handler When setting with differential model, the above condition can be true even if Linking related tickets here: #5098, #3934, I have also gone through them and make sure it works fine after my changes in this PR
double last_theta = tf2::getYaw(path.poses[idx - 1].pose.orientation);
double dtheta = angles::shortest_angular_distance(last_theta, cur_theta);
if (fabs(oa_x) < 1e-4 && fabs(oa_y) < 1e-4 && fabs(dtheta) > 1e-4) {
curr_segment.end = idx;
segments.push_back(curr_segment);
curr_segment.start = idx;
}here navigation2/nav2_smoother/include/nav2_smoother/smoother_utils.hpp Lines 78 to 85 in 627a8b6 Is it because idx 0 (start) and idx 1 will always differ in position? |
|
@mini-1235 I will admit that I'm starting to lose the thread on these PRs that are all touching similar sections of the code. I'd love to get some of these in 😆 . What's actually going on with #4763? Is that still relevant now with this PR and moving the path handler into the controller server?
I think what I'm meaning in that comment is that we're updating the RPP logic to better handle these things. If we're doing that for RPP, should we also simultaneously update DWB/Graceful controllers so that they're all consistent (as MPPI already has it). If we're moving the path handler to the controller server, then that does it implicitly. So I guess we can ignore that for now.
OK - but also if we're moving the path handler globally into the controller server in the other one - do we need this PR? Though I do understand this work in RPP is going to be shared across them so its useful as a stepping stone to do it separately. Is that your understanding as well? |
Ah, this was my thinking: I get that for But I'm realizing now the path is already pruned so even if we don't reduce this distance, it will only get up to the inversion anyway 👍 I grepped through the code and don't see that snippet you mention with |
Ok, I will try to conclude everything here:
So, my original plan here is to update RPP, Graceful, and then DWB, some might only involve updating the path handler, while others might need to update its internal logic as well. If everything works properly after my PRs in DWB/Graceful/RPP/MPPI then I can continue to move the path handlers to controller server. Personally, I would prefer to have this PR merged in, this helps me to test the logic when the other two controllers are paired with feasible planners. Also, it seems like we need to do a major update to DWB, I think it will be too much to do all of these in #5446, what do you think? |
I mean this part navigation2/nav2_smoother/include/nav2_smoother/smoother_utils.hpp Lines 78 to 85 in 627a8b6 double last_theta = tf2::getYaw(path.poses[idx - 1].pose.orientation);
double dtheta = angles::shortest_angular_distance(last_theta, cur_theta);
if (fabs(oa_x) < 1e-4 && fabs(oa_y) < 1e-4 && fabs(dtheta) > 1e-4) {
curr_segment.end = idx;
segments.push_back(curr_segment);
curr_segment.start = idx;
}
double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation);
double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation);
double dtheta = angles::shortest_angular_distance(cur_theta, next_theta);
if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) {
curr_segment.end = idx;
segments.push_back(curr_segment);
curr_segment.start = idx;
}no? |
|
Thanks!
Should this be closed since its been open for most of a year? If we're fixing in
MPPI sounds like that's all set. RPP it sounds like from the context of this PR that's been refactored and working, no? For graceful, if we just use the Controller Server-based Path Handler, doesn't that enable that feature into Graceful - We'd just need to test? DWB doesn't handle path inversions really, it doesn't have the critics that consider the path-point's orientation. While that's notable to address, I don't think its critical to resolve before moving the path handler into the controller server as long as it works as well as it works now when given path inversions. If its not respecting them now and doesn't respect them with the Controller Server-based Path Handler, then we're no worse for wear.
Is your plan to incrementally add these features to each to test, then refactor to remove it from the controllers into the controller server? If so, #5446 would be a WIP draft. I'm actually OK with it all in #5446 except for vast DWB changes - as long as DWB still works as well as it works now. If it breaks, then I agree with a more piecemeal strategy. If a temporary API for DWB to 'skip' inversion consideration for a controller is possible, then that would be OK too to merge in and then remove that + fix DWB in a follow up PR would be fine. On the smoother, I don't think your snippet compiles since it references variables not yet defined. I think I catch your drift, but is this trying to fix an actual issue or something notional? Note that if we did that for just the last path point, that would be a segment size of 1. We can't smooth a segment size of one - that's not meaningful. |
I guess we can close the PR, if the original author / others is interested in implementing this feature targeting humble, I can help reviewing as well
Yes, I think RPP is ready to go
Ahhh, Ok, I thought handling path inversions in DWB is a must before moving on in #5446. Then I think I can just close this PR and move everything here to #5446, right?
It was just something I am trying to understand when reading planner's navigation2/nav2_smac_planner/src/smoother.cpp Lines 238 to 267 in 7468843 Smoother's navigation2/nav2_smoother/include/nav2_smoother/smoother_utils.hpp Lines 51 to 92 in 7468843 vs RPP's |
OK! Can you link the ticket #4757 as solved by your PR then so that it autocloses on merge?
As long as (a) DWB is not handling it correctly now and (b) the behavior is consistent afterwards in the way that its still not working. A follow up PR would be great to make DWB work, but if its not working right now, its not breaking anything to have it still not work then 😆 Just make sure to either have the defaults set to a working state for DWB or have DWB explicitly disable that feature by setting the parameter itself to disable if it doesn't work (and print a warning) If that's all good, then we can move it over and you're welcome to close this.
The Smac |
I think you are referring to MPPI's navigation2/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp Lines 518 to 545 in 547820f We actually cares about angle in RPP because we want to handle in place rotation, no? |
|
RPP too, we just return a distance: |
|
Sorry if I wasn't clear earlier, what I wanted to discuss is actually this part about in place rotation RPP evaluates both last_orientation → current_orientation and current_orientation → next_orientation, whereas the planner and smoother only evaluate current_orientation → next_orientation, I feel like this is not exactly the same for me |
|
Ah. The Smoother Utils is used by the Simple / SG Smoothers which do not specifically model or care about in place rotations. So we don't consider that in those. The Smac Planner doesn't check the last point because we enforce boundary conditions elsewhere in the smoother (I believe) so we maintain that orientation after smoothing for any possible in-place rotation of the final goal angle. So while not modeled as a new path segment, it is maintained after a smoothing process either way -- whether including an in-place rotation or not. Both of these are generating the list of path segments to smooth individually, whereas the RPP controller is looking for when they happen, which could/often will happen in the very first starting path point to rotate to a new orientation before starting to track a path. So we check MPPI might be a gap - though I don't think anything in MPPI right now will actually listen to a planned request for an in place rotation explicitly. We could detect the cusp to stop, but will then start from that point which may not exactly include an in place rotation (as much as generally then going in that new orientation in the more MPPI-flexible-behavior fashion). MPPI isn't as rigid as RPP, but we could make it listen to it for cusp detection for pruning and then let the critics decide how to handle it! Might be good as parameterization though. |
Thanks for the explanation! |
|
Review what I said as being sensible / accurate. I wouldn't be surprised if there were gaps |
I believe what we discussed here is related to #2683, I will revisit the discussions here when I have time to go through the ticket |
|
#5446 supersedes |
2 participants
Basic Info
Description of contribution in a few bullet points
Description of documentation updates required from your changes
New parameters are added
Description of how this change was tested
In gazebo sim, I will show the result below
Future work that may be required in bullet points
For graceful and DWB as well?
For Maintainers:
backport-*.