Add API to gracefully cancel a controller

[Rayman]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4033
Primary OS tested on	Ubuntu
Robotic platform tested on	Gazebo simulation of inhouse robot
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

Ability to let controllers handle cancelations gracefully. See the discussion at #4033.

I'm opening this PR for a first review of the API. I've added gracefull cancellation to one of our in-house controllers and tested that this API is sufficient. I hope the API fits the design of the core APIs.

Description of documentation updates required from your changes

---

Future work that may be required in bullet points

• Add gracefull cancellation to one of the existing controllers.
• Merge Document gracefull cancellation API docs.nav2.org#535

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]

Codecov Report

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

Project coverage is 89.86%. Comparing base (80bb5bf) to head (1d22c51).

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

Files	Patch %	Lines
nav2_controller/src/controller_server.cpp	66.66%	2 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #4136      +/-   ##
==========================================
+ Coverage   89.49%   89.86%   +0.37%     
==========================================
  Files         431      432       +1     
  Lines       19555    19411     -144     
==========================================
- Hits        17500    17443      -57     
+ Misses       2055     1968      -87     

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

[SteveMacenski]

I think having the cancel state projected back to the computeVelocityCommands as an argument would be important. Right now, I don't know how the controller would know when cancel stopped and a new request came in otherwise. So I think cancelling should be given to it.

Or possibly even have separate computeCancelCommands() API to use when cancelling = true. That may be a more ergonomics thing: I imagine that when you have a controller that is handling normal vs cancel state, you're basically having an if/else for 2 very different methods either way. Those would probably render into separate functions, so we could just call separate functions. We could have the if/else in computeAndPublishVelocity with new arg cancelling

Then, the if (controllers_[current_controller_]->cancel()) { check woudl be more like cancelled() to indicate that its completed its cancellation to allow us to action_server_->terminate_all(); and publishZeroVelocity(); for good measure (and where the timeout is located)

So all in all, I like your design, but I think we an do it a little bit better with addresses the problem of the controller knowing when we're back outside of the non-canceled state for normal operations & adding a timeout in case a plugin isn't properly implemented

[Rayman]

@SteveMacenski which controller is the easiest to modify with this new API? I was thinking about the pure pursuit controller but that one doesn't have decelleration limits so I don't know how fast to brake. How is the acceleration determined?

[SteveMacenski]

Which ever one you like - I think DWB is the only one with explicit decel/accel parameters right now. MPPI will have it before EOY, but not its current state. RPP is a pure geometric approach, so there's not really any dynamic modeling (right, its just following a carrot). We have some constraints on speed based on some heuristic functions, but that's also not dynamics based, its situational.

[mergify]

@Rayman, 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]

@Rayman anything I need to review? Or still working on it 😄

[mergify]

@Rayman, 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).

[Rayman]

Yes sorry I'm still working on it.

I've modified the RPP controller to add a cancel_decelleration parameter that shows how to use the new API. Is this in line with the design of the controller? I thought that one was the easiest to modify. Most controllers don't have acceleration limits, only the DWB. But that one is quite complicated with the critics so I didn't know how to modify it to support cancellation.

I've also adressed one of your review comments. Last thing to do is add some unit tests probably.

[mergify]

@Rayman, 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]

Generally looks good to me!

[mergify]

@Rayman, 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).

[Rayman]

I don't understand the CI failure. I didn't edit any package.xml so why is it complaining about dependency issues?

[SteveMacenski]

@Rayman we're in a weird state due to the rolling transition to 24.04. I'll plan to look into it in more detail on Friday, but for now, if you tell me it works and you tested it, I'd trust that assuming the other checks look good

[mergify]

@Rayman, 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).

[Rayman]

I've tested this only in simulation for now. I'll test this on a robot in the next coming weeks but I need some more parts to be ready for that. I'll send this PR also to a collegue to review

[Timple]

Looks good to me!

This now works in simulation for the pure pursuit and our own controller. So for me that's good enough 🙂. I don't think the physical interfaces of the robot will influence this behavior. But I'll leave that up to @SteveMacenski

[Timple]

Nitpick: this is an implementation detail. "Waiting for the controller to finish cancellation"

[Timple]

On the other hand, publishZeroVelocity(); enforces a stop, so this is the only option 😄. You can leave it.

[SteveMacenski]

Sure, @Rayman can you make that string update? Then I can merge

[Rayman]

Done

[SteveMacenski]

Sim is good enough for me - yal know what you're doing. Feel free to implement other controllers if you like (or contribute your controller here) 😉

[SteveMacenski]

Thanks @Rayman !

[doisyg]

Nice work, I haven't tested it but reading on the changes, 2 questions:

1. In the way it is implemented in RPP, I believe it is now a mandatory behavior to use the cancel_deceleration. Any way to making it optional, for instance when one wants to rely on the deceleration configured and applied in a velocity smoother node (and without having to manage another decel limit parameter somewhere else?
2. Is it applied when preempting the controller server action ? (in my current use-case I would not want that, but I can think of use cases where it would be desirable)

[SteveMacenski]

I believe it is now a mandatory behavior to use the cancel_deceleration

You could set the deceleration to something insanely high to in effect disable, but this is a good point, it should be an option. We can make a param pretty easily that'll bypass this if needed. I filed a ticket #4238 for record keeping

Is it applied when preempting the controller server action

Nope, only on cancel:

      if (action_server_->is_cancel_requested()) {
        if (controllers_[current_controller_]->cancel()) {
          ...
        } ...
      }

[doisyg]

All good short term then!