Tracking error layer #5605

silanus23 · 2025-10-13T16:12:38Z

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

Description of contribution in a few bullet points

I have added a layer that can draw a corridor around path with obstacles. In addition I added a geometry util that can draw smoother lines.

Description of documentation updates required from your changes

A costmap layer and it's params

Description of how this change was tested

Wrote a unit test and I have seen visually it is working

Future work that may be required in bullet points

Tests definitely need a see throgh.

Note:

I had to copy paste and carry changes to a new branch. Like I did on the prev 2 PR. This going to make me move with less burden. Commits look lesser and rushy but they ain't.

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

I'd recommend spending some time to think about how we can most optimally compute this since this is going to be a little heavy so its worth some performance considerations.

Off the cuff:

We don't really need to store an internal representation of the layer at all, so this could derive from the Layer rather than CostmapLayer since we shouldn't really need the internal costmap structure.
We default assume all points are lethal unless proven otherwise in the for each loop in the update window.
Given the path, we shift the path left/right by the left/right tolerances by the base frame. If we enclose the start of the path and the end of the path segment, we should end up with a polygon.
Using some polygon math, we can ID if a point is inside of that polygon or not. If inside, we set ignore. If outside, we set as lethal. We'd need to make sure this works with concave polygons since there's no promise that this is convex.

Also maybe ways that can improve the method you have now and/or convolved the path by some radius left/right?

We probably want this to mark more than just 1 cell around the boundary as lethal so that small quantization errors don't allow the system to break out. At least 3 cells thick, but also I was thinking perhaps it would be sensible to just default mark everything as occupied unless within the bounds of tracking. But maybe a "thick" line is fine actually if that gives us some computational gains :-)

SteveMacenski · 2025-10-13T22:26:24Z

nav2_costmap_2d/plugins/tracking_error_layer.cpp

+      this, std::placeholders::_1));
+
+  path_sub_ = node->create_subscription<nav_msgs::msg::Path>(
+    "/plan",


Remove forward slash in both subscriptions

SteveMacenski · 2025-10-13T22:26:47Z

nav2_costmap_2d/plugins/tracking_error_layer.cpp

+  tracking_feedback_sub_ = node->create_subscription<nav2_msgs::msg::TrackingFeedback>(
+    "/tracking_feedback",
+    std::bind(&TrackingErrorLayer::trackingCallback, this, std::placeholders::_1),
+    rclcpp::QoS(10).reliable()


For all QoS: Use the established Nav2 policies in nav2_ros_common

SteveMacenski · 2025-10-13T22:27:21Z

nav2_costmap_2d/plugins/tracking_error_layer.cpp

+    std::bind(&TrackingErrorLayer::trackingCallback, this, std::placeholders::_1),
+    rclcpp::QoS(10).reliable()
+  );
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node->get_clock());


You should have this already from base classes to this class. You shouldn't need to manually create another (which is very heavy)

SteveMacenski · 2025-10-13T22:28:01Z

nav2_costmap_2d/include/nav2_costmap_2d/tracking_error_layer.hpp

+  std::mutex path_mutex_;
+  std::mutex tracking_error_mutex_;


I think you can get away with a single data mutex

SteveMacenski · 2025-10-13T22:28:23Z

nav2_costmap_2d/plugins/tracking_error_layer.cpp

+  return result;
+}
+
+TrackingErrorLayer::~TrackingErrorLayer()


Move destructor right after the constructor

SteveMacenski · 2025-10-13T22:28:34Z

nav2_costmap_2d/plugins/tracking_error_layer.cpp

+void TrackingErrorLayer::reset() {}
+void TrackingErrorLayer::activate() {enabled_ = true;}
+void TrackingErrorLayer::deactivate() {enabled_ = false;}
+
+void TrackingErrorLayer::onFootprintChanged() {}
+void TrackingErrorLayer::cleanup() {}


Dont override if not implementing

SteveMacenski · 2025-10-13T22:28:48Z

nav2_costmap_2d/plugins/tracking_error_layer.cpp

+void TrackingErrorLayer::activate() {enabled_ = true;}
+void TrackingErrorLayer::deactivate() {enabled_ = false;}


