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

james-ward · 2023-08-31T07:22:31Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	Closes #3784
Primary OS tested on	Ubuntu
Robotic platform tested on	Ackermann steered EV

Description of contribution in a few bullet points

Projects the carrot past the end of the path
- Only when interpolation mode is on (users that want actual path poses will still get them)
- Prevents the effective gain of the steering system being increased when approaching the goal
- Uses vector between last two path poses for direction - ignores orientation of last path pose

Description of documentation updates required from your changes

None

Future work that may be required in bullet points

None

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

codecov · 2023-08-31T08:07:04Z

Codecov Report

Attention: 4 lines in your changes are missing coverage. Please review.

Comparison is base (913f7b6) 90.10% compared to head (bc8a89e) 90.20%.

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

Files	Patch %	Lines
...ntroller/src/regulated_pure_pursuit_controller.cpp	95.78%	4 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3788      +/-   ##
==========================================
+ Coverage   90.10%   90.20%   +0.09%     
==========================================
  Files         431      417      -14     
  Lines       19393    18614     -779     
==========================================
- Hits        17475    16790     -685     
+ Misses       1918     1824      -94

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

SteveMacenski · 2023-08-31T15:16:38Z

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -322,23 +322,42 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin
  // If the no pose is not far enough, take the last pose
  if (goal_pose_it == transformed_plan.poses.end()) {
    goal_pose_it = std::prev(transformed_plan.poses.end());
-  } else if (params_->use_interpolation && goal_pose_it != transformed_plan.poses.begin()) {
+  }
+  if (params_->use_interpolation && goal_pose_it != transformed_plan.poses.begin()) {


Interpolation is regarding interpolating between path points for sparse paths / large vehicles for selecting the lookahead points at steady state.

I think this approach to goal stuff is something separate. Mostly because people might want this feature you're adding, but not care about path tracking interpolation

Followup: I'd like to understand the behavior that this interpolation after the goal creates. If there's no reason that this isn't just always enabled, let me know and we can just make this the new default behavior without a parameterized option

My reasoning is that if someone isn't turning on the interpolation then they (presumably) care about only having carrots that are poses from the path. Projecting off the end of the path would violate that as we are creating a new pose to track.

WRT the default behaviour - yes, I believe it should always be enabled. At the moment if you are asking for interpolated carrots, you are in effect asking for the planner to always produce a carrot that is exactly the lookahead distance from the robot. At the moment when approaching the goal this is not the case - the carrot gets closer and closer.
For an Ackermann steered vehicle where the base_link is on the rear axle as you approach the goal the carrot ends up behind the front steering wheels. This makes it very unstable, and small cross-track errors lead to big deviations in yaw. With very little path left this very often ends up with the robot reaching the goal with very poor alignment (on our platform anyway!).

I have seen the other work done here on slowing down on approach to goal, and that is something that we are interested in because we also have a large platform with limits on how quickly it can speed up and slow down. But this PR isn't meant to address any of those things - just tracking of the path as the robot approaches the goal. All the calcs about when to start slowing down are done with reference to the last pose in the path, so this PR does not change that.

The interpolation is mostly for smooth motion purposes so that between iterations that the velocity curves are more continuous -- which is important for larger vehicles that might have pretty sparse paths (this was created by an ag-tech company). I don't think the rationale is really about being on the exact path with exact path points.

WRT the default behaviour - yes, I believe it should always be enabled ... I have seen the other work done here on slowing down on approach to goal, But this PR isn't meant to address any of those things

Ah, do you think we could? For instance

Do this forward projection to the lookahead distance

But reduce the velocity in proportion to the distance remaining to the actual goal pose

If we had something like that, we could simply make it default then without a parameterization. In fact, isn't that the behavior with this enabled + approachVelocityConstraint (which is non-optional)? I believe the slowdown logic is already decoupled the lookahead distance

The interpolation is mostly for smooth motion purposes so that between iterations that the velocity curves are more continuous -- which is important for larger vehicles that might have pretty sparse paths (this was created by an ag-tech company). I don't think the rationale is really about being on the exact path with exact path points.

I was just guessing - I would never want to run without the interpolation on for exactly the reasons you state! ;) Even if the path wasn't sparse.

If we had something like that, we could simply make it default then without a parameterization. In fact, isn't that the behavior with this enabled + approachVelocityConstraint (which is non-optional)? I believe the slowdown logic is already decoupled the lookahead distance

Yes, the slowdown is already decoupled - it uses the current pose and last pose in the path as parameters. The carrot is not involved in those calculations.

