Implementation of Dynamic Window Pure Pursuit (DWPP) #5591

Decwest · 2025-10-08T14:41:21Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5524
Primary OS tested on	Ubuntu 24.04 (ROS2 rolling)
Robotic platform tested on	gazebo simulation of turtlebot3_waffle
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

Description of contribution in a few bullet points

Implement DWPP as an extension to the current RPP implementation

Description of documentation updates required from your changes

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

Added Parameters

max_linear_vel (rename of desired_linear_vel) (double): maximum linear velocity (m/s)
min_linear_vel (double): minimum linear velocity(m/s)
max_angular_vel (rename of desired_angular_vel) (double): maximum angular velocity (rad/s)
min_angular_vel (double): minimum angular velocity (rad/s)
max_linear_accel (double): maximum linear acceleration (m/s²)
max_linear_decel (double): maximum linear deceleration (m/s²)
max_angular_decel (double): maximum angular deceleration (m/rad²)
use_dynamic_window (bool): enable DWPP
velocity_feedback (string, no need to change) : How the current velocity is obtained during dynamic window computation.
- "OPEN_LOOP": Uses the last commanded velocity (recommended)
- "CLOSED_LOOP": Uses odometry velocity (may hinder proper acceleration/deceleration)

Description of how this change was tested

Built a simulation environment that allows comparison between different methods, and verified the implementation and effectiveness of DWPP
- Docker environment: https://github.com/Decwest/dwpp_test_environment/tree/feature/nav2_integration
- Nav2 parameter file used during simulation: https://github.com/Decwest/dwpp_test_simulation/blob/b90e1c0e43885f1146b16cff64f4358b5907b110/params/test_params.yaml

Simulation video

DWPP_simulation.mp4

Future work that may be required in bullet points

Discussion and modification regarding the renaming of Regulated Pure Pursuit
Documentation update

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
Should this be backported to current distributions? If so, tag with backport-*.

SteveMacenski

@doisyg FYI - would this be of interest? See the video

Also, please add unit testing for the functions and features you added. Check out test/ in the package to see some examples. Your main function should be tested for all edge cases at the bare minimum

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

doisyg · 2025-10-09T07:03:23Z

@doisyg FYI - would this be of interest? See the video

Absolutely ! @Decwest, we were discussing internally your DWPP repo (at Dexory) and I am super happy that you guys are working toward a nav2 integration.
Having linear acceleration constraints would be an immediate win for us.
Side note: could be a separate PR, but it would be even better to split the accel constraints between max_linear_accel and max_linear_decel. And in doing so, beware of bi-directionality, e.g. #3529
cc @kaichie @tonynajjar

…n2 into feature/implement_dwpp

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

This reverts commit 53c5cd4.

This reverts commit 547c54e.

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

codecov · 2025-10-11T13:27:29Z

Codecov Report

❌ Patch coverage is 99.25373% with 1 line in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
..._pure_pursuit_controller/src/parameter_handler.cpp	98.11%	1 Missing ⚠️

Files with missing lines	Coverage Δ
...ntroller/dynamic_window_pure_pursuit_functions.hpp	`100.00% <100.00%> (ø)`
...t_controller/regulated_pure_pursuit_controller.hpp	`100.00% <ø> (ø)`
...ntroller/src/regulated_pure_pursuit_controller.cpp	`86.80% <100.00%> (+1.08%)`	⬆️
..._pure_pursuit_controller/src/parameter_handler.cpp	`94.03% <98.11%> (+1.05%)`	⬆️

... and 8 files with indirect coverage changes

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

SteveMacenski · 2025-10-15T00:30:21Z

Let me know when you want me to take a look again after the review comments are addressed! Only things at a glance:

  const double A_MAX = params_->max_linear_accel;       // A_MAX
  const double V_MAX = params_->desired_linear_vel;     // V_MAX
  const double AW_MAX = params_->max_angular_accel;     // AW_MAX
  const double W_MAX = params_->desired_angular_vel;    // W_MAX
  const double DT = control_duration_;         // DT

  const double V_MIN = -V_MAX;
  const double W_MIN = -W_MAX;

Your very large block of code may be more concise as