These two should create / destroy the subscriptions

SteveMacenski · 2025-10-13T22:29:06Z

nav2_costmap_2d/plugins/tracking_error_layer.cpp

+  dyn_params_handler_.reset();
+}
+
+void TrackingErrorLayer::reset() {}


This should reset data and other states. Please check out other layers for example

silanus23 · 2025-12-05T13:38:15Z

@SteveMacenski I deleted bresenham line drawer to make it faster. I think I fixed your review points. I think adding constraints as seperate obstacles is a better choice, cause if a user tries to use this on a bigger local costmap it can become a huge computation. Also with rasterization, sharp turns could create problems. Current version creates big distinct blocks (like watchtowers) one after another. This can be parameterized and improved later. Not looking good but would do the job I think.

SteveMacenski · 2025-12-05T18:30:28Z

Related to your other PR, I'm passing off the first review stage to @mini-1235 and I'll come in afterwards

SteveMacenski · 2025-12-05T18:31:05Z

@SteveMacenski I deleted bresenham line drawer to make it faster. I think I fixed your review points. I think adding constraints as seperate obstacles is a better choice, cause if a user tries to use this on a bigger local costmap it can become a huge computation. Also with rasterization, sharp turns could create problems. Current version creates big distinct blocks (like watchtowers) one after another. This can be parameterized and improved later. Not looking good but would do the job I think.

Can you show an image of this behavior for example?

SteveMacenski

OK I can't help myself, here's a few notes. I didn't look at the main get path segments / wall functions, but I looked at the layer boilerplate

SteveMacenski · 2025-12-05T18:31:34Z

nav2_costmap_2d/costmap_plugins.xml

    <class type="nav2_costmap_2d::VoxelLayer"     base_class_type="nav2_costmap_2d::Layer">
      <description>Similar to obstacle costmap, but uses 3D voxel grid to store data.</description>
    </class>
+    <class type="nav2_costmap_2d::TrackingErrorLayer"    base_class_type="nav2_costmap_2d::Layer">


Maybe rename to BoundedTrackingErrorLayer since its adding bounds

SteveMacenski · 2025-12-05T18:32:25Z

nav2_costmap_2d/plugins/tracking_error_layer.cpp

