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

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).

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

SteveMacenski

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

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

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

RBT22 · 2025-01-22T12:12:22Z

Hi @SteveMacenski,

I’ve been looking into the testing setup, and I believe I might have misunderstood it initially. If I’m interpreting this correctly now, the tests for the behaviors themselves are part of the nav2_system_tests package. Is that correct?

If so, I’ll need to edit the backup_tester.py and drive_tester.py scripts to incorporate the acceleration checks.

Thanks for your guidance!

SteveMacenski · 2025-01-23T02:28:26Z

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

@@ -133,7 +134,35 @@ class DriveOnHeading : public TimedBehavior<ActionT>
    cmd_vel->header.frame_id = this->robot_base_frame_;
    cmd_vel->twist.linear.y = 0.0;
    cmd_vel->twist.angular.z = 0.0;
-    cmd_vel->twist.linear.x = command_speed_;
+    if (acceleration_limit_ <= 0.0 || deceleration_limit_ >= 0.0) {
+      cmd_vel->twist.linear.x = command_speed_;


Add an RCLCPP_INFO_ONCE that accel/decel limits were not set

SteveMacenski · 2025-01-23T02:28:52Z

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

+      rclcpp::ParameterValue(0.10));
+    node->get_parameter(this->behavior_name_ + ".acceleration_limit", acceleration_limit_);
+    node->get_parameter(this->behavior_name_ + ".deceleration_limit", deceleration_limit_);
+    node->get_parameter(this->behavior_name_ + ".minimum_speed", minimum_speed_);


Check after this to force the accel/decel to be positive/negative. That will save users some headaches

SteveMacenski · 2025-01-23T02:34:14Z

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

+    } else {
+      double current_speed = last_vel_ == std::numeric_limits<double>::max() ? 0.0 : last_vel_;
+
+      double min_feasible_speed = current_speed + deceleration_limit_ / this->cycle_frequency_;


What if the current_speed is negative? If you have asymmetric acceleration and deceleration limits (i.e. -1.0, 2.5) then the 'accelerating backward' will be deceleration and decelerating backward will be acceleration

(we have this issue in MPPI too, so its an easy thing to miss #4601)

SteveMacenski · 2025-01-23T02:39:18Z

If I’m interpreting this correctly now, the tests for the behaviors themselves are part of the nav2_system_tests package. Is that correct?

Yes!

.. Actually I don't remember what tests you were modifying now. The force pushes removed the changes from the history so I can't recount.

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 force-pushed the rolling/acc-drive-on-heading branch from 544887c to 9b9a6ce Compare January 9, 2025 10:02

SteveMacenski reviewed Jan 11, 2025

View reviewed changes

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp Outdated Show resolved Hide resolved

SteveMacenski reviewed Jan 11, 2025

View reviewed changes

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp Outdated Show resolved Hide resolved

RBT22 mentioned this pull request Jan 17, 2025

Documentation for acceleration limits in DriveOnHeading and BackUp behaviors ros-navigation/docs.nav2.org#634

Open

RBT22 force-pushed the rolling/acc-drive-on-heading branch from bd6054d to 112b5fb Compare January 22, 2025 08:43

RBT22 added 5 commits January 22, 2025 09:59

Add acceleration constraints

8bb038d

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

Cleanup code

5ff40c6

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

Format code

7e398a7

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

Add <limits> header to drive_on_heading.hpp

33ff0c4

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

Remove vel pointer

00792e7

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

RBT22 added 5 commits January 22, 2025 09:59

Use the limits only if both of them is set

5293a96

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

Fix onActionCompletion params

a477769

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

Add default acc params and change decel sign

e1f089c

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

Add minimum speed parameter

074bf40

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

Update minimum speed parameter to 0.10

84b1fec

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

RBT22 force-pushed the rolling/acc-drive-on-heading branch from 112b5fb to 84b1fec Compare January 22, 2025 09:00

SteveMacenski reviewed Jan 23, 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

mergify bot commented Jan 9, 2025

SteveMacenski left a comment

RBT22 commented Jan 22, 2025

SteveMacenski Jan 23, 2025

SteveMacenski Jan 23, 2025

SteveMacenski Jan 23, 2025

SteveMacenski Jan 23, 2025

SteveMacenski commented Jan 23, 2025 •

edited

Loading

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

mergify bot commented Jan 9, 2025

SteveMacenski left a comment

Choose a reason for hiding this comment

RBT22 commented Jan 22, 2025

SteveMacenski Jan 23, 2025

Choose a reason for hiding this comment

SteveMacenski Jan 23, 2025

Choose a reason for hiding this comment

SteveMacenski Jan 23, 2025

Choose a reason for hiding this comment

SteveMacenski Jan 23, 2025

Choose a reason for hiding this comment

SteveMacenski commented Jan 23, 2025 • edited Loading

codecov bot commented Jan 2, 2025 •

edited

Loading

SteveMacenski commented Jan 7, 2025 •

edited

Loading

SteveMacenski commented Jan 23, 2025 •

edited

Loading