Honestly, this might be a kick for me to disable the option for interpolation and just have it happen. That was a new feature so I didn't want to break users, but its had enough baking time now I think it can just be always included. But something to do after this PR is figured out!

SteveMacenski · 2023-08-31T15:18:56Z

@doisyg please review :-)

SteveMacenski · 2023-09-01T16:45:13Z

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

+      // Projecting past the last point by the lookahead distance ensures that the point
+      // is at least the lookahead distance away
+      // (maybe more, but we will interpolate in the next step)
+      goal_position.x += dx / d * lookahead_dist;


I... don't think this is correct

If the total distance between points is 1 unit and equal component X and Y, then dx and dy would be 0.7071 for a total d of 1.0. In that case, we're adding more to the goal pose than would be the length of the lookahead_distance.

I think you need to do some trig with the angles to see what component of each you need to apply like the commit I originally linked to did, unless I'm missing something.

Yes, that is deliberate because the direction is the important bit. The distance is truncated in the following code by the call:
https://github.com/ros-planning/navigation2/blob/681d7e15acbb27618272e0530d7b8f385d614fad/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp#L353

I think you need to do some trig with the angles to see what component of each you need to apply like the commit I originally linked to did, unless I'm missing something.

This is the same as in the linked commit https://github.com/ros-planning/navigation2/blob/a9d8c4c9889c7d8684296c5d7733f507255520c1/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp#L232-L239 just without using trig functions to convert to an intermediate angle.

The distance is set to the lookahead distance so that it is outside the radius and circleSegmentationIntersection() succeeds.

The difference is approach is that in the previous work in a9d8c4c the remaining lookahead distance is calculated by taking the "path end as carrot" distance, and working out how much shorter than the lookahead distance it is. This distance is then added to the last segment of the path and treated as the new carrot. (There is an implicit assumption that the direction from the robot to the last path pose is the same as the direction of the last segment, but I suspect that in practice this would make very little difference/be very unlikely to cause problems as it would need odd dog-leg paths at the scale of the lookahead distance for it to matter).
My approach checks to see if the last pose is being selected before the call to find the circle-segment intersection (which runs for all interpolations along the path), and shifts the goal pose before this call. So every iteration uses the circle-segment intersection approach, even when it is on the last segment.

👍 @doisyg thoughts? I suppose this is worth testing on your platform to let me know if this alternative works for you

Trying to find a slot to test on a real big robot. Give me 24h

(There is an implicit assumption that the direction from the robot to the last path pose is the same as the direction of the last segment, but I suspect that in practice this would make very little difference/be very unlikely to cause problems as it would need odd dog-leg paths at the scale of the lookahead distance for it to matter).

Indeed, I developed and tested a9d8c4c for straight paths only. I agree this should be mostly fine for general paths but I did not make the exercise of thinking and testing about corner cases.

james-ward · 2023-09-14T13:38:44Z

Hi @SteveMacenski and @doisyg
I have been running this patch on our platform a lot in the last few days and it does make things a lot better. However I have found a problem - when the path is pruned so that there is only one pose left this doesn't fire, and the carrot gets snapped back to the last pose right at the end. I only noticed this recently with paths where the second last point was a decent distance away from the goal.

The method proposed by @doisyg avoids this problem but it does so by just keeping the robot moving in a straight line at the end - ie it stops trying to align itself with the path direction at the end.

We could adopt a hybrid approach - project the last segment until there is no segment to project and at that point project the robot->goal direction.
I don't think we can assume that the orientation of the pose has always been set by the planner, otherwise we could just use the goal orientation to project from the goal pose itself. I believe that we consider a path consisting of (x,y) coordinates without directions to be a valid input to the controllers?
Thoughts?

There is also work that needs to be done to project the carrot at cusps in reversing paths - this will be trickier to solve cleanly.

SteveMacenski · 2023-09-14T18:47:20Z

ie it stops trying to align itself with the path direction at the end

Is that necessarily problematic? If you're within a short distance of the path, you're probably a straight-line estimate from the goal anyway. The only time I would think that would be a problem is if you have a very, very high curvature path towards the end of the task. But in that case, we rotate to goal heading at the end so that could just trigger https://github.com/ros-planning/navigation2/blob/main/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp#L214. I suppose it could be problamatic if its really confined and thus near obstacles, but I wouldn't think the tiny semantic change like that in path following would be the deciding factor.

Is that a real problem you're aware of?

We could adopt a hybrid approach - project the last segment until there is no segment to project and at that point project the robot->goal direction.

