Skip to content

Path trackin conditon node#5602

Closed
silanus23 wants to merge 19 commits intoros-navigation:mainfrom
silanus23:path-trackin-conditon-node
Closed

Path trackin conditon node#5602
silanus23 wants to merge 19 commits intoros-navigation:mainfrom
silanus23:path-trackin-conditon-node

Conversation

@silanus23
Copy link
Contributor

@silanus23 silanus23 commented Oct 13, 2025


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 test
Was this PR description generated by AI software? nope

Description of contribution in a few bullet points

Description of documentation updates required from your changes

BT node addition
max_error parameter
If there is a document for it maybe upcoming xml

Description of how this change was tested

I tried to take inspiration from is_battery_low condition node. I created unit tests too.


Future work that may be required in bullet points

  • There is an upcoming xml I will put this in
<RateController hz="3.0">
          <Fallback name="PathTrackingRecoveryPlanner">
            <IsWithinPathTrackingBounds max_tracking_error="2.0" />
            <Fallback name="TieredReplanning">
              <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
              <Sequence>
                <ClearEntireCostmap name="ClearGlobalCostmap-Tier2" service_name="global_costmap/clear_entirely_global_costmap"/>
                <RetryUntilSuccessful num_attempts="1">
                  <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
                </RetryUntilSuccessful>
              </Sequence>
            </Fallback>
          </Fallback>
        </RateController>

place this in navigating path through pose xml.

Note:

I had to take out every file with copy paste to a new branch. Cause I had created this branch as a sub branch of the last messy branch. This was the only clean choice that can save me.
I think this will solve my cycling comments and confused commits problem.

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

@mergify
Copy link
Contributor

mergify bot commented Oct 13, 2025

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

<input_port name="battery_topic">Topic for battery info</input_port>
</Condition>

<Decorator ID="IsWithinPathTrackingBounds">
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

For docs: this needs to be having its configuration guide page + add to Navigation Plugins table + migration guide with the larger feature description

@silanus23 silanus23 force-pushed the path-trackin-conditon-node branch from 61389fd to 3ea85d8 Compare October 24, 2025 10:11
@codecov
Copy link

codecov bot commented Oct 24, 2025

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines Coverage Δ
...ition/is_within_path_tracking_bounds_condition.hpp 100.00% <100.00%> (ø)
...ition/is_within_path_tracking_bounds_condition.cpp 100.00% <100.00%> (ø)

... and 18 files with indirect coverage changes

🚀 New features to boost your workflow:
  • ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

My sincere apologoes for the gross delay - ROSCon + vacation + time to get back to speed meant that some things didn't get my time until today. Overall this looks great and its just small quibbles and we should be able to merge this soon!

Can this be moved from a draft? Did you test this to work well?

For docs: this needs to be having its configuration guide page + add to Navigation Plugins table + migration guide with the larger feature description



+   if (!getInput("max_error_left", max_error_left_)) {
+     RCLCPP_ERROR(logger_, "max_error_left parameter not provided");
+     return BT::NodeStatus::FAILURE;
+   }
+ 
+   if (max_error_left_ < 0.0) {
+     RCLCPP_WARN(logger_, "max_error_left should be positive, using absolute value");
+     max_error_left_ = std::abs(max_error_left_);
+   }
+ 
+   if (!getInput("max_error_right", max_error_right_)) {
+     RCLCPP_ERROR(logger_, "max_error_right parameter not provided");
+     return BT::NodeStatus::FAILURE;
+   }
+ 
+   if (max_error_right_ < 0.0) {
+     RCLCPP_WARN(logger_, "max_error_right should be positive, using absolute value");
+     max_error_right_ = std::abs(max_error_right_);
+   }

These lines should be covered in tests

