Tracking critics dwb and mppi by silanus23 · Pull Request #5603 · ros-navigation/navigation2

silanus23 · 2025-10-13T14:51:09Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5037
Primary OS tested on	Ubuntu
Robotic platform tested on	nav2_bringup
Does this PR contain AI generated software?	util tests
Was this PR description generated by AI software?	nope definitely not

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Critic additons and their parameter additions

Description of how this change was tested

Added util tests. I have succeded to create cases where turtlebot has to create a new path cause it's so out of bounds(done this on dwb).

Future work that may be required in bullet points

These can be added to default yaml

###Note:
I had to create a new branch cause the original branch was sub branch of my first PR. I had to carry it via copy paste couldn't find a better way.

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 · 2025-10-13T22:20:14Z

Have you tested these critics in some way - in particular for MPPI? The TB4 world with some strategically placed obstacles in the way might be a good test case. The MPPI critic looks pretty complex. I would have intuitively started with something that simply:

Evaluates the distance from the path segments (using the utility function already merged) to a given trajectory sample. If outside, then penalize highly.
As an add on feature, add the option to also have a penalty that scales with the distance away from the path (with some margin before application so we don't overscore based on small adjustments).

I suspect (?) that it may be possible to instead strategically place this logic into the Path Align Critic and/or share some of its computations to make that critic more simple and concise. Is it possible to surgically place this into the Path Align logic and/or some of the computation is shared? Either we can share some functions and/or data, or put the Path Hug scoring inside of that critic as additional weight penalties (if we already have path-trajectory distances computed for its own scoring)

silanus23 · 2025-10-13T23:05:39Z

@SteveMacenski I didn't try to get full results. I tried to solve the problem of the Planner can not keep up with the HZ (this part needed optimizations making this PR a lot complex). PathAlign integration is actually sounds a lot better than my solution.In addition PathFollow can be considered too. But I have to look it to be sure. There are plenty of path mppi critic that uses sampling strides etc. I don't know if it's too daring but maybe unification of path critics and keeping both versions.
BTW realized some of my additions to dwb after testing (ones I shared as visuals in previous PR) causing some problems took them back and will try to get visual results in this version too.
Regardless of the solution my initial proposal is deleting everything about MPPI. Making this PR only about DWB and handle MPPI in another sperate PR. What do you think about this approach?

SteveMacenski · 2025-10-14T18:44:07Z

I think that makes sense - though I think MPPI is much higher priority than DWB as the default controller. DWB is more for legacy users.

SteveMacenski

Otherwise, I can't comment too too much on the formulation without you/I doing some some simulation testing to see how well functionally it works.

SteveMacenski · 2025-11-27T01:10:53Z

nav2_dwb_controller/dwb_critics/src/path_hug.cpp

+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  nav2::declare_parameter_if_not_declared(


Should we also set a max allowable left/right deviation to score as invalid if any path of the path exceeds?

SteveMacenski · 2025-11-27T01:11:39Z

nav2_dwb_controller/dwb_critics/src/path_hug.cpp

+namespace dwb_critics
+{
+
+void PathHugCritic::onInit()


I actually rather like this name, but for the description docs / doxygen, can we mention cross tracking error (if not rename the critic to be more 'technical') so that this is clear the itention?

SteveMacenski · 2025-11-27T01:14:35Z

nav2_dwb_controller/dwb_critics/src/path_hug.cpp

+      start_index_,
+      search_window_);
+    distance += search_result.distance;
+    start_index_ = search_result.closest_segment_index;


The trajectories can technically go backwards so I don't think we should reset the start index, no?

SteveMacenski · 2025-11-27T01:14:59Z

nav2_dwb_controller/dwb_critics/include/dwb_critics/path_hug.hpp

+  double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override;
+
+protected:
+  bool zero_scale_{false};


This isn't exposed as a parameter?

SteveMacenski · 2025-11-27T01:15:14Z

nav2_dwb_controller/dwb_critics/include/dwb_critics/path_hug.hpp

+  bool zero_scale_{false};
+  nav_msgs::msg::Path global_path_;
+  size_t start_index_{0};
+  geometry_msgs::msg::Pose current_pose_{};


I don't think this needs to be a class member variable

SteveMacenski · 2025-11-27T01:15:24Z

nav2_dwb_controller/dwb_critics/include/dwb_critics/path_hug.hpp

+protected:
+  bool zero_scale_{false};
+  nav_msgs::msg::Path global_path_;
+  size_t start_index_{0};


mergify · 2025-12-12T01:21:11Z

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

silanus23 · 2025-12-12T13:14:39Z

  ```
  critics: ["Oscillation", "BaseObstacle", "GoalAlign","PathHug"]
  PathHug.scale: 144.0
  ```

To isolate the effect of critic I used above params. Didn't notice weirdness or negative effect but to be honest didn't see any improvement unlike my mppi PR. Maybe it's about the nature of dwb or maybe I need to test in diffirent environments.
Current bideractional logic can cause some direction issue but it's a trade off since we can't know the intended direction. Other critics should be able to compansate it.

Later edit after "fixing absolute value problem" commit. There was a mathematicall error currently with the above setup I am able to get better results especially around corners with tb4.

silanus23 · 2025-12-13T11:07:27Z

@mini-1235 Can you help me on this PR?

Signed-off-by: silanus23 <berkantali23@outlook.com>

codecov · 2025-12-13T21:38:35Z

Codecov Report

❌ Patch coverage is 67.92453% with 17 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_dwb_controller/dwb_critics/src/path_hug.cpp	67.92%	17 Missing ⚠️

Files with missing lines	Coverage Δ
nav2_dwb_controller/dwb_critics/src/path_hug.cpp	`67.92% <67.92%> (ø)`

... and 23 files with indirect coverage changes

🚀 New features to boost your workflow:

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

Signed-off-by: silanus23 <berkantali23@outlook.com>

silanus23 force-pushed the tracking-critics-dwb-and-mppi branch from 8f77cec to dd0faed Compare October 28, 2025 07:11

SteveMacenski reviewed Nov 27, 2025

View reviewed changes

silanus23 added 4 commits December 14, 2025 00:09

DWB critics addition

0bf9368

Signed-off-by: silanus23 <berkantali23@outlook.com>

mppi critic addition

2397e7c

Signed-off-by: silanus23 <berkantali23@outlook.com>

Taking back later additions creating problem

4fc9b0e

Signed-off-by: silanus23 <berkantali23@outlook.com>

addressing reviews and adding bideractional window

115e700

Signed-off-by: silanus23 <berkantali23@outlook.com>

silanus23 force-pushed the tracking-critics-dwb-and-mppi branch from 22426a7 to 115e700 Compare December 13, 2025 21:09

fixing absolute value problem

dfab167

Signed-off-by: silanus23 <berkantali23@outlook.com>

Conversation

silanus23 commented Oct 13, 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 commented Oct 13, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

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

Uh oh!

Uh oh!

SteveMacenski commented Oct 14, 2025

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Nov 27, 2025

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Nov 27, 2025

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Nov 27, 2025

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Nov 27, 2025

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Nov 27, 2025

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Nov 27, 2025

Choose a reason for hiding this comment

Uh oh!

mergify bot commented Dec 12, 2025

Uh oh!

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

Uh oh!

Uh oh!

silanus23 commented Dec 13, 2025

Uh oh!

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

Uh oh!

Codecov Report

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

silanus23 commented Oct 13, 2025 •

edited

Loading

SteveMacenski commented Oct 13, 2025 •

edited

Loading

silanus23 commented Oct 13, 2025 •

edited

Loading

silanus23 commented Dec 12, 2025 •

edited

Loading

codecov bot commented Dec 13, 2025 •

edited

Loading