// Lambda approach
auto check = [&](double v, double w) {
  if (v >= dw_vmin && v <= dw_vmax && w >= dw_wmin && w <= dw_wmax && v > best_v) {
    best_v = v; best_w = w;
  }
};
check(dw_vmin, k * dw_vmin);
check(dw_vmax, k * dw_vmax);
check(dw_wmin / k, dw_wmin);
check(dw_wmax / k, dw_wmax);

or

std::pair<double, double> candidates[] = {
  {dw_vmin, k * dw_vmin},
  {dw_vmax, k * dw_vmax},
  {dw_wmin / k, dw_wmin},
  {dw_wmax / k, dw_wmax}
};

for (auto [v, w] : candidates) {
  if (v >= dw_vmin && v <= dw_vmax && w >= dw_wmin && w <= dw_wmax && v > best_v) {
    best_v = v; best_w = w;
  }
}

I'd suggest reviewing your code a bit for how it can be as concise and self-descriptive as possible
Try to rename these variables to be more inline with the rest of the codebase please to make this function more readable.

Decwest · 2025-10-15T00:55:42Z

@SteveMacenski
Thank you for reviewing again, and sorry for the delay in my response.
I’ll address all of your comments one by one and will mention you once everything has been updated.
I’ll also work on improving the code readability.

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

mergify · 2025-10-22T12:09:49Z

@Decwest, 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: Decwest <[email protected]>

SteveMacenski · 2025-10-22T18:45:08Z

Let me know when I should take a look again! Note that you should probably rebase and/or pull in main so that CI can pass :-)

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

mergify · 2025-10-23T11:32:39Z

@Decwest, 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: Decwest <[email protected]>

…, add test (WIP) Signed-off-by: Decwest <[email protected]>

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

mergify · 2025-10-29T14:56:44Z

@Decwest, 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: Decwest <[email protected]>

Decwest · 2025-11-27T12:49:45Z

@SteveMacenski
I have finished addressing all of the comments, and I would appreciate it if you could take a look.
I have also updated the documentation, so I would be grateful if you could review that as well.
ros-navigation/docs.nav2.org#802

I have one question.
Currently, the code coverage of nav2_regulated_pure_pursuit_controller.cpp is below the required threshold.
However, I have written tests for each DWPP function, and the coverage of dynamic_window_pure_pursuit_functions.hpp is at 100 percent.
How should I address this?
Solved.

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

Decwest · 2025-12-10T09:55:27Z

@SteveMacenski
I am currently writing a journal paper on DWPP. If possible, I would like to conduct real-world experiments using the code merged into the main branch to ensure validity.
Would it be possible for you to review this PR at your earliest convenience?
Once submitted, I plan to upload the paper to arXiv and add a link to it in the RPP README or documentation. I believe this paper will serve as a good reference demonstrating the performance of this Nav2 implementation.

mergify · 2025-12-10T12:05:47Z

This pull request is in conflict. Could you fix it @Decwest?

Decwest · 2025-12-10T12:36:58Z

I was unable to fix the DCO issue.
I attempted an interactive rebase (git rebase -i) to add the missing sign-offs to past commits. However, after pushing, it seemed to include other merged commits as if I had authored them, which cluttered the commit log and the PR diff. I have reverted the branch to its original state for now.

SteveMacenski · 2025-12-10T19:45:38Z

git rebase HEAD~41 --signoff
git push --force-with-lease origin feature/implement_dwpp

Doesn't work? Maybe it would be best to get your work, squash it into a single commit that you sign off on and force push back? This is an important thing we need for contributors to verify that they have permission to contribute their work to the project.

Sorry for the delay, I'm not sure why this didn't come up in my notification tray.

SteveMacenski

I think you need to check linting / indents a bit, but this is MUCH improved, thank you!

nav2_regulated_pure_pursuit_controller/README.md

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

SteveMacenski · 2025-12-10T20:04:50Z

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

