Tracking critic dwb 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

+    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

+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

+      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

+  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

+  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

+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?

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

Codecov Report

❌ Patch coverage is 74.52229% with 40 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_dwb_controller/dwb_critics/src/path_hug.cpp	74.30%	37 Missing ⚠️
...oller/dwb_critics/include/dwb_critics/path_hug.hpp	76.92%	3 Missing ⚠️

Files with missing lines	Coverage Δ
...oller/dwb_critics/include/dwb_critics/path_hug.hpp	`76.92% <76.92%> (ø)`
nav2_dwb_controller/dwb_critics/src/path_hug.cpp	`74.30% <74.30%> (ø)`

... and 29 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>

…eading error, have backward search and implement own deviation calculator to achive more control. Update tests accordingly. Signed-off-by: silanus23 <berkantali23@outlook.com>

silanus23 · 2026-06-03T19:57:44Z

Hey I have implemented this version from mppi's improvements. Since this is an alternative and can work without path align I have added a basic heading critic too. Right now it seems to be working and it's performance is better than path align. Unlike mppi I didn't come across any velocity issue.
While I draw a very optimistic outcome I must say I didn't push this code much. I am trying this on tb3 which since it's more harmonized with dwb than tb4. I think this is a good base we can develop on.
I will try to push videos before going offline but not sure if I can
Ohh lastly I know you want a diffirent name. I have 4 alternatives at the top my head. Path adhesive, tracking error, bounded tracking error, path tracking. I can change to one of these or to your alternative if you want, both mppi and this.

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 force-pushed the tracking-critics-dwb-and-mppi branch from 22426a7 to 115e700 Compare December 13, 2025 21:09

silanus23 changed the title ~~Tracking critics dwb and mppi~~ Tracking critic dwb Apr 16, 2026

silanus23 added 5 commits June 3, 2026 01:20

DWB critics addition

4e8ad81

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

mppi critic addition

40e57a0

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

Taking back later additions creating problem

5e8b79b

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

addressing reviews and adding bideractional window

3fd0498

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

fixing absolute value problem

1dd96a4

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

silanus23 force-pushed the tracking-critics-dwb-and-mppi branch from dfab167 to 1dd96a4 Compare June 2, 2026 22:21

update with mppi foundings: trajectory and path min distancing, add h…

f255092

…eading error, have backward search and implement own deviation calculator to achive more control. Update tests accordingly. Signed-off-by: silanus23 <berkantali23@outlook.com>

silanus23 marked this pull request as ready for review June 3, 2026 19:57

Uh oh!

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!

silanus23 commented Jun 3, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

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

silanus23 commented Jun 3, 2026 •

edited

Loading