Implementation of Dynamic Window Pure Pursuit (DWPP)

[Decwest]

---

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

[doisyg]

@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

[codecov]

Codecov Report

❌ Patch coverage is 97.45763% with 3 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...ntroller/src/regulated_pure_pursuit_controller.cpp	96.59%	3 Missing ⚠️

Files with missing lines	Coverage Δ
...ated_pure_pursuit_controller/parameter_handler.hpp	100.00% <ø> (ø)
...t_controller/regulated_pure_pursuit_controller.hpp	100.00% <ø> (ø)
..._pure_pursuit_controller/src/parameter_handler.cpp	94.18% <100.00%> (+0.55%)	⬆️
...ntroller/src/regulated_pure_pursuit_controller.cpp	89.05% <96.59%> (+3.26%)	⬆️

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

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]

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

[mergify]

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

[SteveMacenski]

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

[mergify]

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

[mergify]

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

[Decwest]

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