RPP Dexory tweaks #4140

doisyg · 2024-02-23T16:12:36Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	Similar to #3788 (comment)
Primary OS tested on	Ubuntu
Robotic platform tested on	Dexory Robot
Does this PR contain AI generated software?	NO

Description of contribution in a few bullet points

This is me finally contributing back after rebasing and testing with latest main the RPP tweaks that we have been using for months in production.
Warning, we only use RPP for linear paths, so caution when generalizing the usage of the added feature.

New behavior controlled by a new parameter interpolate_curvature_after_goal (default false) that interpolates a carrot after the goal in order to maintain a constant curvature lookahead distance. This is to avoid instabilities at the end of the path on the generation of the angular speed. The carrot used for the linear speed computation stays the same.
- Interpolation is based on the orientation of the vector formed by the last 2 poses of the path. Hence paths of length 1 are rejected when interpolate_curvature_after_goal is true. It required to modify a bit the path handler logic to be sure to keep at least 2 poses on the pruned plan.
- It can be used only when use_fixed_curvature_lookahead: true
- Publish curvature_lookahead_point similarly to lookahead_point for visualisation

2024-02-23.16-18-43.mp4

Fix conflict between use_rotate_to_heading and allow_reversing (added x_vel_sign parameter to shouldRotateToPath for that). So use_rotate_to_heading can now be used backward.

These 2 tweaks are a bit intertwined, hence contributing them together. This may be partially redundant with #3788 @james-ward

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

Check that any new parameters added are updated in navigation.ros.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

SteveMacenski · 2024-02-24T06:43:17Z

@james-ward can you look at this and confirm its a suitable alternative? If so, I can fix up a small number of pendantic things I'd like to change here (remove AMCL dependency...) and merge it. Or if there are elements of your PR you think need to still be here, let me know and I can make sure to review them both as a pair

james-ward · 2024-02-26T01:46:15Z

@james-ward can you look at this and confirm its a suitable alternative? If so, I can fix up a small number of pendantic things I'd like to change here (remove AMCL dependency...) and merge it. Or if there are elements of your PR you think need to still be here, let me know and I can make sure to review them both as a pair

LGTM. This is essentially what I had - the only difference is that I didn't force the path to always contain at least two points, as I wasn't sure if this would break some other behaviour (ie maybe the pruning of already passed points was necessary for correct behaviour of something else.) If that is not the case, this PR is good for me.

I can test on our platforms if the two of you wish, but I can't do that until later in the week.

james-ward · 2024-02-26T22:17:12Z

The only other thing is that you might want to adapt the new test I wrote on my branch for the actual act of projecting off the end of the path?
james-ward@e9dd766

SteveMacenski · 2024-02-27T00:11:53Z

Great! @doisyg can you add that and rebase on main, that should make CI fully turn over

SteveMacenski · 2024-02-27T00:12:09Z

nav2_regulated_pure_pursuit_controller/CMakeLists.txt

@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_regulated_pure_pursuit_controller)

 find_package(ament_cmake REQUIRED)
+find_package(nav2_amcl REQUIRED)


TODO Me: remove AMCL dep and use angles

oh yeah forgot about than one and your eyes when you saw that we are still using the good old amcl angles utils

SteveMacenski · 2024-02-27T00:13:39Z

nav2_regulated_pure_pursuit_controller/README.md

@@ -90,6 +90,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin
 | `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | 
 | `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | 
 | `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | 
+| `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvate calculation (to avoid oscilaltions at the end of the path) | 


Question for @doisyg @james-ward : Do you feel there's any reason to make this a parameterization or always be in play?

I think it should always be in play - for two reasons.
First, it preserves the user's intent when they set the minimum carrot distance.
Second, it only engages in the last segment and fixes bad control behaviour - I don't see a downside to having it running.

@doisyg if you agree, you can remove the param and make it the always-on behavior

On that one, I would be a bit conservative and keep it as a parameter for a while, then changing it later to default true, and finally then removing it. A bit like with the former use_interpolation.
I am only very confident for straight path.

@james-ward can you give this a whirl and let us know what you think? He's testing this on straight-line paths in warehouses, so they don't end on curves (they use other algorithms for free space controlling). If so, we can simplify this a bit

doisyg · 2024-02-27T21:55:41Z

Great! @doisyg can you add that and rebase on main, that should make CI fully turn over

Yes! @james-ward just the test of that commit james-ward@e9dd766, or all the added tests in your branch ?

james-ward · 2024-02-27T22:24:49Z

Great! @doisyg can you add that and rebase on main, that should make CI fully turn over

Yes! @james-ward just the test of that commit james-ward@e9dd766, or all the added tests in your branch ?

