Add acceleration limits to DriveOnHeading and BackUp behaviors #4810

RBT22 · 2024-12-20T11:19:13Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	NA
Primary OS tested on	Ubuntu
Robotic platform tested on	own robot hardware
Does this PR contain AI generated software?	No

Description of contribution in a few bullet points

Added two params: acceleration_limit and deceleration_limit for DriveOnHeading and BackUp behaviors.

Description of documentation updates required from your changes

Added new parameter, so need to add that to default configs and documentation page

Future work that may be required in bullet points

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

mergify · 2024-12-20T11:22:40Z

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

mergify · 2024-12-20T11:28:55Z

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

mergify · 2024-12-20T11:33:11Z

@RBT22, 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

Build is failing - not 100% sure why off hand other than it relates to a method added

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

mergify · 2025-01-02T14:44:06Z

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

mergify · 2025-01-02T14:52:39Z

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

mergify · 2025-01-02T14:57:58Z

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

mergify · 2025-01-02T15:14:45Z

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

mergify · 2025-01-02T15:25:38Z

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

codecov · 2025-01-02T16:15:05Z

Codecov Report

Attention: Patch coverage is 52.00000% with 12 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...nclude/nav2_behaviors/plugins/drive_on_heading.hpp	52.00%	12 Missing ⚠️

Files with missing lines	Coverage Δ
...nclude/nav2_behaviors/plugins/drive_on_heading.hpp	`82.65% <52.00%> (-10.60%)`	⬇️

SteveMacenski · 2025-01-07T19:49:37Z

Sorry for the delayed review - see comment above. It also looks like this block doesn't have any test coverage. I think you should make a new backup or drive on heading unit or system test which specifies acceleration limits to exercise.

This is not ABI/API breaking, so once approved, you could open a PR to backport to Jazzy/Humble if you wanted either (Jazzy I may be able to do automatically, Humble I definitely can't / wouldn't unless someone else opened it)

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

SteveMacenski · 2025-01-07T19:52:53Z

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

+      if (command_speed_ > 0.0) {
+        double max_vel_to_stop = std::sqrt(2.0 * deceleration_limit_ * remaining_distance);
+        if (max_vel_to_stop < cmd_vel->twist.linear.x) {
+          cmd_vel->twist.linear.x = max_vel_to_stop;


I think you may also need to set a minimum velocity parameter. Most robots can't go 1mm/s or have stalling velocities.

Speaking of this -- an aside. Do you use the rotation shim controller? Right now, we don't do the deceleration as a function of its target orientation and this would be great to add in there too if you happened to also use it & be open to submitting the PR

@SteveMacenski I can add the limits for the shim controller after this, but I’m kinda stuck on the tests—can’t get the subscriber to run its callback. Any tips?

It generally looks right ot me, but I'd look at other subscription-based BT node tests for reference :-)

I'd also try:

do rclcpp::spin_some(node_); after the while loop

Print in the callbacks, are you sure its not actually being triggered

Check the topic is correct

Signed-off-by: RBT22 <[email protected]>

mergify · 2025-01-09T10:03:24Z

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

Signed-off-by: RBT22 <[email protected]>

SteveMacenski · 2025-01-11T01:39:07Z

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

+      rclcpp::ParameterValue(-2.5));
+    nav2_util::declare_parameter_if_not_declared(
+      node, this->behavior_name_ + ".minimum_speed",
+      rclcpp::ParameterValue(0.01));


Suggested change

rclcpp::ParameterValue(0.01));

rclcpp::ParameterValue(0.10));

More realistic

Also for the nav2_params.yaml

SteveMacenski

Also, configuration guide entries are needed and migration guide to mention the new params

SteveMacenski · 2025-01-11T01:39:59Z

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

+      rclcpp::ParameterValue(-2.5));
+    nav2_util::declare_parameter_if_not_declared(
+      node, this->behavior_name_ + ".minimum_speed",
+      rclcpp::ParameterValue(0.01));


Also for the nav2_params.yaml

RBT22 force-pushed the rolling/acc-drive-on-heading branch from a6633a8 to 228de3b Compare December 20, 2024 11:29

SteveMacenski reviewed Dec 20, 2024

View reviewed changes

RBT22 force-pushed the rolling/acc-drive-on-heading branch from f8f5bd8 to 36daf76 Compare January 2, 2025 14:49

RBT22 force-pushed the rolling/acc-drive-on-heading branch from 36daf76 to 83b4ff4 Compare January 2, 2025 14:54