: BT::ConditionNode(condition_name, conf),
last_error_(std::numeric_limits<double>::max())
{
auto node = config().blackboard->get<nav2::LifecycleNode::SharedPtr>("node");
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Set logger_ = node->get_logger();

rclcpp::Logger logger_{rclcpp::get_logger("is_within_path_tracking_bounds_node")};
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
rclcpp::Subscription<nav2_msgs::msg::TrackingFeedback>::SharedPtr tracking_feedback_sub_;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Use nav2::Subscription

};
}

private:
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
private:
protected:

double max_error_left_{1.5};
std::chrono::milliseconds bt_loop_duration_;

void trackingFeedbackCallback(const nav2_msgs::msg::TrackingFeedback::SharedPtr msg);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Move this before the variables; all methods before all variables please. Also needs doxygen documentation for the API

@silanus23
Copy link
Contributor Author

@SteveMacenski I hope ROSCon went well. I’d love to join one myself when I get the chance. I’ve made some changes in this and other PRs, but I’m having trouble testing them right now cause the costmap keeps crashing after launch. I’m not sure yet whether it’s my local setup or something in the repo. I’ll push the updates as soon as I can get things running again. BTW I gave a shot about to add MPPI critic too. I will open it's PR too.

@SteveMacenski
Copy link
Member

Great! I didn't see any update in other PRs, but I assume that's coming soon. I'm passing off the initial reviews here to @mini-1235 since this is related to one of his tasks, but I'll follow up for a final review once he approves

@silanus23 silanus23 marked this pull request as ready for review December 5, 2025 12:08
Copilot AI review requested due to automatic review settings December 5, 2025 12:08
Copy link
Contributor

Copilot AI left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Pull request overview

This PR adds a new BT condition node IsWithinPathTrackingBounds to monitor whether a robot's deviation from its planned path exceeds configurable bounds.

Key Changes:

  • Introduces IsWithinPathTrackingBoundsCondition BT node that subscribes to tracking_feedback topic to check if the robot is within acceptable tracking error bounds
  • Supports asymmetric bounds (separate thresholds for left and right deviations)
  • Includes comprehensive unit tests covering various scenarios (symmetric/asymmetric bounds, edge cases, missing parameters)

Reviewed changes

Copilot reviewed 7 out of 7 changed files in this pull request and generated 17 comments.

Show a summary per file
File Description
nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery_w_path_hug.xml Example behavior tree XML demonstrating usage of the new condition node with path tracking recovery and replanning
nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp Header file defining the IsWithinPathTrackingBoundsCondition class with subscription to tracking feedback
nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp Implementation of the condition node with parameter validation and asymmetric bound checking
nav2_behavior_tree/test/plugins/condition/test_is_within_path_tracking_bounds.cpp Comprehensive unit tests covering various scenarios including symmetric/asymmetric bounds, edge cases, and missing parameters
nav2_behavior_tree/test/plugins/condition/CMakeLists.txt Build configuration to include the new test executable
nav2_behavior_tree/nav2_tree_nodes.xml Registration of the new BT node for Groot and XML-based behavior trees
nav2_behavior_tree/CMakeLists.txt Build configuration to compile the new condition node plugin

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

Comment on lines +301 to +333
TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_missing_max_error_left)
{
// Test behavior when max_error_left parameter is not provided
std::string xml_txt =
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<IsWithinPathTrackingBounds max_error_right="1.0"/>
</BehaviorTree>
</root>)";

auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);

publishAndSpin(0.5);
EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
}

TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_missing_max_error_right)
{
// Test behavior when max_error_right parameter is not provided
std::string xml_txt =
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<IsWithinPathTrackingBounds max_error_left="1.0"/>
</BehaviorTree>
</root>)";

auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);

publishAndSpin(-0.5);
EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
}
Copy link

Copilot AI Dec 5, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[nitpick] Incomplete test coverage: The tests test_missing_max_error_left and test_missing_max_error_right verify that FAILURE is returned, but they don't verify that the expected error messages are logged. The implementation logs "max_error_left parameter not provided" and "max_error_right parameter not provided" errors, but these aren't validated in the tests.

Copilot uses AI. Check for mistakes.
@mergify
Copy link
Contributor