I suppose that's also an option. Seems like it might be overkill, but that would be fine with me as long as all of this was definitely broken out into another function. Then that should be good to go. If the path is only 1 point, then a straight line is all we can do anyway so that makes the above remarks moot.

I don't think we can assume that the orientation of the pose has always been set by the planner, otherwise we could just use the goal orientation to project from the goal pose itself. I believe that we consider a path consisting of (x,y) coordinates without directions to be a valid input to the controllers?

All the planners generate an orientation at the end of the path. That's how the controller knows to rotate / achieve the right yaw portion of the pose. But only the feasible Smac Planners generate orientations at all subsequent poses in the path (except if any of the path smoothers are used with any planner; then they populate orientations as part of their work).

Just a reminder to split this from the use_interpolation parameter, these are different behaviors and I've gotten the same remark from another reviewer that they want to see that as well

doisyg · 2023-09-14T20:01:03Z

Just a reminder to split this from the use_interpolation parameter, these are different behaviors and I've gotten the same remark from another reviewer that they want to see that as well

Agreed, one could want to interpolate on the path without interpolating the lookahead point after the goal. And if interpolating on the path becomes the default and the parameter disappears, we still want to be able to control if we want to interpolate the lookahead point or not.

SteveMacenski · 2023-10-03T17:03:19Z

@james-ward any update?

doisyg · 2023-10-03T17:10:44Z

I re did it from scratch starting from main branch in botsandus@4d570f8
Currently testing (so far so good after one week). I will clean and PR soon with a couple of points to discuss.

james-ward · 2023-10-11T23:39:28Z

@james-ward any update?

Sorry, did not get any notifications of new comments on this thread. Odd...

The approach by @doisyg above is basically what I have done on a different branch - I chose to start again. https://github.com/james-ward/navigation2/tree/preserve-last-segment
The approach in the branch pointed to by this current PR is not good. The key, as in @doisyg's commit above, is that if the path is pruned by the controller to only have one pose left then you cannot do the projection properly. Projecting from the robot through the one remaining pose won't work because when the robot is close and has a small crosstrack error it will project the point a long way to the side of the robot, and cause large steering outputs.
I did it by injecting an additional pose into the path upon receipt an infinitesimally small distance from the endpoint, but perhaps @doisyg's method is better in just preventing the controller from pruning if there are only two points left.

In all of these approaches there is still an issue with projecting past cusps in paths with reversals. I cannot see a nice way to do that at the present time.

james-ward · 2023-10-11T23:41:37Z

The approach by @doisyg above is basically what I have done on a different branch - I chose to start again. https://github.com/james-ward/navigation2/tree/preserve-last-segment

For reference we have been using that branch on our platform for the last three weeks and it has been a great improvement.

SteveMacenski · 2023-10-12T00:24:23Z

@james-ward do you think we should use your new branch or @doisyg's for merging a fix into Nav2? Just trying to make sure we can rectify these two similar directions to get something that satisfies everyone!

james-ward · 2023-10-12T00:28:15Z

I'm happy with @doisyg's approach. I didn't do it that way because I was unsure if there might be problems if the controller has poses behind the robot after it has finished pruning.

I was also going to update the documentation and add a warning if the minimum lookahead distance is non-zero and the new projection flag isn't turned on.

If you are happy for me to make the history in this PR invalid I can force push my new branch to replace the one referenced in this PR which will make looking at my patch set easier.

SteveMacenski · 2023-10-12T00:39:35Z

Yes please!

@doisyg do you agree we should use his as well? [rectification in progress. . . . 90%]

SteveMacenski · 2023-10-12T16:58:31Z

I'll let @doisyg give the first review analysis when he has some time

mergify · 2023-11-08T04:32:17Z

This pull request is in conflict. Could you fix it @james-ward?

mergify · 2023-11-16T18:23:26Z

This pull request is in conflict. Could you fix it @james-ward?

james-ward · 2023-11-28T02:32:02Z

Hi @doisyg @SteveMacenski
What can I do to help get this progressed? I've had to rebase twice due to other PRs getting merged - the longer it goes on the more likely I have to do it again. ;)
FWIW we are still running this patch on our platforms and gives much better behaviour at the end of the path (ie no heaving the steering to one side or the other).

SteveMacenski · 2023-11-28T17:31:59Z

I think its on @doisyg list for this week or next?

Signed-off-by: James Ward <[email protected]>

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

Signed-off-by: James Ward <[email protected]>

james-ward · 2024-02-21T23:11:05Z

@RhysMcK Thanks for looking at this. I had to rebase a couple of times due to upstream changes, and some formatting changes came through. I have added a commit to remove them, so the only changes in this PR are now the algorithmic ones. Much easier to review!

