Path trackin conditon node#5602
Conversation
|
@silanus23, your PR has failed to build. Please check CI outputs and resolve issues. |
...ee/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp
Outdated
Show resolved
Hide resolved
| <input_port name="battery_topic">Topic for battery info</input_port> | ||
| </Condition> | ||
|
|
||
| <Decorator ID="IsWithinPathTrackingBounds"> |
There was a problem hiding this comment.
For docs: this needs to be having its configuration guide page + add to Navigation Plugins table + migration guide with the larger feature description
nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp
Outdated
Show resolved
Hide resolved
nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp
Outdated
Show resolved
Hide resolved
nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp
Outdated
Show resolved
Hide resolved
nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp
Outdated
Show resolved
Hide resolved
nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp
Outdated
Show resolved
Hide resolved
nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp
Outdated
Show resolved
Hide resolved
61389fd to
3ea85d8
Compare
Codecov Report✅ All modified and coverable lines are covered by tests.
... and 18 files with indirect coverage changes 🚀 New features to boost your workflow:
|
There was a problem hiding this comment.
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"); |
There was a problem hiding this comment.
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_; |
| }; | ||
| } | ||
|
|
||
| private: |
There was a problem hiding this comment.
| private: | |
| protected: |
| double max_error_left_{1.5}; | ||
| std::chrono::milliseconds bt_loop_duration_; | ||
|
|
||
| void trackingFeedbackCallback(const nav2_msgs::msg::TrackingFeedback::SharedPtr msg); |
There was a problem hiding this comment.
Move this before the variables; all methods before all variables please. Also needs doxygen documentation for the API
|
@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. |
|
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 |
There was a problem hiding this comment.
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
IsWithinPathTrackingBoundsConditionBT node that subscribes totracking_feedbacktopic 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.
nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery_w_path_hug.xml
Outdated
Show resolved
Hide resolved
...ee/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp
Outdated
Show resolved
Hide resolved
...ee/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp
Outdated
Show resolved
Hide resolved
...ee/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp
Outdated
Show resolved
Hide resolved
nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery_w_path_hug.xml
Outdated
Show resolved
Hide resolved
| 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); | ||
| } |
There was a problem hiding this comment.
[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.
...ee/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp
Outdated
Show resolved
Hide resolved
...ee/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp
Outdated
Show resolved
Hide resolved
...ee/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp
Outdated
Show resolved
Hide resolved
|
@silanus23, your PR has failed to build. Please check CI outputs and resolve issues. |
|
@silanus23 can you pull in / rebase main, we had some CI issues that should be fixed now |
4972a27 to
361035a
Compare
|
@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 |
mini-1235
left a comment
There was a problem hiding this comment.
@silanus23 Can you pull in / rebase main again? I think the build failure should be fixed now
| @@ -0,0 +1,69 @@ | |||
|
|
|||
| <!-- | |||
There was a problem hiding this comment.
I think this should be reverted, but we could provide a minimal example in the migration guide
There was a problem hiding this comment.
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!
There was a problem hiding this comment.
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
There was a problem hiding this comment.
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:
- Create an Action node version of this condition node that validates bounds or throws/fails. Maybe FAILURE is enough with the BT design.
- 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
There was a problem hiding this comment.
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 |
There was a problem hiding this comment.
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_)) { |
There was a problem hiding this comment.
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) { |
There was a problem hiding this comment.
This should be moved to initialize, I think
| "tracking_feedback", | ||
| std::bind(&IsWithinPathTrackingBoundsCondition::trackingFeedbackCallback, this, | ||
| std::placeholders::_1), | ||
| rclcpp::SystemDefaultsQoS(), |
There was a problem hiding this comment.
We don't use rclcpp::SystemDefaultsQoS, please check other nodes
| bt_loop_duration_ = | ||
| config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration"); | ||
|
|
||
| RCLCPP_INFO(logger_, "Initialized IsWithinPathTrackingBoundsCondition BT node"); |
| } | ||
| } | ||
|
|
||
| void IsWithinPathTrackingBoundsCondition::initialize() |
There was a problem hiding this comment.
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}; |
There was a problem hiding this comment.
I guess we should remove the default value here
|
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: Could you explain this a bit? I am not sure I understand the idea behind it. |
|
@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? |
|
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 :) |
|
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. |
|
@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? |
|
@silanus23, could you please show/explain how the navigation fails when the robot goes out of bounds? A short video would be especially helpful! |
|
@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( |
There was a problem hiding this comment.
Per #5804, this should be updated to ConstSharedPtr
|
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 |
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>
c568266 to
2f51af5
Compare
|
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 OK I have pushed my last local changes to the branch. Can move forward from there. |
Basic Info
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
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: