RPP approach velocity scaling should not use lookahead distance as a metric

[Aposhian]

Feature request

Feature description

Currently, RPP decides when to trigger approach velocity scaling based on if the actual lookahead distance is different from the requested lookahead distance by a certain margin. This is problematic because lookahead distance is determined from current velocity, so there is a cyclic dependency that can lead to oscillations at the stop point.

graph TD
    velocity --> lookahead_distance --> velocity_scaling --> cmd_vel --> velocity

Loading

Implementation considerations

Rather than looking to lookahead distance as a proxy measure, I wonder if there is some way to pipe the distance to the goal from a goal checker? Because simple euclidean distance to the goal pose may not be compatible with all goal checkers.

Velocity smoothing and motion profiling has been suggested as a fix to this issue, but I think that we should be able to come up with a simple solution for RPP that fixes this underlying cyclic dependency in the algorithm.

[SteveMacenski]

From #2971

The first place I'd go is to say that the distance error is the wrong metric to use because of this case. Instead, we could have it be a radius to goal metric with the same other logic. If the last point on the path is within lookahead_dist radius, apply the same logic with the same 'error' calculation. That would be a similar proxy, though inexact.

Perhaps that is the answer? Just a new parameter for proximity to apply it and have the velocity scaling be a function of radial distance from goal rather than lookahead error? I believe this would also solve your other ticket #2971

I wonder if there is some way to pipe the distance to the goal from a goal checker?

The thing about the goal checker is that is to check if its within tolerances to stop navigation entirely. We would need to be scaling velocities to slow down before we got into that tolerance range from what a goal checker would have to say - else we (1) just stop abruptly negating the feature or (2) overshoot the goal because we only then start to slow down. So there would have to be additional fields about applying goal-centric behaviors. I'm not opposed to that in concept, but my first comment above seems the path of least design delay.

[Aposhian]

If the last point on the path is within lookahead_dist radius, apply the same logic with the same 'error' calculation.

The only problem is that this may result in approach velocity scaling when you are not actually at the end of the path. In my application, I have custom goal checker that allows me to pass by the goal without slowing down, but then slowing down at the actual end of the path.

[Aposhian]

So the goal checker's responsibility is to stop the navigation based on a desired criteria. The RPP controller's job is to sensibly follow the path given it, which should probably include slowing to a stop at the end. So maybe you are right that the goal checker should remain separate, but I would still like a solution that still works for my use case, and radius won't work. 😄 What if it looked at the integrated path distance remaining on the transformed plan as the criteria for when to apply approach velocity scaling? That would make it not have to indirectly reference lookahead distance like the current approach does.

[Aposhian]

I made a quick draft of what this could look like, although it is not tested yet (it may not even compile yet). Just something to start the discussion.

[SteveMacenski]

What if it looked at the integrated path distance remaining on the transformed plan as the criteria for when to apply approach velocity scaling? That would make it not have to indirectly reference lookahead distance like the current approach does.

Agreed, and solve the other ticket too. I see the open PR, I'll take a look at it tomorrow during my daily reviewing/triaging block of time. What you suggest seems entirely reasonable though so if your implementation looks like that idea, I'd approve of it after some testing just to sanity check that it solves your oscillation problem -- which I fully expect it would. Just would be good to see the graph analog you posted above.

[SteveMacenski]

Merging imminent