Feature request: Graceful cancellation of `follow_path` actions

[Rayman]

Feature request: Graceful cancellation of `follow_path` actions

[SteveMacenski]

Have you tried using the nav2_velocity_smoother? It is designed to take potentially infeasible commands from nav2_controller and output feasible commands given a set of acceleration or velocity constraints. Think of it as a post-processing stage in case something is misconfigured or the technique you use in the controller doesn't strictly promise feasible continuity.

That should square you away :-) Its also enabled by default since Iron and newer

If that's not workable, I'm sure we could expose the controller plugin another option for what to do on cancellation for reducing speed. But, I'll admit that seems overkill unless there's more you want to do than just decelerate by dynamic limits.

[Timple]

The idea is to decelerate by dynamic limits whilst tracking the path.

When following a turn, it would be undesired to slow down in a straight line upon a cancel action, which I think/guess is what the nav2_velocity_smoother does?

[SteveMacenski]

I also don't think that's actually a generically safe solution? Deceleration in turns could cause the kinds of "throwing loads off my robot top" that the instantaneous braking could due to centrifugal forces. I imagine as well there are techniques where reducing the velocity scale will actually make it deviate off the path due to its internal modeling (thinking MPC/MPPI which are maintaining some state)

This isn't my opposition to your request - just throwing out some additional constraints perhaps that weren't considered giving me pause

[SteveMacenski]

When following a turn, it would be undesired to slow down in a straight line upon a cancel action, which I think/guess is what the nav2_velocity_smoother does?

The Vel Smoother will ramp down from the last request to the zero velocity. It won't necessarily be a straight line. If there's some non-zero angular component, that will be ramped down too by its own deceleration constraints. The eta scaling mechanics I wrote will also make sure the velocities are proportionally scaled to the last command's vector when that command has infeasible request (e.g. stopping from full speed), so it won't deviate from the last command's outlined path it would have taken if that last command continued forward.

The eta scaling was written so that invalid acceleration profile jumps wouldn't create different than intended motion vectors (which previous ROS 1 velocity smoothers did). I don't think its the role of the velocity smoother to make the robot change direction, even if ever so slightly - its role is to scale the intent down to what's possible. It wasn't written with the intention of this stoppage case in mind, but I think that is also beneficial here.

So if moving straight, it'll be a straight line. If turning, it'll continue with the turning maneuver's velocity from the last command sent. It is obviously not predictive for different path-tracking behavior beyond that last non-zero command, but perhaps that's sufficient? I'd say so for many applications, but I don't know what your stopping distances are with your set deceleration constraints. If you're stopping distance is 5m in dense shelving, then yeah, probably not 😆