SteveMacenski · 2024-02-27T01:22:47Z

#4140 supersedes. Thanks @james-ward for your help and prototype here!

SteveMacenski reviewed Aug 31, 2023

View reviewed changes

SteveMacenski reviewed Sep 1, 2023

View reviewed changes

james-ward force-pushed the rpp-project-carrot branch from 681d7e1 to 24fd460 Compare October 12, 2023 00:42

SteveMacenski requested a review from doisyg October 12, 2023 16:58

james-ward force-pushed the rpp-project-carrot branch from 24fd460 to 5dcb7b4 Compare November 12, 2023 22:47

james-ward force-pushed the rpp-project-carrot branch from 5dcb7b4 to 9de1624 Compare November 25, 2023 01:39

james-ward added 4 commits February 14, 2024 14:56

Add parameter for projecting carrot beyond end of path

2abec7e

Signed-off-by: James Ward <[email protected]>

Add helper functions to allow for carrot projection off end of path

e1325f8

Signed-off-by: James Ward <[email protected]>

Enable projection of carrot off end of path

9ac6748

Signed-off-by: James Ward <[email protected]>

Add test for projecting carrot on single pose path

e9dd766

Signed-off-by: James Ward <[email protected]>

james-ward force-pushed the rpp-project-carrot branch from bc8a89e to e9dd766 Compare February 14, 2024 03:56

RhysMcK reviewed Feb 19, 2024

View reviewed changes

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

Fix incorrect argument to find circle segment intersection

15d4446

Signed-off-by: James Ward <[email protected]>

Remove formatting changes from conflict with upstream rebase

4296c84

Signed-off-by: James Ward <[email protected]>

doisyg mentioned this pull request Feb 23, 2024

RPP Dexory tweaks #4140

Merged

7 tasks

SteveMacenski closed this Feb 27, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

james-ward commented Aug 31, 2023

codecov bot commented Aug 31, 2023 •

edited

Loading

SteveMacenski Aug 31, 2023

SteveMacenski Aug 31, 2023

james-ward Aug 31, 2023

SteveMacenski Sep 1, 2023

james-ward Sep 3, 2023

SteveMacenski Sep 5, 2023

SteveMacenski commented Aug 31, 2023

SteveMacenski Sep 1, 2023 •

edited

Loading

james-ward Sep 3, 2023

SteveMacenski Sep 5, 2023

doisyg Sep 6, 2023

james-ward commented Sep 14, 2023

SteveMacenski commented Sep 14, 2023 •

edited

Loading

doisyg commented Sep 14, 2023

SteveMacenski commented Oct 3, 2023

doisyg commented Oct 3, 2023 •

edited

Loading

james-ward commented Oct 11, 2023

james-ward commented Oct 11, 2023

SteveMacenski commented Oct 12, 2023

james-ward commented Oct 12, 2023

SteveMacenski commented Oct 12, 2023

SteveMacenski commented Oct 12, 2023

mergify bot commented Nov 8, 2023

mergify bot commented Nov 16, 2023

james-ward commented Nov 28, 2023

SteveMacenski commented Nov 28, 2023

james-ward commented Feb 21, 2024

SteveMacenski commented Feb 27, 2024

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

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

Conversation

james-ward commented Aug 31, 2023

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:

codecov bot commented Aug 31, 2023 • edited Loading

Codecov Report

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

SteveMacenski commented Aug 31, 2023

SteveMacenski Sep 1, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

james-ward commented Sep 14, 2023

SteveMacenski commented Sep 14, 2023 • edited Loading

doisyg commented Sep 14, 2023

SteveMacenski commented Oct 3, 2023

doisyg commented Oct 3, 2023 • edited Loading

james-ward commented Oct 11, 2023

james-ward commented Oct 11, 2023

SteveMacenski commented Oct 12, 2023

james-ward commented Oct 12, 2023

SteveMacenski commented Oct 12, 2023

SteveMacenski commented Oct 12, 2023

mergify bot commented Nov 8, 2023

mergify bot commented Nov 16, 2023

james-ward commented Nov 28, 2023

SteveMacenski commented Nov 28, 2023

james-ward commented Feb 21, 2024

SteveMacenski commented Feb 27, 2024

codecov bot commented Aug 31, 2023 •

edited

Loading

SteveMacenski Sep 1, 2023 •

edited

Loading

SteveMacenski commented Sep 14, 2023 •

edited

Loading

doisyg commented Oct 3, 2023 •

edited

Loading