Feature request: Graceful cancellation of follow_path actions

[Rayman]

Feature request

Ability to let controllers handle cancelations gracefully

Feature description

At the moment when the follow_path action is cancelled, a zero velocity is immediately published (see the code and the figure below).
We have a big heavy robot that needs a lot of time to slow down so we want to keep following the current path while decelerating within the deceleration limits.

With move base flex, we could override the cancel method and return when the cancellation was performed (see
magazino/move_base_flex#171)
Is there a way to achieve this graceful cancel or can we add the same API to Navigation2?

[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 😆

[Rayman]

When the robot is paused during operation (no emergency stop) it'll slow down with these parameters:

target_x_acc: 0.25 m/s^2
target_x_vel: 3.8333 m/2. 

That means it takes ~15 seconds to fully slow down. The braking distance is (if I remember correctly)

s = v^2/2/a = 29.4 m

Driving 30 meters without following the path will not work for us. Image being on a straight line just before a turn, the robot would continue driving straight and drive 30 meters off the path.

I think our requirements are a bit different than a lot of other users, but it would be nice if we could stick to a common standard.

Does that command velocity smoother have access to the path? That would solve the problem. Otherwise the controller could signal when a cancel was handled so we can have ~15 seconds of extra control time.

[SteveMacenski]

Ah ok. I understand the need then better. The velocity smoother doesn't have access to the path, but I suppose it could be made to have it. The main question I'd have there is how to follow the path in that cancel ramp down?

I think a path forward here would be for you to propose a design to enable this for us to discuss. Once we agree, I'm also agreeing to merge that in once you put up a PR.

A gut instinct would be to have a new Controller plugin API for handling cancellations. I'm not 100% sure what that would entail internally off hand, but this is clearly something you've thought about so I'll leave that to your experience :-). The Controller Server can call that cancel API when a cancel request is formed. Or, I suppose it could flip a state and call the cancel API instead of the compute API in the while loop so we still perform all the other server's work without duplication. Just adds an if/else statement and the rest is the same.

The only request I'd have beyond what you'd naturally do anyway would be to implement such a cancellation ramp down for +1 of the existing controllers so that there is an example of it to build from with the others. If you're already going to modify one of them for your needs that you use currently, I'd ask that you do a second just to help what otherwise I'll have to come back and do (or if you want to be very generous, do them all, which I would appreciate :-) )

[Rayman]

Thank you for your answer. I'll come back with a proposal

[SteveMacenski]

@Rayman any update? :-)

[Rayman]

I'm trying to figure out which API we could use. I have nav2 building from souce and I'm trying to modify the pure pursuit planner to handle cancellation. But I'm bumping against some issues with the simulator (and another project which wants my attention)

At the moment I have

// Signal the controller that a cancellation is requested. By default returns true to indicate an immediate cancellation is OK.
bool cancel()

If you return false you get an extended gracefull cancel. computeVelocityCommand should raise an CancellationCompleted exception when the cancellation has completed successfully.

But I'm not really happy with raising an expection to indicate a successful cancellation, so I'm thinking some other methods.

[SteveMacenski]

Maybe setting something in the is_cancel_requested conditional? I think there are 2 places that jump out to me:

• The is_cancel_requested conditional, add a new cancellation section before we terminate_all() like computeAndPublishCancelVelocity.
• Or setting some state so that we continue in the main loop, to compute cancel controls (which by default we can set in the controllers to just send 0). That has the benefit of keeping the current control rate object, updating global paths, getting robot pose to send, and such all the same. If we rearrange the main while loop to have the costmap current, update global path checks before this condition, we could actually keep the same loop if we continue or set an if/else state for cancel vs control as to which branch to execute before the shared sleep (probably if/else orswitch instead of continue will make the shared rate sleep easier to handle).

The second seems like the most reasonable to share as much as possible and keep the cancel control have the same loop rate and behavior as main control and have just some computeVelocityCommands for cancelation: computeCancelCommands. That API should return a state if its "done" so we can use that to break out of the cancel state and finally terminate and return like in the current code setup. It involves a bit more updates in the main control server callback, but should be straight forward to make pretty easy to follow (for now; with only 2 states)