graceful_controller: implement iterative selection of control points

[mikeferguson]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4115
Primary OS tested on	Ubuntu
Robotic platform tested on	UBR-1
Does this PR contain AI generated software?	No, only real intelligence was used

---

Description of contribution in a few bullet points

• Iteratively selects a valid target pose to use with the control law. This is done by forward simulating the control law towards the target pose and rejecting the target pose if there is a collision detected.
• Adds collision checking to the "final_rotation". If this fails, we will attempt to roll out further movement via the control law (this is especially important for non-circular robots!). With this in mind, the parameter final_rotation has been renamed to prefer_final_rotation.
• Modifies how final_rotation is done a bit - this is closer to how the graceful_controller did it. When prefer_final_rotation is true, we ignore the rotation of the final pose in the path and drive straight towards (and then do an in-place rotation). Previously, this code was simply using final_rotation as "do we latch and rotate", which is only half the battle if you aren't using a kinematically aware planner.

Description of documentation updates required from your changes

• Replaces motion_target_dist parameter with two new parameters: min_lookahead and max_lookahead.
• Renames final_rotation to prefer_final_rotation (to match graceful_controller and also indicate that this doesn't actually ensure there is a final rotation, just that the controller will prefer to generate one if it is collision free)
• Replace boolean initial_rotation and double initial_rotation_min_angle with initial_rotation_tolerance which is a better name for what this parameter does (and avoids needing two parameters to do one thing). This also involved a number of tests updates to ensure that initial_rotation_tolerance cannot conflict with allow_backward.
• Adds parameter v_angular_min_in_place to make sure that rotation commands actually work.
• Fixed a test that was erroneously looking at twist.angular.x

Future work that may be required in bullet points

These will be done before this converts from DRAFT:

• This still needs to be tested on robot (I'm simply opening for visibility and to see where we are in terms of code coverage)
• There is a TODO around returning cmd_vel when TF fails - this seems like the wrong thing to do, need to look at it closer - updated this to throw, similar to no command found
• Review parameter max_robot_pose_search_dist - seems to be a potentially VERY large number? This is consistent with other controllers in nav2
• Port orientation filter (at least NavfnPlanner doesn't set orientations)

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.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

[mikeferguson]

A few other things noted when porting to UBR-1:

• need to add a "min_in_place_vel_theta" equivalent - most robots can't generate arbitrarily low angular rotations
• Got my answer - yeah, not all planners output headings - 100% incompatible with the NavfnPlanner - I'm going to port over the orientation filter stuff as well then.

[codecov]

Codecov Report

Attention: Patch coverage is 95.28302% with 5 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...v2_graceful_controller/src/graceful_controller.cpp	93.24%	5 Missing ⚠️

Files with missing lines	Coverage Δ
...e/nav2_graceful_controller/graceful_controller.hpp	100.00% <ø> (ø)
...ude/nav2_graceful_controller/parameter_handler.hpp	100.00% <ø> (ø)
nav2_graceful_controller/src/parameter_handler.cpp	100.00% <100.00%> (ø)
...v2_graceful_controller/src/graceful_controller.cpp	89.69% <93.24%> (-0.82%)	⬇️

... and 5 files with indirect coverage changes

[mikeferguson]

This was the updated config for testing this on the UBR-1 (moving from the version of graceful controller in my repo): mikeferguson/ubr_reloaded@a24ad85

[mikeferguson]

Note for posterity: I did also test "allow_backward" - and it still works

[mikeferguson]

Notes on things that are still different versus my controller (that maybe we'd want to do in the future):

• In the control law - the slowdown_radius is a poor approximation for actual deceleration limits. Really, you want to cap the velocity based on knowing you can slow to a stop by the time you arrive at the pose.
• The collision checking in the iterative loop assumes you are starting at max speed (not at your current speed). This is probably not that big of a deal, but if you're going very slowly in a tight area, the controller might wobble a bit more.
• add_orientations just adds the orientations but doesn't do any more sophisticated filtering - I'm not actually convinced that additional filtering is really needed though (and, it always had a few odd edge cases - if we were to implement something here, it should really be a bit more sophisticated than what was in my controller).

[SteveMacenski]

I'll admit, this was reasonably hard for me to review with the diffs and reordering of the main code, so I'm not 100% confident in not having missed something on an initial look. I'll probably need another complete read through after changes to sanity check.

@ajtudela since this touches the docking code and I know you're thinking about these areas - want to review / test?

[SteveMacenski]

I'm curious about your thoughts from the ticket brainstorming #4115 (comment) (or is the wobbling here more or less gone with this method recomputing at the full rate?)

[mikeferguson]

I'm curious about your thoughts from the ticket brainstorming #4115 (comment) (or is the wobbling here more or less gone with this method recomputing at the full rate?)

I expect most of the wobbling will be gone since our motion_target can be so much farther away in most cases

[mikeferguson]

@ajtudela since this touches the docking code and I know you're thinking about these areas - want to review / test?

I don't think this affects the docking code at all, because that is only using the SmoothControlLaw - which is unmodified

[mikeferguson]

@SteveMacenski all review comments have been addressed - this is ready for another look when you get a chance

[ajtudela]

I'll admit, this was reasonably hard for me to review with the diffs and reordering of the main code, so I'm not 100% confident in not having missed something on an initial look. I'll probably need another complete read through after changes to sanity check.

@ajtudela since this touches the docking code and I know you're thinking about these areas - want to review / test?

Of course I'll test it on our robots on Monday morning.

This part doesn't change the controller of the docking but this is exactly the behaviour I need for the new following server. I'll have to figure out how to integrate this part into the server reusing most of the code.

[ajtudela]

I tested the improvements with the UBR-1 this morning. Although I don't really like the new order of the "steps" (rotation before movement, control, final rotation, ...) because it's harder (for me) to read, I have to admit that these changes take care of the wobbling much better than before.

Tomorrow I'll test all the combinations for the new parameters to make sure I haven't missed anything.

I recorded a little video if you want to see the changes in motion:

new_graceful.mov

[mikeferguson]

I have to admit that these changes take care of the wobbling much better than before.

What are you running for the k_phi/k_delta parameters with the controller? I found that the defaults in the controller right now are pretty aggressive on heading correction (which causes both wobbling, and divergence from the path - which appears to be also causing lots of replanning for you).

I went with lower values that were actually the defaults in https://github.com/mikeferguson/graceful_controller:

GracefulController:
      plugin: "nav2_graceful_controller::GracefulController"
      v_linear_min: 0.1
      v_linear_max: 1.0
      v_angular_max: 2.8
      v_angular_min_in_place: 0.6
      max_lookahead: 1.25
      initial_rotation: true
      initial_rotation_tolerance: 0.25
      prefer_final_rotation: true
      # The defaults aren't as good as this (more wiggling)
      k_phi: 2.0
      k_delta: 1.0
      beta: 0.4
      lambda: 2.0
      # This isn't quite comparable to using actual acceleration limits
      # (but works well in practice)
      slowdown_radius: 0.75

I didn't update these defaults - at first thinking it might impact some people's docking servers - but upon further review, it looks like that isn't the case (the docking server has it's own parameter set). @SteveMacenski thoughts? Should we update the defaults here?

[ajtudela]

Yes, they might be a bit high. I used these:

    Graceful:
      plugin: nav2_graceful_controller::GracefulController
      transform_tolerance: 0.5
      motion_target_dist: 1.2
      initial_rotation: true
      initial_rotation_min_angle: 0.75
      final_rotation: true
      allow_backward: false
      k_phi: 3.0
      k_delta: 2.0
      beta: 0.4
      lambda: 2.0
      v_linear_min: 0.1
      v_linear_max: 1.0
      v_angular_max: 5.0
      slowdown_radius: 0.5

But I agree with you. I had to lower k_phi/k_delta for the docking server. I'd try with these settings tomorrow.

[mikeferguson]

I had to lower k_phi/k_delta for the docking server

I think I might have actually been the one who chose those higher values for the docking server initially - that is an application where you want the robot to come at the charge dock as straight-on as possible, so the values make sense for docking. I think they are too high for general navigation controlling (but it wasn't something I caught back when we originally added the controller here)

[ajtudela]

I made a new video this morning. Lowered k_phi/k_delta reduces the wobble more.

new_graceful_02.mov

[mikeferguson]

@SteveMacenski bumping this - wasn't sure if you maybe missed my comment that this was ready for another review

[SteveMacenski]

I found that the defaults in the controller right now are pretty aggressive on heading correction (which causes both wobbling, and divergence from the path - which appears to be also causing lots of replanning for you).

but upon further review, it looks like that isn't the case (the docking server has it's own parameter set). @SteveMacenski thoughts? Should we update the defaults here?

I think I might have actually been the one who chose those higher values for the docking server initially - that is an application where you want the robot to come at the charge dock as straight-on as possible, so the values make sense for docking. I think they are too high for general navigation controlling (but it wasn't something I caught back when we originally added the controller here)

We can change those, definitely! I want all defaults to have a really nice out of the box experience to showcase algorithm's values without the need of heavy tuning for 'most' applications.

[SteveMacenski]

@ajtudela any thoughts here? This is your work too, so I want to make sure you have a chance to review and give thoughts!

[ajtudela]

I tested heavily this week and it works pretty well, even in narrow corridors.

I think there's missing coverage in L212-216 (or I'm ready Codecov wrong).

Otherwise, LGTM.

[mikeferguson]

@SteveMacenski what else do we need to get this merged?

[SteveMacenski]

@mikeferguson a couple of small updates in the docs still needed, but code is good -- thanks for the revisions and help! ros-navigation/docs.nav2.org#624