Tracking error layer#5605
Conversation
SteveMacenski
left a comment
SteveMacenski
left a comment
There was a problem hiding this comment.
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
Layerrather thanCostmapLayersince we shouldn't really need the internal costmap structure. - We default assume all points are lethal unless proven otherwise in the
for eachloop 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 :-)
| this, std::placeholders::_1)); | ||
|
|
||
| path_sub_ = node->create_subscription<nav_msgs::msg::Path>( | ||
| "/plan", |
There was a problem hiding this comment.
Remove forward slash in both subscriptions
| tracking_feedback_sub_ = node->create_subscription<nav2_msgs::msg::TrackingFeedback>( | ||
| "/tracking_feedback", | ||
| std::bind(&TrackingErrorLayer::trackingCallback, this, std::placeholders::_1), | ||
| rclcpp::QoS(10).reliable() |
There was a problem hiding this comment.
For all QoS: Use the established Nav2 policies in nav2_ros_common
| std::bind(&TrackingErrorLayer::trackingCallback, this, std::placeholders::_1), | ||
| rclcpp::QoS(10).reliable() | ||
| ); | ||
| tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node->get_clock()); |
There was a problem hiding this comment.
You should have this already from base classes to this class. You shouldn't need to manually create another (which is very heavy)
| std::mutex path_mutex_; | ||
| std::mutex tracking_error_mutex_; |
There was a problem hiding this comment.
I think you can get away with a single data mutex
| return result; | ||
| } | ||
|
|
||
| TrackingErrorLayer::~TrackingErrorLayer() |
There was a problem hiding this comment.
Move destructor right after the constructor
| void TrackingErrorLayer::reset() {} | ||
| void TrackingErrorLayer::activate() {enabled_ = true;} | ||
| void TrackingErrorLayer::deactivate() {enabled_ = false;} | ||
|
|
||
| void TrackingErrorLayer::onFootprintChanged() {} | ||
| void TrackingErrorLayer::cleanup() {} |
There was a problem hiding this comment.
Dont override if not implementing
| void TrackingErrorLayer::activate() {enabled_ = true;} | ||
| void TrackingErrorLayer::deactivate() {enabled_ = false;} |
There was a problem hiding this comment.
These two should create / destroy the subscriptions
| dyn_params_handler_.reset(); | ||
| } | ||
|
|
||
| void TrackingErrorLayer::reset() {} |
There was a problem hiding this comment.
This should reset data and other states. Please check out other layers for example
silanus23
force-pushed
the
tracking_error_layer
branch
2 times, most recently
from
936f1e4 to
97ed4c8
Compare
|
@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. |
|
Related to your other PR, I'm passing off the first review stage to @mini-1235 and I'll come in afterwards |
Can you show an image of this behavior for example? |
SteveMacenski
left a comment
SteveMacenski
left a comment
There was a problem hiding this comment.
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
| <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"> |
There was a problem hiding this comment.
Maybe rename to BoundedTrackingErrorLayer since its adding bounds
| namespace nav2_costmap_2d | ||
| { | ||
|
|
||
| TrackingErrorLayer::TrackingErrorLayer() {} |
There was a problem hiding this comment.
You can just set this to TrackingErrorLayer() = default; in the header file instead
| void TrackingErrorLayer::trackingCallback( | ||
| const nav2_msgs::msg::TrackingFeedback::SharedPtr msg) | ||
| { | ||
| std::lock_guard<std::mutex> lock(data_mutex_); | ||
| last_tracking_feedback_ = *msg; | ||
| } | ||
|
|
There was a problem hiding this comment.
It would be good to check on timestamps to make sure these are sufficiently current
|
|
||
| // Create subscriptions when layer is activated | ||
| path_sub_ = node->create_subscription<nav_msgs::msg::Path>( | ||
| "/plan", |
There was a problem hiding this comment.
Remove forward slashes here and in the tracking feedback to let this work with namespaces
There was a problem hiding this comment.
I have tried without "/" but didn't work. It wasn't subscribing to those topics. Am I doing anything else wrong about this?
There was a problem hiding this comment.
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
| last_path_.poses.clear(); | ||
| last_path_.header = std_msgs::msg::Header(); |
There was a problem hiding this comment.
| last_path_.poses.clear(); | |
| last_path_.header = std_msgs::msg::Header(); | |
| last_path_ = nav_msgs::msg::Path(); |
@SteveMacenski currently looks like this. with bresenham function it was a smooth straight line. |
There was a problem hiding this comment.
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 |
| 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; |
There was a problem hiding this comment.
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()| enabled: True | ||
| look_ahead: 5.0 | ||
| step: 5 | ||
| corridor_width: 2.0 |
There was a problem hiding this comment.
[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"| corridor_width: 2.0 | |
| corridor_width: 2.0 | |
| tracking_feedback_topic: "tracking_feedback" | |
| path_topic: "plan" |
This doesn't look very dense / complete (?). I see large gaps there.
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. |
|
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 |
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. |
|
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 |
|
Should I parameterize it? |
|
I think that might be good. Default at 2 though I think would be good. |
|
This pull request is in conflict. Could you fix it @silanus23? |
|
@SteveMacenski I will solve conflict and open a doc. About computational weight I can bring actually numbers with |
|
@mini-1235 is taking over the first set of reviews on this feature - please direct to him for now :-) |
silanus23
force-pushed
the
tracking_error_layer
branch
from
52b0dba to
308b361
Compare
Codecov Report❌ Patch coverage is
... and 11 files with indirect coverage changes 🚀 New features to boost your workflow:
|
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 |
|
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. :-) |
silanus23
force-pushed
the
tracking_error_layer
branch
from
fe89b10 to
3fd5bca
Compare
|
silanus23
force-pushed
the
tracking_error_layer
branch
from
3fd5bca to
74d4d46
Compare
Signed-off-by: silanus23 <berkantali23@outlook.com>
Signed-off-by: silanus23 <berkantali23@outlook.com>
… refactor applyFillOutside func Signed-off-by: silanus23 <berkantali23@outlook.com>
Signed-off-by: silanus23 <berkantali23@outlook.com>
Signed-off-by: silanus23 <berkantali23@outlook.com>
Signed-off-by: silanus23 <berkantali23@outlook.com>
Signed-off-by: silanus23 <berkantali23@outlook.com>
Signed-off-by: silanus23 <berkantali23@outlook.com>
Signed-off-by: silanus23 <berkantali23@outlook.com>
Signed-off-by: silanus23 <berkantali23@outlook.com>
Signed-off-by: silanus23 <berkantali23@outlook.com>
Signed-off-by: silanus23 <berkantali23@outlook.com>
Signed-off-by: silanus23 <berkantali23@outlook.com>
Signed-off-by: silanus23 <berkantali23@outlook.com>
Signed-off-by: silanus23 <berkantali23@outlook.com>
silanus23
force-pushed
the
tracking_error_layer
branch
from
3d81919 to
8b951d9
Compare
|
@mini-1235 relevant code snippet // costmap_2d_ros.cpp :: Costmap2DROS::mapUpdateLoop()
nav2::Rate r(this, frequency);
while (rclcpp::ok() && !map_update_thread_shutdown_) {
if (!stopped_) {
std::scoped_lock<std::mutex> lock(_dynamic_parameter_mutex);
updateMap();
...
}
// Make sure to sleep for the remainder of our cycle time
r.sleep();
}A layer has no way to insert itself into this cycle independently. To make the layer more up to date I could come up with these 3 choices:
About path age: I made it distance based. It doesn't solve a major problem but there are 2 reasons:
If there is any other way I'm not seeing about age check and frequency, I'm open to suggestions but I really can't find much more. |
That's not what I meant, do you see a similar "delay" in other layers? From the previous videos, we can clearly see the plan is being replanned multiple times with very large deviations due to this delay. In reality, we can have many combinations of costmap update rate + plan rate, and we are already seeing issues with the default values. I would still argue this is something we need to solve. As you mentioned this issue might be specific to low costmap update rates, here is what I would suggest: try lowering the update rate on the local costmap first. If you see the same result, then maybe your assumption holds, and we would need to think about how to handle it from there. Overall, looking at the video, I don't think we have really achieved the desired result yet, but I don't have a complete solution off the top of my head without taking this over and working on it myself On the "path age" thing, it doesn't immediately strike me as necessary, but it doesn't hurt either, so I think you can keep it. I would however rename it to ps: I also went back and rewatched all your videos, looks like the last two only show the result after a single plan, which is different from the earlier videos you showed me that made me think there was an issue |
|
https://drive.google.com/file/d/1B75-0bWF8P82O9EdJskl8rGnK9INIh5R/view?usp=drive_link I sent the video to assure you about current situation. It can handle multiple mid plan changes still.
I think now I can understand the problem. multiple replans etc. is happenning in goal changes. The replanning deviations happen specifically at goal change, not during normal navigation. We allready have a goal change tracker. We should 0 out weights for 1 plan cycle. Wait till new plan comes after cost updated so with this way we are giving planner a breathing room to create plans without weights of this layer. I will try to send video of this today. I sent this message to confirm solution and assurance about current situation. Edit: For plan once scenario I will put a parameterized fallback timer. |
…d circle anchor fixes, race fixes Signed-off-by: silanus23 <berkantali23@outlook.com>
Signed-off-by: silanus23 <berkantali23@outlook.com>
Signed-off-by: silanus23 <berkantali23@outlook.com>
Signed-off-by: silanus23 <berkantali23@outlook.com>
Signed-off-by: silanus23 <berkantali23@outlook.com>
|
https://drive.google.com/file/d/1d-epKvXq2fZ2964wtnlFlQNQF2CN4_ya/view?usp=drive_link I have implemented above refactor that I have sent. In addition coming after some time with fresh eyes I have caught some necessary changes in styling. This is a frequency mismatch issue rather than direct delay inside layer. To make this work regardless of frequency combination I suppress weights whenever there is a goal change. It goes like:
In addition to this for plan once scenarios I have put a timer fallback. Still the first plan is going to be with old costmap version but that's inevitable since planners triggered by new goals but costmap updates with a constant frequency. Is current version acceptable? Edit: just in case curving back itself a necessary edge case for you. |
|
Dear nav2 crew @mini-1235 @SteveMacenski : |
|
@mini-1235 Just pinging to let you know I'm back online. I'm ready to address reviews whenever you have a chance to take a look. |
Basic Info
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:
backport-*.