mergify bot commented Dec 5, 2025

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

@mini-1235
Copy link
Collaborator

@silanus23 can you pull in / rebase main, we had some CI issues that should be fixed now

@silanus23 silanus23 force-pushed the path-trackin-conditon-node branch from 4972a27 to 361035a Compare December 5, 2025 13:22
@SteveMacenski SteveMacenski removed their request for review December 5, 2025 18:29
@mini-1235
Copy link
Collaborator

@silanus23 can you open a docs PR in https://github.com/ros-navigation/docs.nav2.org? I am fixing the CI today, once it is fixed, I will review this PR again

Copy link
Collaborator

@mini-1235 mini-1235 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@silanus23 Can you pull in / rebase main again? I think the build failure should be fixed now

@@ -0,0 +1,69 @@

<!--
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this should be reverted, but we could provide a minimal example in the migration guide

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think it would be good to have a xml demo of using this BT node, but maybe for now doing something more basic like after the FollowPath node, check if its not within bounds. If so, log something. I'm not sure right now what good 'generic' recovery action I would recommend if it is required to be in bounds but is now found to be out of bounds (replan? not sure that's the intent. clear costmaps or rotate? not going to help). Fail navigation? actually that might make sense to trigger a total task failure since a core condition is breached. Also would make a cool BT XML demo to show that since I don't think we have any that do that yet!

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I agree that failing the navigation task makes sense. A recovery that tries to pull the robot back in bounds might also be a good idea, but that needs some brainstorming. We can revisit the idea once we start working on #2599

Copy link
Member

@SteveMacenski SteveMacenski Dec 10, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe worth ~10 minutes to think about some ideas. If there's not something obvious / generic, I think failing navigation is a good enough demo and be in line with what I think some users would want (a loud notification that this constraint is breached to record, recover from manually, and restart). I don't know if BT.CPP has a "throw exception" BT node / capability or we'd have to:

  1. Create an Action node version of this condition node that validates bounds or throws/fails. Maybe FAILURE is enough with the BT design.
  2. Create an ExceptionThrow Action BT node that we can use the conditional in concert with (and might actually have pretty good general purpose utility - have the input port be the exception string to throw)

Else, another option is that we bake this into the controller server with a new error code / exception for the bounds breaching for that to be propagated to the NavigateToPose / NavigateThroughPoses caller. The thresholded limits left/right would then ideally need to be on a per-path basis, but we could start with parameterized limits as well. Those would need to be propagated from the behavior tree anyway, so maybe handling it in the BT is more flexible. That does beg though if we need to add this limit to the NavigateToPose / NavigateThroughPoses actions

Maybe this is a big enough topic to discuss in the main ticket thread again

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think the closest concept in BTCPP for implementing an emergency stop is using halt/halttree. I like the idea of creating an ExceptionThrow Action BT Node, but I am not entirely sure whether that really fits the BTCPP design philosophy, I will need to think about that again.

I think more commonly, the pattern is to place the node at the top level and have it return failure + error code when it is not within the bound.

return BT::NodeStatus::FAILURE;
}

if (current_error > 0.0) { // Positive = left side
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think we need to store the variable current_error / last_error here, instead we can have a variable that checks if this is within path tracking bounds. In every tick, we only need to check this variable


callback_group_executor_.spin_all(bt_loop_duration_);

if (!getInput("max_error_left", max_error_left_)) {
Copy link
Collaborator

@mini-1235 mini-1235 Dec 9, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

For consistency with other nodes, I don't think the check here is needed
Or move it to initialize

return BT::NodeStatus::FAILURE;
}

if (max_error_left_ < 0.0) {
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This should be moved to initialize, I think

"tracking_feedback",
std::bind(&IsWithinPathTrackingBoundsCondition::trackingFeedbackCallback, this,
std::placeholders::_1),
rclcpp::SystemDefaultsQoS(),
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We don't use rclcpp::SystemDefaultsQoS, please check other nodes

@silanus23 silanus23 requested a review from mini-1235 December 9, 2025 18:17
bt_loop_duration_ =
config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");

RCLCPP_INFO(logger_, "Initialized IsWithinPathTrackingBoundsCondition BT node");
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove the log please

}
}