+  const std::string old_name = plugin_name_ + ".desired_linear_vel";
+  const std::string new_name = plugin_name_ + ".max_linear_vel";
+  const auto nan_val = std::numeric_limits<double>::quiet_NaN();
  declare_parameter_if_not_declared(
-    node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5));
+    node, old_name, rclcpp::ParameterValue(nan_val));
+  double old_val = nan_val;
+  if (node->get_parameter(old_name, old_val) &&
+    !std::isnan(old_val))
+  {
+    RCLCPP_WARN(
+      logger_,
+      "Parameter '%s' is deprecated. Use '%s' instead.",
+      old_name.c_str(), new_name.c_str());
+  }
+  double default_val = std::isnan(old_val) ? 0.5 : old_val;
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".max_linear_vel", rclcpp::ParameterValue(default_val));


This totally works, no issues, but I think we can do this better

`declare_parameter_if_not_declared(node, plugin_name_ + ".desired_linear_vel", rclcpp::PARAMETER_DOUBLE);` double max_vel_value = 0.5; try { node->get_parameter(plugin_name_ + ".desired_linear_vel", max_vel_value); // Then its set, so you should log the error } catch { // Then its not set, so can just pass } declare_parameter_if_not_declared( node, plugin_name_ + ".max_linear_vel", rclcpp::ParameterValue(max_vel_value));

Sincenode->get_parameterdoes not appear to throw an exception, I tried implementing it as shown below. What do you think?

navigation2/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

Line 39 in 7d9fbee

// Declare max_linear_vel with backward compatibility

If you declare without a default value it should throw. But maybe only if you use this API?

auto my_param = get_parameter("name").as<double>();

Thank you, I modified to use node->get_parameter(old_name).as_double(), and use try-catch statement

navigation2/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

Line 45 in c61debe

auto desired_linear_vel = node->get_parameter(old_name).as_double();

...ler/include/nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp

mergify · 2025-12-11T16:40:13Z

This pull request is in conflict. Could you fix it @Decwest?

Decwest · 2025-12-11T16:40:38Z

git rebase HEAD~41 --signoff
git push --force-with-lease origin feature/implement_dwpp
Doesn't work? Maybe it would be best to get your work, squash it into a single commit that you sign off on and force push back? This is an important thing we need for contributors to verify that they have permission to contribute their work to the project.

Sorry for the delay, I'm not sure why this didn't come up in my notification tray.

@SteveMacenski
Thank you as always for your careful review.
Regarding the DCO, if I rebase and force-push, the commits and the Files changed section become messy as they do now, but the DCO check does pass.
Would this be acceptable? I can still restore the branch to its original state since the history remains locally.

Decwest · 2025-12-11T16:42:17Z

If that works for you, I will go ahead and resolve the current conflicts.
If not, an alternative would be to create a new PR and recommit only the changes from this PR with the proper sign-off...

SteveMacenski · 2025-12-11T18:50:00Z

Just to short cut this issue, maybe it would be best to create a new branch & PR in which you migrate your commits over to that, sign off on it, and open a new PR to get around this. I'm not 100% what situation your git setup is in and rather than coaching you through it remotely sometimes just saying "screw this" and starting from a clean, current main branch and applying your changes is faster.

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

Decwest · 2025-12-14T09:24:42Z

@SteveMacenski
I’ve created a new PR with the same diff as this one:
#5783

I’ve addressed all of your comments except for the migration guide.
When you have time, I’d appreciate it if you could take a look.

Decwest added 2 commits October 6, 2025 20:50

⚡ add dwpp algorithm

142e30b

Merge branch 'ros-navigation:main' into feature/implement_dwpp

5279665

SteveMacenski requested changes Oct 8, 2025

View reviewed changes

Decwest added 6 commits October 11, 2025 21:45

♻️ modify to pass lint

547c54e

Merge branch 'feature/implement_dwpp' of github.com:Decwest/navigatio…

b380815

…n2 into feature/implement_dwpp

♻️ delete unrelated files & test signed off

53c5cd4

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

Revert ":recycle: delete unrelated files & test signed off"

e2176e1

This reverts commit 53c5cd4.

Revert ":recycle: modify to pass lint"

df7b2af

This reverts commit 547c54e.

♻️ reformat & revert unintentional file change

318e917

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

Decwest added 2 commits October 22, 2025 20:13

♻️ change variable names for consistency

68b6dd4

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

:refactor: modify value handling

3d123ac

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

Decwest added 3 commits October 22, 2025 21:31

🐛 fix type issue

7d2485f

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

♻️ reformat

e725917

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

⚡ add minimum velocity configuration

ff2b3cc

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

Decwest added 2 commits October 23, 2025 19:28

⚡ handle different accel and decel considering bidirectionality

3a57631

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

Merge branch 'ros-navigation:main' into feature/implement_dwpp

e85780a

Decwest added 4 commits October 23, 2025 21:42

♻️ refactor dwpp function to improve readability

9033141

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

Merge branch 'ros-navigation:main' into feature/implement_dwpp

8cb1fcb

🐛 fix regulation procedure of dynamic window during negative velocity…

79ea88b

…, add test (WIP) Signed-off-by: Decwest <[email protected]>

🐛 fix optimal velocity selection when moving backward

5d21f30

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

Decwest added 2 commits October 30, 2025 00:02

Merge branch 'ros-navigation:main' into feature/implement_dwpp

efb5795

♻️ refactor for easy unit test

0795c7d

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

Decwest added 7 commits November 27, 2025 15:30

⚡ add migration log of desired_linear_vel

469f8aa

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

🔥 remove velocity feedback parameter

c1fc519

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

♻️ remove implicit return using tuple

6c6db20

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

♻️ move dwpp functions to utils

735276c

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

♻️ use struct to represent dynamic window bounds

36c540f

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

Merge branch 'ros-navigation:main' into feature/implement_dwpp

e01cc20

📝 modify docstring and readme

43acc07

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

Decwest added 5 commits December 10, 2025 15:14

Merge branch 'ros-navigation:main' into feature/implement_dwpp

c91a092

🐛 add computeVelocityCommands test to improve code coverage

5008a97

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

📝 fix default value of readme

5b0b8d3

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

🩹 delete unused function

3ec5367

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

🐛 add test dependency

45cb87f

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

Decwest force-pushed the feature/implement_dwpp branch from 45cb87f to 43a74ea Compare December 10, 2025 12:05

Decwest force-pushed the feature/implement_dwpp branch from 43a74ea to 45cb87f Compare December 10, 2025 12:28

SteveMacenski reviewed Dec 10, 2025

View reviewed changes

Decwest force-pushed the feature/implement_dwpp branch from 7d9fbee to 6923056 Compare December 11, 2025 16:37

Decwest force-pushed the feature/implement_dwpp branch from 6923056 to 45cb87f Compare December 14, 2025 06:40

♻️ use try-catch for deprecation warning

c61debe

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

Decwest mentioned this pull request Dec 14, 2025

Implementation of Dynamic Window Pure Pursuit (DWPP) #5783

Open

8 tasks

⚡ handle comments

845fd41

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

Implementation of Dynamic Window Pure Pursuit (DWPP) #5591

Are you sure you want to change the base?

Implementation of Dynamic Window Pure Pursuit (DWPP) #5591

Conversation

Decwest commented Oct 8, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

Future work that may be required in bullet points

For Maintainers:

Uh oh!

SteveMacenski left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

doisyg commented Oct 9, 2025

Uh oh!

codecov bot commented Oct 11, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

SteveMacenski commented Oct 15, 2025

Uh oh!

Decwest commented Oct 15, 2025

Uh oh!

mergify bot commented Oct 22, 2025

Uh oh!

SteveMacenski commented Oct 22, 2025

Uh oh!

mergify bot commented Oct 23, 2025

Uh oh!

mergify bot commented Oct 29, 2025

Uh oh!

Decwest commented Nov 27, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Decwest commented Dec 10, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

mergify bot commented Dec 10, 2025

Uh oh!

Decwest commented Dec 10, 2025

Uh oh!

SteveMacenski commented Dec 10, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski Dec 10, 2025

Choose a reason for hiding this comment

Uh oh!

Decwest Dec 11, 2025

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Dec 11, 2025

Choose a reason for hiding this comment

Uh oh!

Decwest Dec 14, 2025

Choose a reason for hiding this comment

Uh oh!

Decwest commented Oct 8, 2025 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

codecov bot commented Oct 11, 2025 •

edited

Loading

Decwest commented Nov 27, 2025 •

edited

Loading

Decwest commented Dec 10, 2025 •

edited

Loading

SteveMacenski commented Dec 10, 2025 •

edited

Loading