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 86.95652% with 15 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...ntroller/src/regulated_pure_pursuit_controller.cpp	36.36%	14 Missing ⚠️
..._pure_pursuit_controller/src/parameter_handler.cpp	97.29%	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% <ø> (ø)`
..._pure_pursuit_controller/src/parameter_handler.cpp	`93.38% <97.29%> (+0.40%)`	⬆️
...ntroller/src/regulated_pure_pursuit_controller.cpp	`79.89% <36.36%> (-5.82%)`	⬇️

... and 6 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-10-30T12:00:17Z

@SteveMacenski Hi,
I hope you had a great time at ROSCon.
Apologies for the delay, but I’ve completed the updates in response to your review.
I’ve also created a pull request for the documentation changes in the docs.nav2.org repository:
ros-navigation/docs.nav2.org#802
I’d appreciate it if you could take another look when you have a moment.

In addition, I have a couple of points I’d like to ask about:

Rename of desired_linear_vel
I renamed desired_linear_vel to max_linear_vel to follow the naming convention used in other packages for specifying the maximum velocity. However, I’d like to confirm whether this change is appropriate from a compatibility perspective.
Sign of deceleration parameters
The newly added max_linear_decel and max_angular_decel parameters are implemented assuming positive values. For reference, cancel_deceleration in RPP uses positive values, while max_decel in velocity_smoother is defined as negative. I’ve currently implemented them to be positive following RPP, but I’d appreciate your confirmation on whether this approach is appropriate.

Thank you very much for your time and support.

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

Decwest · 2025-11-20T11:48:22Z

@SteveMacenski friendly ping:)

SteveMacenski · 2025-11-24T21:20:23Z

Please fix the merge conflict

Sorry for the delay. ROSCon + vacation took me out of the office for some time and takes a moment to get up to speed on all the PRs again that have piled up

This looks much better now though!

SteveMacenski · 2025-11-24T21:31:37Z

nav2_regulated_pure_pursuit_controller/README.md

+| `min_linear_vel` | The minimum linear velocity to use. |
+| `max_angular_vel` | The maximum angular velocity to use. |
+| `min_angular_vel` | The minimum angular velocity to use. |
+| `max_linear_accel` | The maximum linear acceleration to use. |
+| `max_linear_decel` | The maximum linear deceleration to use. |
+| `max_angular_accel` | The maximum angular acceleration to use. |
+| `max_angular_decel` | The maximum angular deceleration to use. |


Mention any that are specific to your technique

I added "used when use_dynamic_window is true" except max_linear_vel and max_angular_accel

SteveMacenski · 2025-11-24T21:32:04Z

nav2_regulated_pure_pursuit_controller/README.md

      cost_scaling_gain: 1.0
      inflation_cost_scaling_factor: 3.0
+      min_distance_to_obstacle: 0.0
+      use_dynamic_window: true


Default this to false please

Sorry, I set use_dynamic_window's default value in parameter_handler.cpp and setting in README as false.

Also, I set proper default values in nav2.docs.org
ros-navigation/docs.nav2.org#802

SteveMacenski · 2025-11-24T21:34:19Z

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

  logger_ = logger;

  declare_parameter_if_not_declared(
-    node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5));


We should have a migration log here. If desired_linear_vel is set, log a warning that it has been changed to max_linear_vel and that they should migrate the use.

I modified that if user uses desired_linear_vel, set max_linear_vel value equals to desired_linear_vel and print a migration log.
https://github.com/Decwest/navigation2/blob/feature/implement_dwpp/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp#L39-L56

SteveMacenski · 2025-11-24T21:34:48Z

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".max_linear_decel", rclcpp::ParameterValue(0.5));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(1.0));


Make these reasonable, we had defaults here already, do not override them. Ditto for all the acceleration settings here

I set these same as current RPP's default value.

SteveMacenski · 2025-11-24T21:35:03Z

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".min_linear_vel", rclcpp::ParameterValue(0.0));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".max_angular_vel", rclcpp::ParameterValue(1.0));


Ditto defaults here, see other controllers for sensible defaults

I set these same as velocity smoother's default values

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

SteveMacenski · 2025-11-24T21:46:15Z

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

+      // considering velocity and acceleration constraints (DWPP)
+      const double regulated_linear_vel = linear_vel;
+      geometry_msgs::msg::Twist current_speed;
+      if (params_->velocity_feedback == "CLOSED_LOOP") {


Shouldn't be be made of use more generally if we're enabling open vs closed loop? This is not the only place speed is utilized

Indeed, since the current velocity is also used in other parts of RPP, for example when computing the look ahead distance, we should have an issue or a PR to discuss OPEN_LOOP vs CLOSED_LOOP at the level of the entire RPP controller.

For now, I am using OPEN_LOOP in the Dynamic Window computation.

Signed-off-by: Fumiya Ohnishi <[email protected]>

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

Decwest · 2025-11-25T13:28:39Z

@SteveMacenski
Thank you very much for taking the time to provide such a careful review despite your busy schedule.
I will address all of your comments once again.

I have one issue that I would like to ask for your advice on.
At the moment, I am developing in a Rolling Docker environment based on osrf/ros:rolling-desktop-full:
https://github.com/Decwest/dwpp_test_environment/blob/feature/nav2_integration/docker/Dockerfile.rolling

However, after syncing Nav2 to the latest version today and building and running it in this environment, I encountered a segmentation fault, and Nav2 no longer starts.
If you happen to have any insight into what might be causing this, I would greatly appreciate your advice.

Below is a detailed description of the situation.

Log obtained when running controller_server in debug mode

ROLLING ubuntu:~/ros2_ws$ ros2 run nav2_controller controller_server \
  --ros-args --log-level debug \
  -r __ns:=/ \
  -p use_sim_time=False
[DEBUG] [1764069038.870090908] [rclcpp]: signal handler installed
[DEBUG] [1764069038.870102907] [rclcpp]: deferred_signal_handler(): waiting for SIGINT/SIGTERM or uninstall
[DEBUG] [1764069038.870473289] [rcl]: Initializing node 'controller_server' in namespace ''
[DEBUG] [1764069038.870496899] [rcl]: Using domain ID of '0'
[ros2run]: Segmentation fault

From this, it seems that the process runs at least up to rclcpp::init() and rcl_init(), but it crashes immediately afterward. In addition to controller_server, planner_server, behavior_server, smoother_server, route_server, and opennav_docking all crashed as well. bt_navigator, waypoint_follower, collision_monitor, velocity_smoother, and lifecycle_manager remained alive. No other error messages were printed.

Clean build
I deleted the build, install, and log directories and rebuilt everything, but the issue persisted.
Recreating the environment
I cloned the repository from scratch on another PC and rebuilt the Docker environment, but the same problem occurred. Because of this, I believe it is unlikely that any local files are causing the issue.
Updating binaries
I ran sudo apt update and upgrade and then performed a clean build, but the same issue occurred.
Downgrading the Nav2 version
I reverted the Nav2 commit to an earlier one (10/26, Decwest@d536d33
) and performed a clean build, but the issue persisted.
Reverting the Docker image to a previous version
I attempted this, but I could not find older tags for osrf/ros:rolling-desktop-full. When I ran Docker build today, the image was cloned from scratch, so it is certain that the base image has been updated since the last time the environment was working. My suspicion is that this update might be related to the issue.

If you have any ideas or suggestions, I would be grateful to hear them.
Thank you very much for your time and assistance.

SteveMacenski · 2025-11-25T18:33:58Z

Unfortunately I'm not sure - it sounds like you took some of the right debugging steps to try to isolate the issue. The next steps I would try are to compile with debug flags and run with GDB to see what that says. This tutorial may be helpful: https://docs.nav2.org/tutorials/docs/get_backtrace.html

If its the core docker image, which it sounds like it may be, the right answer would be try to use an older version. Another option would be to build that image yourself locally and see if that does the trick.

SteveMacenski · 2025-11-25T19:03:34Z

We also have a nav2 docker that hasn't been updated in some time and we keep older images around for 30 or 60 days so you should be able to find one from October even after I get it turning over right now https://github.com/ros-navigation/nav2_docker

(annoyingly github disables jobs after 30 days of the repo being inactive, even though its job is fully automated so there's not really any active pushes required for a year at time)

Decwest · 2025-11-26T16:34:32Z

Thank you very much for providing such helpful information.

After trying a very, very large number of things, I found that building BehaviorTree.CPP from source resolved the issue described above.
https://github.com/BehaviorTree/BehaviorTree.CPP

GDB did not produce any readable clues, and neither the Rolling version nor the Nav2 version had anything to do with the issue.
However, the presence or absence of the issue clearly changed depending on whether BehaviorTree.CPP was built from source, so that is certainly the cause. The only thing I still do not understand is why a source build suddenly became necessary despite everything having worked fine until now.

Anyway, the clue that led me to the solution came from the fact that the nav2_docker you introduced builds BehaviorTree.CPP from source. I would like to express my sincere gratitude once again.

I will continue addressing the comments you provided.

SteveMacenski · 2025-11-26T19:29:50Z

Ah, I know why: BehaviorTree/BehaviorTree.CPP#1028. It supposedly was resolved but needs to be synced up to the binaries to resolve. It sounds like the issue wasn't a controller server crash but a BT Navigator crash.

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?

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 4 commits October 30, 2025 12:32

⚡ add unit test of computeDynamicWindow()

abb6c09

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

⚡ add and pass all unit test

3da3e29

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

⚡ update README

1785233

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

📝 update README

e9585d4

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

Decwest mentioned this pull request Oct 30, 2025

Add DWPP Description ros-navigation/docs.nav2.org#802

Open

🩹 fix hyperlink of README

cd170ab

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

SteveMacenski requested changes Nov 24, 2025

View reviewed changes

Decwest added 3 commits November 25, 2025 15:29

Merge branch 'main' into feature/implement_dwpp

ab82855

Signed-off-by: Fumiya Ohnishi <[email protected]>

🩹 dwpp defalut option set to false

de292df

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

🩹 fix default values

d69470a

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]>

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

Uh oh!

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 Oct 30, 2025

Uh oh!

Decwest commented Nov 20, 2025

Uh oh!

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

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Decwest commented Oct 8, 2025 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

codecov bot commented Oct 11, 2025 •

edited

Loading

SteveMacenski commented Nov 24, 2025 •

edited

Loading