RBT22 force-pushed the rolling/acc-drive-on-heading branch from 79dbabc to 836fb46 Compare January 2, 2025 15:21

RBT22 force-pushed the rolling/acc-drive-on-heading branch 3 times, most recently from a56b9c8 to 544887c Compare January 2, 2025 15:48

SteveMacenski requested changes Jan 7, 2025

View reviewed changes

RBT22 added 7 commits January 9, 2025 10:21

Add acceleration constraints

2a921f8

Signed-off-by: RBT22 <[email protected]>

Cleanup code

22980c2

Signed-off-by: RBT22 <[email protected]>

Format code

cd280c8

Signed-off-by: RBT22 <[email protected]>

Add <limits> header to drive_on_heading.hpp

3691936

Signed-off-by: RBT22 <[email protected]>

Remove vel pointer

f999511

Signed-off-by: RBT22 <[email protected]>

Use the limits only if both of them is set

bb11ff9

Signed-off-by: RBT22 <[email protected]>

Fix onActionCompletion params

9b9a6ce

Signed-off-by: RBT22 <[email protected]>

RBT22 force-pushed the rolling/acc-drive-on-heading branch from 544887c to 9b9a6ce Compare January 9, 2025 10:02

RBT22 added 2 commits January 10, 2025 14:37

Add default acc params and change decel sign

1098962

Signed-off-by: RBT22 <[email protected]>

Add minimum speed parameter

4a544d5

Signed-off-by: RBT22 <[email protected]>

WIP test

f1a87f8

Signed-off-by: RBT22 <[email protected]>

SteveMacenski reviewed Jan 11, 2025

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add acceleration limits to DriveOnHeading and BackUp behaviors #4810

Add acceleration limits to DriveOnHeading and BackUp behaviors #4810

RBT22 commented Dec 20, 2024

mergify bot commented Dec 20, 2024

mergify bot commented Dec 20, 2024

mergify bot commented Dec 20, 2024

SteveMacenski left a comment

mergify bot commented Jan 2, 2025

mergify bot commented Jan 2, 2025

mergify bot commented Jan 2, 2025

mergify bot commented Jan 2, 2025

mergify bot commented Jan 2, 2025

codecov bot commented Jan 2, 2025 •

edited

Loading

SteveMacenski commented Jan 7, 2025 •

edited

Loading

SteveMacenski Jan 7, 2025

SteveMacenski Jan 7, 2025

RBT22 Jan 10, 2025

SteveMacenski Jan 11, 2025 •

edited

Loading

mergify bot commented Jan 9, 2025

SteveMacenski Jan 11, 2025

SteveMacenski Jan 11, 2025

SteveMacenski left a comment

SteveMacenski Jan 11, 2025

	rclcpp::ParameterValue(0.01));
	rclcpp::ParameterValue(0.10));

Add acceleration limits to DriveOnHeading and BackUp behaviors #4810

Are you sure you want to change the base?

Add acceleration limits to DriveOnHeading and BackUp behaviors #4810

Conversation

RBT22 commented Dec 20, 2024

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

mergify bot commented Dec 20, 2024

mergify bot commented Dec 20, 2024

mergify bot commented Dec 20, 2024

SteveMacenski left a comment

Choose a reason for hiding this comment

mergify bot commented Jan 2, 2025

mergify bot commented Jan 2, 2025

mergify bot commented Jan 2, 2025

mergify bot commented Jan 2, 2025

mergify bot commented Jan 2, 2025

codecov bot commented Jan 2, 2025 • edited Loading

Codecov Report

SteveMacenski commented Jan 7, 2025 • edited Loading

SteveMacenski Jan 7, 2025

Choose a reason for hiding this comment

SteveMacenski Jan 7, 2025

Choose a reason for hiding this comment

RBT22 Jan 10, 2025

Choose a reason for hiding this comment

SteveMacenski Jan 11, 2025 • edited Loading

Choose a reason for hiding this comment

mergify bot commented Jan 9, 2025

SteveMacenski Jan 11, 2025

Choose a reason for hiding this comment

SteveMacenski Jan 11, 2025

Choose a reason for hiding this comment

SteveMacenski left a comment

Choose a reason for hiding this comment

SteveMacenski Jan 11, 2025

Choose a reason for hiding this comment

codecov bot commented Jan 2, 2025 •

edited

Loading

SteveMacenski commented Jan 7, 2025 •

edited

Loading

SteveMacenski Jan 11, 2025 •

edited

Loading