+namespace nav2_costmap_2d
+{
+
+TrackingErrorLayer::TrackingErrorLayer() {}


You can just set this to TrackingErrorLayer() = default; in the header file instead

nav2_costmap_2d/plugins/bounded_tracking_error_layer.cpp

SteveMacenski · 2025-12-05T18:34:06Z

nav2_costmap_2d/plugins/tracking_error_layer.cpp

+void TrackingErrorLayer::trackingCallback(
+  const nav2_msgs::msg::TrackingFeedback::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  last_tracking_feedback_ = *msg;
+}
+


It would be good to check on timestamps to make sure these are sufficiently current

nav2_costmap_2d/plugins/bounded_tracking_error_layer.cpp

SteveMacenski · 2025-12-05T18:35:29Z

nav2_costmap_2d/plugins/tracking_error_layer.cpp

+
+  // Create subscriptions when layer is activated
+  path_sub_ = node->create_subscription<nav_msgs::msg::Path>(
+    "/plan",


Remove forward slashes here and in the tracking feedback to let this work with namespaces

I have tried without "/" but didn't work. It wasn't subscribing to those topics. Am I doing anything else wrong about this?

Ah. I know why. The costmap namespaces things on its topic. I think maybe instead you should make these parameters + add those parameters to the yaml file where there you set it.

Just make sure to use this function to parse the parameter https://github.com/ros-navigation/navigation2/blob/main/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp#L180. Grep the obstacle / static layer for use examples

SteveMacenski · 2025-12-05T18:35:46Z

nav2_costmap_2d/plugins/tracking_error_layer.cpp

+    last_path_.poses.clear();
+    last_path_.header = std_msgs::msg::Header();


Suggested change

last_path_.poses.clear();

last_path_.header = std_msgs::msg::Header();

last_path_ = nav_msgs::msg::Path();

nav2_costmap_2d/plugins/bounded_tracking_error_layer.cpp

silanus23 · 2025-12-08T15:54:50Z

@SteveMacenski currently looks like this. with bresenham function it was a smooth straight line.
About naming I though trackingbounds layer was better and shorter but if you want I can make it boundedtrackingerror. I couldn't find tf function you mentioned about path transforming. Can you help me about that? Or maybe I misunderstood you.

Copilot

Pull request overview

This PR introduces a new TrackingBoundsLayer costmap plugin that creates a corridor around the robot's path with obstacles, helping to constrain navigation within path boundaries. The implementation adds a new costmap layer, configuration parameters, and comprehensive unit tests.

Key Changes

Added TrackingBoundsLayer plugin that subscribes to path and tracking feedback topics to dynamically create corridor boundaries
Implemented wall point generation algorithm that calculates perpendicular boundaries around path segments
Added unit tests covering initialization, path segment extraction, wall point generation, and edge cases

Reviewed changes

Copilot reviewed 8 out of 8 changed files in this pull request and generated 19 comments.

Show a summary per file

File	Description
nav2_util/include/nav2_util/geometry_utils.hpp	Adds utility include for geometry operations
nav2_costmap_2d/include/nav2_costmap_2d/tracking_bounds_layer.hpp	Defines the TrackingBoundsLayer class interface with lifecycle methods and member variables
nav2_costmap_2d/plugins/tracking_bounds_layer.cpp	Implements the tracking bounds layer logic including callbacks, path segmentation, and costmap updates
nav2_costmap_2d/test/unit/tracking_bounds_layer_test.cpp	Provides comprehensive unit tests for the new layer functionality
nav2_costmap_2d/test/unit/CMakeLists.txt	Adds test target for tracking_bounds_layer_test
nav2_costmap_2d/CMakeLists.txt	Integrates tracking_bounds_layer.cpp into the layers library
nav2_costmap_2d/costmap_plugins.xml	Registers the TrackingBoundsLayer as a plugin
nav2_bringup/params/nav2_params.yaml	Configures the tracking_bounds_layer in the local costmap with default parameters

nav2_costmap_2d/test/unit/bounded_tracking_error_layer_test.cpp

nav2_costmap_2d/plugins/bounded_tracking_error_layer.cpp

nav2_costmap_2d/include/nav2_costmap_2d/tracking_bounds_layer.hpp

Copilot · 2025-12-08T16:08:45Z

nav2_costmap_2d/plugins/tracking_bounds_layer.cpp

+void TrackingBoundsLayer::pathCallback(const nav_msgs::msg::Path::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  auto now = node_.lock()->now();
+  auto msg_time = rclcpp::Time(msg->header.stamp);
+  auto age = (now - msg_time).seconds();
+
+  if (age > 1.0) {
+    RCLCPP_WARN_THROTTLE(
+      node_.lock()->get_logger(),
+      *node_.lock()->get_clock(),
+      5000,
+      "Path is %.2f seconds old", age);
+    return;


Multiple calls to node_.lock() without checking if the lock is successful. If the node has been destroyed, this could lead to a crash. Store the locked node in a local variable and check it once at the beginning of the function.

Example fix:

void TrackingBoundsLayer::pathCallback(const nav_msgs::msg::Path::SharedPtr msg) { std::lock_guard<std::mutex> lock(data_mutex_); auto node = node_.lock(); if (!node) { return; } auto now = node->now(); // ... rest of the function using 'node' instead of node_.lock()

nav2_costmap_2d/plugins/bounded_tracking_error_layer.cpp

Copilot · 2025-12-08T16:08:46Z

nav2_bringup/params/nav2_params.yaml

+        enabled: True
+        look_ahead: 5.0
+        step: 5
+        corridor_width: 2.0


[nitpick] The tracking_bounds_layer configuration is missing the tracking_feedback_topic and path_topic parameters. While these have defaults in the code ("tracking_feedback" and "plan"), they should be explicitly documented in the configuration file for clarity, especially since this is a new plugin. Add:

tracking_bounds_layer: plugin: "nav2_costmap_2d::TrackingBoundsLayer" enabled: True look_ahead: 5.0 step: 5 corridor_width: 2.0 tracking_feedback_topic: "tracking_feedback" path_topic: "plan"

Suggested change

corridor_width: 2.0

corridor_width: 2.0

tracking_feedback_topic: "tracking_feedback"

path_topic: "plan"

SteveMacenski · 2025-12-08T19:30:26Z

@SteveMacenski currently looks like this. with bresenham function it was a smooth straight line.

This doesn't look very dense / complete (?). I see large gaps there.

About naming I though trackingbounds layer was better and shorter but if you want I can make it boundedtrackingerror. I couldn't find tf function you mentioned about path transforming. Can you help me about that? Or maybe I misunderstood you.

Please update the name - I think the "Error" bit is an important clarification. You are correct on the TF thing - Maurice convinced me that my internal AI was hallucinating about it. That seems like something we should add to Nav2 utils.

SteveMacenski · 2025-12-09T16:55:35Z

Can you provide some updated picture + compute times? I think the density is worth it, but maybe there are other methods we could consider and/or optimize if the performance isn't high enough

silanus23 · 2025-12-09T17:13:22Z

I can make lines thicker. About computation I can try to bring numbers but I gotta say my pc is extremely bad (gpu burnt and 5 y.o.) and didn't felt any diffirence.

SteveMacenski · 2025-12-09T18:17:47Z

Can we make the line at least 2 pixels thick? 1 pixel, as before, could cause issues if the search window is larger than 1 cell length (like smac, which is $\sqrt{2}$, so in some cases it could skip over a good chunk of a cell at an angle that would bypass that check when the 2 lethal cells added are not edge-connected but corner-connected)

silanus23 · 2025-12-09T18:20:36Z

Should I parameterize it?

SteveMacenski · 2025-12-09T18:24:32Z

I think that might be good. Default at 2 though I think would be good.

mergify · 2025-12-12T00:56:53Z

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

silanus23 · 2025-12-12T12:42:56Z

@SteveMacenski I will solve conflict and open a doc. About computational weight I can bring actually numbers with perf but this can take too long in addition theoretically corridor logic (especially better in bigger local costmap bounds) and bresenham line function (specifically made for this usage) is the most performative choice I think. I still can bring out the numbers if you want.

SteveMacenski · 2025-12-12T19:55:34Z

@mini-1235 is taking over the first set of reviews on this feature - please direct to him for now :-)

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

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: silanus <berkantali23@outlook.com>

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

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

codecov · 2025-12-15T03:46:48Z

Codecov Report

❌ Patch coverage is 76.54867% with 53 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...ostmap_2d/plugins/bounded_tracking_error_layer.cpp	74.51%	53 Missing ⚠️

Files with missing lines	Coverage Δ
...e/nav2_costmap_2d/bounded_tracking_error_layer.hpp	`100.00% <100.00%> (ø)`
nav2_util/include/nav2_util/geometry_utils.hpp	`97.72% <100.00%> (+0.50%)`	⬆️
...ostmap_2d/plugins/bounded_tracking_error_layer.cpp	`74.51% <74.51%> (ø)`

... and 11 files with indirect coverage changes

🚀 New features to boost your workflow:

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

mini-1235 · 2025-12-22T12:34:45Z

I can make lines thicker. About computation I can try to bring numbers but I gotta say my pc is extremely bad (gpu burnt and 5 y.o.) and didn't felt any diffirence.

Hi @silanus23, regarding the visualization, I don't think it is very clear at the moment. It would also be helpful to get some metrics to see whether we are on the right track, especially to ensure this works well on weaker CPUs

silanus23 · 2025-12-22T12:58:17Z

Hi @mini-1235 I am aware that a PC's cpu is enough to compensate diffirences so you are right about asking for metrics. I just wanted to point out no overflows and in complexity wise current one was the best I could find. I will try to bring out metrics . In addition Currently lines are parameterized and double thickness of those and the thickness is parameterized.

As a last note: I might be away for a short period soon, but I will jump back into this and all other pending PRs as soon as I return. :-)

SteveMacenski reviewed Oct 13, 2025

View reviewed changes

SteveMacenski mentioned this pull request Nov 27, 2025

Constrain distance from robot to path: helper function, Controller feedback, BT condition node, DWB/MPPI Controller Critics, Possible Costmap Layer Constraint for other controllers #5037

Open

8 tasks

silanus23 force-pushed the tracking_error_layer branch 2 times, most recently from 936f1e4 to 97ed4c8 Compare December 5, 2025 13:21

SteveMacenski requested a review from mini-1235 December 5, 2025 18:29

SteveMacenski requested changes Dec 5, 2025

View reviewed changes

silanus23 marked this pull request as ready for review December 8, 2025 15:53

Copilot AI review requested due to automatic review settings December 8, 2025 15:53

Copilot started reviewing on behalf of silanus23 December 8, 2025 15:54 View session

Copilot AI reviewed Dec 8, 2025

View reviewed changes

silanus23 added 7 commits December 14, 2025 00:06

Library addition

1fe63c5

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

source file addition

86f9b12

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

test file added

88f830e

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

bresenhamLineaddition

03e825d

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

Integration and last additions

6aa126e

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

first styling changes

a183e9a

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

fixed tf usage made it layer test fix

2055dbc

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

silanus23 and others added 11 commits December 14, 2025 00:06

less complicated line creation

e7263c8

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

styling

457ca95

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

topic naming issue

a58eb9c

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

renamind and topic name parametiraztion alongside small changes.

175e720

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

Update nav2_costmap_2d/include/nav2_costmap_2d/tracking_bounds_layer.hpp

d9f7821

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: silanus <berkantali23@outlook.com>

copilot reviews: adding doxygen and adding costmap mutex

a24b52c

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

fixing tests

fc0ebe5

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

readding bresenham.

8a9dc31

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

renaming

7b6d489

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

typos

8a82f97

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

wall thickness parameterization

308b361

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

silanus23 force-pushed the tracking_error_layer branch from 52b0dba to 308b361 Compare December 13, 2025 21:06

silanus23 added 3 commits December 14, 2025 03:46

styling and testing

1802830

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

linting

17b0e18

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

making tf faster, long term usage test corrected, general code quali…

fe89b10

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

		void TrackingErrorLayer::activate() {enabled_ = true;}
		void TrackingErrorLayer::deactivate() {enabled_ = false;}

		last_path_.poses.clear();
		last_path_.header = std_msgs::msg::Header();

	last_path_.poses.clear();
	last_path_.header = std_msgs::msg::Header();
	last_path_ = nav_msgs::msg::Path();

Tracking error layer #5605

Are you sure you want to change the base?

Tracking error layer #5605

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

Note:

For Maintainers:

SteveMacenski left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

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

Uh oh!

SteveMacenski commented Dec 5, 2025

SteveMacenski commented Dec 5, 2025

SteveMacenski left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

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

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

Uh oh!

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Uh oh!

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

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Key Changes

Reviewed changes

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Copilot AI Dec 8, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Dec 8, 2025

Choose a reason for hiding this comment

SteveMacenski commented Dec 8, 2025

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

Uh oh!

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

Uh oh!

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

Uh oh!

silanus23 commented Dec 9, 2025

SteveMacenski commented Dec 9, 2025

mergify bot commented Dec 12, 2025

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

Uh oh!

SteveMacenski commented Dec 12, 2025

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

Uh oh!

Codecov Report

mini-1235 commented Dec 22, 2025

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

Uh oh!

Labels

silanus23 commented Oct 13, 2025 •

edited

Loading

silanus23 commented Dec 5, 2025 •

edited

Loading

silanus23 Dec 5, 2025 •

edited

Loading

silanus23 commented Dec 8, 2025 •

edited

Loading

SteveMacenski commented Dec 9, 2025 •

edited

Loading

silanus23 commented Dec 9, 2025 •

edited

Loading

SteveMacenski commented Dec 9, 2025 •

edited

Loading

silanus23 commented Dec 12, 2025 •

edited

Loading

codecov bot commented Dec 15, 2025 •

edited

Loading

silanus23 commented Dec 22, 2025 •

edited

Loading