Add isWithinPathTrackingBounds node#5983
Conversation
Co-authored-by: silanus23 <berkantali23@outlook.com> Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
Feb.24.2026.Video.mp4For reference, I am setting the position error to 0.2, and it immediately fails if the error is >= 0.2 |
| <root BTCPP_format="4" main_tree_to_execute="NavigateToPoseWBoundsCheck"> | ||
| <BehaviorTree ID="NavigateToPoseWBoundsCheck"> | ||
| <Sequence> | ||
| <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/> |
There was a problem hiding this comment.
What other details do you think we should include here? In this kind of use case, do people typically plan only once / replan only when an obstacle is detected / or replan continuously like in the default behavior tree?
There was a problem hiding this comment.
I think it's worth thinking of VDA5050 connection in here. As far as I know it's planning system waits for the message and then resend a new goal. In addition the previous PR it has been decided as just fail OK.
Other than these when obstacle detected it would better replan for the sake of smaller tracking errors
There was a problem hiding this comment.
I think failing the navigation task makes sense for demo purposes. This is a small demo tree anyway :-)
I actually have use for this with a client immediately in that way!
Codecov Report✅ All modified and coverable lines are covered by tests.
... and 23 files with indirect coverage changes 🚀 New features to boost your workflow:
|
Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
...ee/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp
Outdated
Show resolved
Hide resolved
| getInput<double>("max_position_error_left", max_position_error_left_); | ||
| getInput<double>("max_position_error_right", max_position_error_right_); | ||
| getInput<double>("max_heading_error", max_heading_error_); | ||
| getInput<nav2_msgs::msg::TrackingFeedback>("tracking_feedback", tracking_feedback_); |
There was a problem hiding this comment.
Shouldn't we get this each tick? I think this makes sense in the main tick function. I think this may tick anyway each iteration but it would be good to make this clear.
We should also check the case that the tracking_error isn't set yet at all
There was a problem hiding this comment.
My bad, brain fart :)
There was a problem hiding this comment.
We should also check the case that the tracking_error isn't set yet at all
Not sure if we need to explicitly check for that and log an error / return failure if tracking_error hasn't been set yet. If we did, I would likely need to skip the first tick of this node in the demo, since FollowPath is ticked after the condition node. I think it may be reasonable to treat it as an empty tracking feedback instead (i.e., 0 position error and 0 heading error) (?)
There was a problem hiding this comment.
I think it would be good to at least make that an explicit policy, ie
// If not set yet (like on starting to track), pass as initialization should always be within tracking bounds
if (!getInput<nav2_msgs::msg::TrackingFeedback>("tracking_feedback", tracking_feedback_)) {
return SUCCESS;
}
That way someone in the future can argue with it if they disagree. Right now that is hidden
nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp
Outdated
Show resolved
Hide resolved
| <root BTCPP_format="4" main_tree_to_execute="NavigateToPoseWBoundsCheck"> | ||
| <BehaviorTree ID="NavigateToPoseWBoundsCheck"> | ||
| <Sequence> | ||
| <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/> |
There was a problem hiding this comment.
I think failing the navigation task makes sense for demo purposes. This is a small demo tree anyway :-)
I actually have use for this with a client immediately in that way!
Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
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:
backport-*.