void IsWithinPathTrackingBoundsCondition::initialize()
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can we move this before the subscription callback

rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
nav2::Subscription<nav2_msgs::msg::TrackingFeedback>::SharedPtr tracking_feedback_sub_;
bool is_within_bounds_{false};
double max_error_right_{1.5};
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I guess we should remove the default value here

@mini-1235
Copy link
Collaborator

Thanks for fixing this, this looks much better IMO. I have one last question before I ask Steve for a final review:

I saw this tree in the XML demo and also in the migration guide:

<RateController hz="3.0">
      <Fallback name="PathTrackingRecoveryPlanner">
        <IsWithinPathTrackingBounds max_error_left="2.0" max_error_right="2.0" />
        <Fallback name="TieredReplanning">
          <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
          <Sequence>
            <ClearEntireCostmap name="ClearGlobalCostmap-Tier2" service_name="global_costmap/clear_entirely_global_costmap"/>
            <RetryUntilSuccessful num_attempts="1">
              <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
            </RetryUntilSuccessful>
          </Sequence>
        </Fallback>
      </Fallback>
    </RateController>

Could you explain this a bit? I am not sure I understand the idea behind it.

@silanus23
Copy link
Contributor Author

@mini-1235 Sorry for late response. Reading above conversations I think I misunderstood concept a bit. I was trying to create a beh. tree that controls robot at 3 Hz. and replans if out bounds. As I understand this requires some other work too I will delete it in the next commit (as soon as I get to chance) and try to implement it with another PR. What is your advice on this?

@mini-1235
Copy link
Collaborator

I don't have a strong preference on whether the demo XML should be added in this PR or a follow-up one, so I will leave that decision to @SteveMacenski. But I would like to request including a minimal example in the migration guide so users know how to use this new node :)

@SteveMacenski
Copy link
Member

For now, a demo could be using the condition node to log the event if out of bounds. Maybe publish out a topic indicating the event.

@mini-1235
Copy link
Collaborator

@silanus23, we discussed this today. In short, we would like the demo to explicitly show a navigation failure when the robot goes out of bounds. It can be a very basic example, similar to other demos in that directory.

Could you please revisit #5602 (comment) and revise the demo accordingly?

@mini-1235
Copy link
Collaborator

@silanus23, could you please show/explain how the navigation fails when the robot goes out of bounds? A short video would be especially helpful!

@silanus23
Copy link
Contributor Author

silanus23 commented Dec 16, 2025

@mini-1235 I will handle the test problem and send a video where robot stops moving with a log. In addition maybe an extra explanation in docs can help.

@mini-1235
Copy link
Collaborator

@mini-1235 I will handle the test problem and send a video where robot stops moving with a log. In addition maybe an extra explanation in docs can help.

Sounds good, thanks!

}
}

void IsWithinPathTrackingBoundsCondition::trackingFeedbackCallback(
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Per #5804, this should be updated to ConstSharedPtr

@mini-1235
Copy link
Collaborator

In #5894, I actually added heading_tracking_error in addition to position_tracking_error. I think this could also be included in this PR, as it can be useful especially in cases where the robot never reverse

silanus23 and others added 19 commits January 29, 2026 10:33
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>
…n/is_within_path_tracking_bounds_condition.hpp

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

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Signed-off-by: silanus <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 silanus23 force-pushed the path-trackin-conditon-node branch from c568266 to 2f51af5 Compare January 30, 2026 14:56
@mini-1235
Copy link
Collaborator

Hey @silanus23, I will take over this PR + docs PR and add you as a co-author so we can move things forward more quickly

@mini-1235 mini-1235 closed this Feb 24, 2026
@silanus23
Copy link
Contributor Author

@mini-1235 OK I have pushed my last local changes to the branch. Can move forward from there.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

4 participants