I think only that commit is relevant. The others were for internal functions that I created.

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

SteveMacenski · 2024-02-28T17:11:42Z

I took a look, beyond the parameter remark, I think this is good to go. Once merged, I'll remove the AMCL bits myself.

Without parameter:

Just need you to do the migration guide entry (add that gif!)

With parameter

Without parameter instructions and ...
Param guide entry for the RPP param

doisyg · 2024-02-28T19:37:35Z

Rebased on main and added / adapted your test @james-ward (testing full quadrant and no single pose path as we reject them now).
If all good, will do the documentation asap

codecov · 2024-02-28T20:45:42Z

Codecov Report

Attention: Patch coverage is 81.08108% with 7 lines in your changes are missing coverage. Please review.

Project coverage is 89.87%. Comparing base (d509fbf) to head (a3b230b).
Report is 7 commits behind head on main.

❗ Current head a3b230b differs from pull request most recent head 994351e. Consider uploading reports for the commit 994351e to get more accurate results

Files	Patch %	Lines
...ntroller/src/regulated_pure_pursuit_controller.cpp	84.00%	4 Missing ⚠️
..._pure_pursuit_controller/src/parameter_handler.cpp	77.77%	2 Missing ⚠️
...lated_pure_pursuit_controller/src/path_handler.cpp	66.66%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #4140      +/-   ##
==========================================
- Coverage   90.01%   89.87%   -0.15%     
==========================================
  Files         432      432              
  Lines       19406    19431      +25     
==========================================
- Hits        17469    17463       -6     
- Misses       1937     1968      +31

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

doisyg · 2024-02-29T08:11:15Z

Docs: ros-navigation/docs.nav2.org#530

SteveMacenski · 2024-02-29T21:07:00Z

LGTM pending doc fix and if @james-ward responds to #4140 (comment)

james-ward · 2024-02-29T23:25:14Z

LGTM pending doc fix and if @james-ward responds to #4140 (comment)

I'll give it a run this afternoon.

james-ward · 2024-03-01T06:16:20Z

I'm afraid I wasn't able to test this. We've had to run my patch up until now to make our platforms drivable, and the mainline has diverged enough that this wasn't just a simple update, keeping the other plugins and configs the same. I could not get the platform to move at all.

I will have another look on Monday.

SteveMacenski · 2024-03-01T17:26:48Z

Thanks, I really appreciate your time here. Making sure all the stakeholders are happy and not going to come back in 3 weeks because we made an error saves us all frustration by having to context switch back 😄

james-ward · 2024-03-04T04:41:09Z

This PR does have the effect of projecting the carrot, but it is not smooth. There are big jumps in the projected position. So it isn't keeping the look ahead distance constant as is desired.
carrot_short.webm

Above video is live from our platform.

This is because the current robot position is assumed to be the second last pose in the path (https://github.com/ros-planning/navigation2/blob/a3b230bb0ed0adac1b4ebb976437eb9e8b7a568e/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp#L354), not its actual position. So the carrot will jump ahead by the spacing of the poses in the path. For big spacings this will result in jumpy carrot movement.

doisyg · 2024-03-04T08:09:24Z

Hi @james-ward, fair enough, we have been using paths/plans with small spacing, hence it works smoothly for us.

auto current_robot_pose_it = transformed_plan.poses.begin(); is maybe not the best name as it is the first pose of the pruned plan, not the real robot pose. It is used here only to compute the curvature carrot distance to interpolate after the goal.
We don't want to use the real robot pose for that in order to have a stable carrot, but the closest path pose, which should be the first pose of the pruned plan. However, as you pointed, the accuracy of using the first pose of the pruned plan is limited by the plan pose density.

An easy workaround would be to use a denser plan. A full solution will be to interpolate between the pose of the plan. Maybe as a separate PR on top of this one ? WDYT ?

james-ward · 2024-03-04T12:25:59Z

The robot position is (0,0) because the PathHandler keeps translating and pruning the path so that it is relative to the robot.

So before the last segment, calling circleSegmentIntersection gets the point on the current segment that is exactly the look ahead distance away from the current robot position.

But this PR introduces different behaviour in the last segment. It finds the length of the last segment, subtracts it from the look ahead distance and then projects the carrot past the segment end by the remaining amount. For a robot at/very near the goal pose, the actual look ahead will end up being the desired lookahead minus the last segment length.

I think that because you force the controller to not drop the second last pose in the path even when the robot has passed it, you could just call the circleSegmentIntersection method on the last segment and it would work. (I think the maths doesn't actually distinguish between finite segments and infinite lines). But failing that, do what I did in my proposed solution and project the segment an additional distance equal to the look ahead and then do the intersection. This would ensure that the extended segment is longer than the look ahead and an intersection with the circle is guaranteed.

james-ward · 2024-03-07T01:11:01Z

@doisyg I have made the changes necessary to keep the carrot at the correct look ahead distance. It clearly wasn't correct before as the tests were looking for distances that were different from the look ahead distance.

My commit is here:
james-ward@1a8b5b3

SteveMacenski · 2024-03-07T18:21:20Z

Do we suppose that closes the loop on this feature? I'm defaulting to whatever you both come to an agreement on for this one - I trust your opinions 😄

james-ward · 2024-03-11T11:35:33Z

Do we suppose that closes the loop on this feature? I'm defaulting to whatever you both come to an agreement on for this one - I trust your opinions 😄

It does from my point of view. I have been running that latest patch since last week and it is good for us.

SteveMacenski · 2024-03-11T19:03:12Z

@doisyg I know you're in Florida, so take your time, but let me know and push the update if appropriate! 😄

mergify · 2024-03-13T17:59:19Z

@doisyg, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

doisyg · 2024-03-13T18:01:51Z

Thanks a lot @james-ward @SteveMacenski for you patience, and sorry that my iterations are a bit slow.
Pushed your commit, fixed lint and tested in simulation, looks good to go.

Signed-off-by: Guillaume Doisy <[email protected]>

…uit_controller.cpp Signed-off-by: Steve Macenski <[email protected]>

Signed-off-by: Steve Macenski <[email protected]>

Signed-off-by: Guillaume Doisy <[email protected]>

This guarantees that the look ahead distance is maintained Signed-off-by: James Ward <[email protected]>

Signed-off-by: Guillaume Doisy <[email protected]>

doisyg · 2024-03-13T18:02:31Z

and rebased on main

mergify · 2024-03-13T18:04:02Z

@doisyg, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

SteveMacenski · 2024-03-13T18:30:02Z

@james-ward we all happy with this now? Can you give it one last whirl around? Then we can merge!

james-ward · 2024-03-13T21:50:44Z

@james-ward we all happy with this now? Can you give it one last whirl around? Then we can merge!

I'll try it this morning. Stand by...

james-ward · 2024-03-14T05:18:47Z

Have confirmed it is good on our platform.

Send it!!

SteveMacenski

@doisyg I don't see when reject_unit_path is ever used. Can this be removed?

Otherwise LGTM and I can come through afterwards on replacing the AMCL dependency

nav2_regulated_pure_pursuit_controller/src/path_handler.cpp

* RPP Dexory tweaks Signed-off-by: Guillaume Doisy <[email protected]> * Update nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp Signed-off-by: Steve Macenski <[email protected]> * Update nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp Signed-off-by: Steve Macenski <[email protected]> * projectCarrotPastGoal test Signed-off-by: Guillaume Doisy <[email protected]> * Use circleSegmentIntersection when projecting past end of path This guarantees that the look ahead distance is maintained Signed-off-by: James Ward <[email protected]> * lint Signed-off-by: Guillaume Doisy <[email protected]> --------- Signed-off-by: Guillaume Doisy <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: James Ward <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: James Ward <[email protected]>

* RPP Dexory tweaks Signed-off-by: Guillaume Doisy <[email protected]> * Update nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp Signed-off-by: Steve Macenski <[email protected]> * Update nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp Signed-off-by: Steve Macenski <[email protected]> * projectCarrotPastGoal test Signed-off-by: Guillaume Doisy <[email protected]> * Use circleSegmentIntersection when projecting past end of path This guarantees that the look ahead distance is maintained Signed-off-by: James Ward <[email protected]> * lint Signed-off-by: Guillaume Doisy <[email protected]> --------- Signed-off-by: Guillaume Doisy <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: James Ward <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: James Ward <[email protected]> Signed-off-by: enricosutera <[email protected]>

* RPP Dexory tweaks Signed-off-by: Guillaume Doisy <[email protected]> * Update nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp Signed-off-by: Steve Macenski <[email protected]> * Update nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp Signed-off-by: Steve Macenski <[email protected]> * projectCarrotPastGoal test Signed-off-by: Guillaume Doisy <[email protected]> * Use circleSegmentIntersection when projecting past end of path This guarantees that the look ahead distance is maintained Signed-off-by: James Ward <[email protected]> * lint Signed-off-by: Guillaume Doisy <[email protected]> --------- Signed-off-by: Guillaume Doisy <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: James Ward <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: James Ward <[email protected]>

doisyg force-pushed the dex_rpp_tweaks branch from a6dce64 to cdae9a0 Compare February 23, 2024 16:22

SteveMacenski reviewed Feb 27, 2024

View reviewed changes

SteveMacenski mentioned this pull request Feb 27, 2024

[RPP] Project carrot beyond end of path if interpolating #3788

Closed

7 tasks

SteveMacenski reviewed Feb 28, 2024

View reviewed changes

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp Outdated Show resolved Hide resolved

SteveMacenski reviewed Feb 28, 2024

View reviewed changes

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp Outdated Show resolved Hide resolved

doisyg force-pushed the dex_rpp_tweaks branch from c62e584 to a3b230b Compare February 28, 2024 19:35

doisyg mentioned this pull request Feb 29, 2024

PR 4140 docs ros-navigation/docs.nav2.org#530

Merged

SteveMacenski approved these changes Feb 29, 2024

View reviewed changes

Guillaume Doisy and others added 6 commits March 13, 2024 18:02

RPP Dexory tweaks

e6eef04

Signed-off-by: Guillaume Doisy <[email protected]>

Update nav2_regulated_pure_pursuit_controller/src/regulated_pure_purs…

2aa762a

…uit_controller.cpp Signed-off-by: Steve Macenski <[email protected]>

Update nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

058397e

Signed-off-by: Steve Macenski <[email protected]>

projectCarrotPastGoal test

4ce993b

Signed-off-by: Guillaume Doisy <[email protected]>

Use circleSegmentIntersection when projecting past end of path

39882d4

This guarantees that the look ahead distance is maintained Signed-off-by: James Ward <[email protected]>

lint

994351e

Signed-off-by: Guillaume Doisy <[email protected]>

doisyg force-pushed the dex_rpp_tweaks branch from 290cf82 to 994351e Compare March 13, 2024 18:02

SteveMacenski reviewed Mar 14, 2024

View reviewed changes

nav2_regulated_pure_pursuit_controller/src/path_handler.cpp Show resolved Hide resolved

SteveMacenski merged commit db974ea into ros-navigation:main Mar 15, 2024
7 of 9 checks passed

SteveMacenski mentioned this pull request Mar 15, 2024

Remove RPP Dependency on AMCL #4185

Closed

RPP Dexory tweaks #4140

RPP Dexory tweaks #4140

Conversation

doisyg commented Feb 23, 2024 • edited Loading

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

SteveMacenski commented Feb 24, 2024

james-ward commented Feb 26, 2024

james-ward commented Feb 26, 2024

SteveMacenski commented Feb 27, 2024

SteveMacenski Feb 27, 2024

Choose a reason for hiding this comment

doisyg Feb 28, 2024

Choose a reason for hiding this comment

SteveMacenski Feb 27, 2024

Choose a reason for hiding this comment

james-ward Feb 27, 2024

Choose a reason for hiding this comment

SteveMacenski Feb 27, 2024

Choose a reason for hiding this comment

doisyg Feb 27, 2024

Choose a reason for hiding this comment

SteveMacenski Feb 28, 2024

Choose a reason for hiding this comment

doisyg commented Feb 27, 2024

james-ward commented Feb 27, 2024

SteveMacenski commented Feb 28, 2024

doisyg commented Feb 28, 2024

codecov bot commented Feb 28, 2024 • edited Loading

Codecov Report

doisyg commented Feb 29, 2024

SteveMacenski commented Feb 29, 2024

james-ward commented Feb 29, 2024

james-ward commented Mar 1, 2024

SteveMacenski commented Mar 1, 2024 • edited Loading

james-ward commented Mar 4, 2024 • edited Loading

doisyg commented Mar 4, 2024

james-ward commented Mar 4, 2024

james-ward commented Mar 7, 2024

SteveMacenski commented Mar 7, 2024 • edited Loading

james-ward commented Mar 11, 2024

SteveMacenski commented Mar 11, 2024

mergify bot commented Mar 13, 2024

doisyg commented Mar 13, 2024

doisyg commented Mar 13, 2024

mergify bot commented Mar 13, 2024

SteveMacenski commented Mar 13, 2024

james-ward commented Mar 13, 2024

james-ward commented Mar 14, 2024

SteveMacenski left a comment • edited Loading

Choose a reason for hiding this comment

doisyg commented Feb 23, 2024 •

edited

Loading

codecov bot commented Feb 28, 2024 •

edited

Loading

SteveMacenski commented Mar 1, 2024 •

edited

Loading

james-ward commented Mar 4, 2024 •

edited

Loading

SteveMacenski commented Mar 7, 2024 •

edited

Loading

SteveMacenski left a comment •

edited

Loading