From a16f0ee2c234879171e7ea5a41796e8a579a67fa Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 13 Oct 2025 17:00:24 +0300 Subject: [PATCH 1/7] header added Signed-off-by: silanus23 --- ..._within_path_tracking_bounds_condition.hpp | 72 +++++++++++++++++++ 1 file changed, 72 insertions(+) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp new file mode 100644 index 00000000000..59db20e3f55 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp @@ -0,0 +1,72 @@ +// Copyright (c) 2025 Berkan Tali +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_WITHIN_PATH_TRACKING_BOUNDS_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_WITHIN_PATH_TRACKING_BOUNDS_CONDITION_HPP_ + +#include +#include +#include + +#include "behaviortree_cpp/condition_node.h" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_msgs/msg/tracking_feedback.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ConditionNode that subscribes to /tracking_feedback and returns SUCCESS + * if the error is within the max_error input port, FAILURE otherwise + */ +class IsWithinPathTrackingBoundsCondition : public BT::ConditionNode +{ +public: + IsWithinPathTrackingBoundsCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + IsWithinPathTrackingBoundsCondition() = delete; + ~IsWithinPathTrackingBoundsCondition() override = default; + + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + + BT::NodeStatus tick() override; + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("max_error", 0.5, "Maximum allowed tracking error") + }; + } + +private: + nav2::LifecycleNode::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + rclcpp::Subscription::SharedPtr tracking_feedback_sub_; + double last_error_{0.0}; + double max_error_{1.5}; + std::chrono::milliseconds bt_loop_duration_; + + void trackingFeedbackCallback(const nav2_msgs::msg::TrackingFeedback::SharedPtr msg); +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_WITHIN_PATH_TRACKING_BOUNDS_CONDITION_HPP_ From 242545275ba177e98d014c5a4d0a4db8b11b1358 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 13 Oct 2025 17:02:43 +0300 Subject: [PATCH 2/7] source file addition Signed-off-by: silanus23 --- ..._within_path_tracking_bounds_condition.cpp | 89 +++++++++++++++++++ 1 file changed, 89 insertions(+) create mode 100644 nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp diff --git a/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp b/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp new file mode 100644 index 00000000000..dd985a42b95 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp @@ -0,0 +1,89 @@ +// Copyright (c) 2025 Berkan Tali +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp" + +namespace nav2_behavior_tree +{ + +IsWithinPathTrackingBoundsCondition::IsWithinPathTrackingBoundsCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf), + last_error_(0.0) +{ + node_ = config().blackboard->get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + + tracking_feedback_sub_ = node_->create_subscription( + "/tracking_feedback", + std::bind(&IsWithinPathTrackingBoundsCondition::trackingFeedbackCallback, this, + std::placeholders::_1), + rclcpp::SystemDefaultsQoS(), + callback_group_); + + bt_loop_duration_ = + config().blackboard->template get("bt_loop_duration"); + + RCLCPP_DEBUG(node_->get_logger(), "Initialized IsWithinPathTrackingBoundsCondition BT node"); + RCLCPP_INFO_ONCE(node_->get_logger(), "Waiting for tracking error"); + initialize(); +} + +void IsWithinPathTrackingBoundsCondition::trackingFeedbackCallback( + const nav2_msgs::msg::TrackingFeedback::SharedPtr msg) +{ + last_error_ = msg->tracking_error; +} + +void IsWithinPathTrackingBoundsCondition::initialize() +{ + getInput("max_error", max_error_); +} + +BT::NodeStatus IsWithinPathTrackingBoundsCondition::tick() +{ + if (!BT::isStatusActive(status())) { + initialize(); + } + + callback_group_executor_.spin_all(bt_loop_duration_); + + if (!getInput("max_error", max_error_)) { + RCLCPP_ERROR(node_->get_logger(), "max_error parameter not provided"); + max_error_ = 1.0; // Default fallback + } + + if (max_error_ < 0.0) { + RCLCPP_WARN(node_->get_logger(), "max_error should be positive, using absolute value"); + max_error_ = std::abs(max_error_); + } + + if (last_error_ <= max_error_) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType( + "IsWithinPathTrackingBounds"); +} From a30356401193bda23658ce19a873cec68418d530 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 13 Oct 2025 17:03:04 +0300 Subject: [PATCH 3/7] test file added Signed-off-by: silanus23 --- .../test_is_within_path_tracking_bounds.cpp | 223 ++++++++++++++++++ 1 file changed, 223 insertions(+) create mode 100644 nav2_behavior_tree/test/plugins/condition/test_is_within_path_tracking_bounds.cpp diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_within_path_tracking_bounds.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_within_path_tracking_bounds.cpp new file mode 100644 index 00000000000..b48d329f45a --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_is_within_path_tracking_bounds.cpp @@ -0,0 +1,223 @@ +// Copyright (c) 2025 Berkan Tali +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "nav2_msgs/msg/tracking_feedback.hpp" + +#include "utils/test_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp" + +class IsWithinPathTrackingBoundsConditionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("test_is_within_path_tracking_bounds"); + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + + factory_->registerNodeType( + "IsWithinPathTrackingBounds"); + + tracking_feedback_pub_ = node_->create_publisher( + "/tracking_feedback", + rclcpp::SystemDefaultsQoS()); + tracking_feedback_pub_->on_activate(); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + tracking_feedback_pub_.reset(); + node_.reset(); + factory_.reset(); + executor_.reset(); + } + +protected: + static nav2::LifecycleNode::SharedPtr node_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static nav2::Publisher::SharedPtr tracking_feedback_pub_; +}; + +nav2::LifecycleNode::SharedPtr IsWithinPathTrackingBoundsConditionTestFixture::node_ = nullptr; +rclcpp::executors::SingleThreadedExecutor::SharedPtr +IsWithinPathTrackingBoundsConditionTestFixture::executor_ = nullptr; +BT::NodeConfiguration * IsWithinPathTrackingBoundsConditionTestFixture::config_ = nullptr; +std::shared_ptr IsWithinPathTrackingBoundsConditionTestFixture::factory_ = + nullptr; +nav2::Publisher::SharedPtr +IsWithinPathTrackingBoundsConditionTestFixture::tracking_feedback_pub_ = nullptr; + +TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_behavior_within_bounds) +{ + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + + // Test case 1: Error is within bounds (should return SUCCESS) + nav2_msgs::msg::TrackingFeedback tracking_feedback_msg; + tracking_feedback_msg.tracking_error = 0.5; + tracking_feedback_pub_->publish(tracking_feedback_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor_->spin_some(); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + + // Test case 2: Error is exactly at the boundary (should return SUCCESS) + tracking_feedback_msg.tracking_error = 1.0; + tracking_feedback_pub_->publish(tracking_feedback_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor_->spin_some(); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + + // Test case 3: Error exceeds bounds (should return FAILURE) + tracking_feedback_msg.tracking_error = 1.5; + tracking_feedback_pub_->publish(tracking_feedback_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor_->spin_some(); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); + + // Test case 4: Zero error (should return SUCCESS) + tracking_feedback_msg.tracking_error = 0.0; + tracking_feedback_pub_->publish(tracking_feedback_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor_->spin_some(); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); +} + +TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_behavior_different_max_error) +{ + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + + // Test case 1: Error is within smaller bounds (should return SUCCESS) + nav2_msgs::msg::TrackingFeedback tracking_feedback_msg; + tracking_feedback_msg.tracking_error = 0.1; + tracking_feedback_pub_->publish(tracking_feedback_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor_->spin_some(); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + + // Test case 2: Error exceeds smaller bounds (should return FAILURE) + tracking_feedback_msg.tracking_error = 0.3; + tracking_feedback_pub_->publish(tracking_feedback_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor_->spin_some(); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); +} + +TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_behavior_large_error_values) +{ + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + + // Test case 1: Large error within bounds (should return SUCCESS) + nav2_msgs::msg::TrackingFeedback tracking_feedback_msg; + tracking_feedback_msg.tracking_error = 4.9; + tracking_feedback_pub_->publish(tracking_feedback_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor_->spin_some(); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + + // Test case 2: Very large error exceeding bounds (should return FAILURE) + tracking_feedback_msg.tracking_error = 10.0; + tracking_feedback_pub_->publish(tracking_feedback_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor_->spin_some(); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); +} + +TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_behavior_edge_cases) +{ + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + + // Test case 1: Zero max_error with zero tracking error (should return SUCCESS) + nav2_msgs::msg::TrackingFeedback tracking_feedback_msg; + tracking_feedback_msg.tracking_error = 0.0; + tracking_feedback_pub_->publish(tracking_feedback_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor_->spin_some(); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + + // Test case 2: Zero max_error with any positive tracking error (should return FAILURE) + tracking_feedback_msg.tracking_error = 0.001; + tracking_feedback_pub_->publish(tracking_feedback_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor_->spin_some(); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} From 6b4e7f8b627df3ecf0336170940df2d0a4a0ea19 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 13 Oct 2025 17:22:32 +0300 Subject: [PATCH 4/7] Last additions Signed-off-by: silanus23 --- nav2_behavior_tree/CMakeLists.txt | 3 +++ nav2_behavior_tree/nav2_tree_nodes.xml | 4 ++++ .../condition/is_within_path_tracking_bounds_condition.cpp | 2 +- nav2_behavior_tree/test/plugins/condition/CMakeLists.txt | 3 +++ 4 files changed, 11 insertions(+), 1 deletion(-) diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 1827fb5665b..575337c61a4 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -156,6 +156,9 @@ list(APPEND plugin_libs nav2_would_a_smoother_recovery_help_condition_bt_node) add_library(nav2_would_a_route_recovery_help_condition_bt_node SHARED plugins/condition/would_a_route_recovery_help_condition.cpp) list(APPEND plugin_libs nav2_would_a_route_recovery_help_condition_bt_node) +add_library(nav2_is_within_path_tracking_bounds_condition_bt_node SHARED plugins/condition/is_within_path_tracking_bounds_condition.cpp) +list(APPEND plugin_libs nav2_is_within_path_tracking_bounds_condition_bt_node) + add_library(nav2_reinitialize_global_localization_service_bt_node SHARED plugins/action/reinitialize_global_localization_service.cpp) list(APPEND plugin_libs nav2_reinitialize_global_localization_service_bt_node) diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 7161f6419ce..a811d6020f9 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -367,6 +367,10 @@ Topic for battery info + + Maximum distance robot can go out in meters + + Distance to check if passed reference frame to check in diff --git a/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp b/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp index dd985a42b95..b16266e7d7c 100644 --- a/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp @@ -67,7 +67,7 @@ BT::NodeStatus IsWithinPathTrackingBoundsCondition::tick() RCLCPP_ERROR(node_->get_logger(), "max_error parameter not provided"); max_error_ = 1.0; // Default fallback } - + if (max_error_ < 0.0) { RCLCPP_WARN(node_->get_logger(), "max_error should be positive, using absolute value"); max_error_ = std::abs(max_error_); diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt index f916d1e7578..259aad4e011 100644 --- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt @@ -30,6 +30,9 @@ plugin_add_test(test_condition_is_pose_occupied test_is_pose_occupied.cpp nav2_i plugin_add_test(test_are_error_codes_present test_are_error_codes_present.cpp nav2_would_a_controller_recovery_help_condition_bt_node) +plugin_add_test(test_condition_is_within_path_tracking_bounds test_is_within_path_tracking_bounds.cpp + nav2_is_within_path_tracking_bounds_condition_bt_node) + plugin_add_test(test_would_a_controller_recovery_help test_would_a_controller_recovery_help.cpp nav2_would_a_controller_recovery_help_condition_bt_node) From 7a02bf60ad9971a12f8754f65e0320b3bdcad8ef Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 23 Oct 2025 05:02:53 +0300 Subject: [PATCH 5/7] Styling changes Signed-off-by: silanus23 --- ..._within_path_tracking_bounds_condition.hpp | 5 ++-- ..._within_path_tracking_bounds_condition.cpp | 26 +++++++++++-------- 2 files changed, 18 insertions(+), 13 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp index 59db20e3f55..11706b93ada 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "behaviortree_cpp/condition_node.h" #include "nav2_ros_common/lifecycle_node.hpp" @@ -51,12 +52,12 @@ class IsWithinPathTrackingBoundsCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort("max_error", 0.5, "Maximum allowed tracking error") + BT::InputPort("max_error", "Maximum allowed tracking error") }; } private: - nav2::LifecycleNode::SharedPtr node_; + 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::SharedPtr tracking_feedback_sub_; diff --git a/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp b/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp index b16266e7d7c..7e8ba36af16 100644 --- a/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp @@ -21,16 +21,16 @@ IsWithinPathTrackingBoundsCondition::IsWithinPathTrackingBoundsCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - last_error_(0.0) + last_error_(std::numeric_limits::max()) { - node_ = config().blackboard->get("node"); - callback_group_ = node_->create_callback_group( + auto node = config().blackboard->get("node"); + callback_group_ = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); - callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + callback_group_executor_.add_callback_group(callback_group_, node->get_node_base_interface()); - tracking_feedback_sub_ = node_->create_subscription( - "/tracking_feedback", + tracking_feedback_sub_ = node->create_subscription( + "tracking_feedback", std::bind(&IsWithinPathTrackingBoundsCondition::trackingFeedbackCallback, this, std::placeholders::_1), rclcpp::SystemDefaultsQoS(), @@ -39,8 +39,8 @@ IsWithinPathTrackingBoundsCondition::IsWithinPathTrackingBoundsCondition( bt_loop_duration_ = config().blackboard->template get("bt_loop_duration"); - RCLCPP_DEBUG(node_->get_logger(), "Initialized IsWithinPathTrackingBoundsCondition BT node"); - RCLCPP_INFO_ONCE(node_->get_logger(), "Waiting for tracking error"); + RCLCPP_INFO_ONCE(logger_, "Waiting for tracking error"); + RCLCPP_INFO(logger_, "Initialized IsWithinPathTrackingBoundsCondition BT node"); initialize(); } @@ -64,15 +64,19 @@ BT::NodeStatus IsWithinPathTrackingBoundsCondition::tick() callback_group_executor_.spin_all(bt_loop_duration_); if (!getInput("max_error", max_error_)) { - RCLCPP_ERROR(node_->get_logger(), "max_error parameter not provided"); - max_error_ = 1.0; // Default fallback + RCLCPP_ERROR(logger_, "max_error parameter not provided"); + return BT::NodeStatus::FAILURE; } if (max_error_ < 0.0) { - RCLCPP_WARN(node_->get_logger(), "max_error should be positive, using absolute value"); + RCLCPP_WARN(logger_, "max_error should be positive, using absolute value"); max_error_ = std::abs(max_error_); } + if (last_error_ == std::numeric_limits::max()) { + RCLCPP_WARN(logger_, "No tracking feedback received yet."); + } + if (last_error_ <= max_error_) { return BT::NodeStatus::SUCCESS; } From 3ea85d84e357305a9da105c6b6ebf718ea5a067c Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 23 Oct 2025 06:57:47 +0300 Subject: [PATCH 6/7] Adding left, right side errors Signed-off-by: silanus23 --- ..._within_path_tracking_bounds_condition.hpp | 6 +- nav2_behavior_tree/nav2_tree_nodes.xml | 3 +- ..._within_path_tracking_bounds_condition.cpp | 37 +++- .../test_is_within_path_tracking_bounds.cpp | 187 +++++++++++------- 4 files changed, 155 insertions(+), 78 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp index 11706b93ada..67a51cc72ab 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp @@ -52,7 +52,8 @@ class IsWithinPathTrackingBoundsCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort("max_error", "Maximum allowed tracking error") + BT::InputPort("max_error_right", "Maximum allowed tracking error on the right side"), + BT::InputPort("max_error_left", "Maximum allowed tracking error left side") }; } @@ -62,7 +63,8 @@ class IsWithinPathTrackingBoundsCondition : public BT::ConditionNode rclcpp::executors::SingleThreadedExecutor callback_group_executor_; rclcpp::Subscription::SharedPtr tracking_feedback_sub_; double last_error_{0.0}; - double max_error_{1.5}; + double max_error_right_{1.5}; + double max_error_left_{1.5}; std::chrono::milliseconds bt_loop_duration_; void trackingFeedbackCallback(const nav2_msgs::msg::TrackingFeedback::SharedPtr msg); diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index a811d6020f9..62867787e54 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -368,7 +368,8 @@ - Maximum distance robot can go out in meters + Maximum distance robot can go out from path in meters. On left side + Maximum distance robot can go out from path in meters. On right side diff --git a/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp b/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp index 7e8ba36af16..95ec65da639 100644 --- a/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp @@ -52,7 +52,8 @@ void IsWithinPathTrackingBoundsCondition::trackingFeedbackCallback( void IsWithinPathTrackingBoundsCondition::initialize() { - getInput("max_error", max_error_); + getInput("max_error_left", max_error_left_); + getInput("max_error_right", max_error_right_); } BT::NodeStatus IsWithinPathTrackingBoundsCondition::tick() @@ -63,24 +64,44 @@ BT::NodeStatus IsWithinPathTrackingBoundsCondition::tick() callback_group_executor_.spin_all(bt_loop_duration_); - if (!getInput("max_error", max_error_)) { + 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 parameter not provided"); return BT::NodeStatus::FAILURE; } - if (max_error_ < 0.0) { - RCLCPP_WARN(logger_, "max_error should be positive, using absolute value"); - max_error_ = std::abs(max_error_); + 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_); } if (last_error_ == std::numeric_limits::max()) { RCLCPP_WARN(logger_, "No tracking feedback received yet."); + return BT::NodeStatus::FAILURE; } - if (last_error_ <= max_error_) { - return BT::NodeStatus::SUCCESS; + if (last_error_ > 0.0) { // Positive = left side + if (last_error_ > max_error_left_) { + return BT::NodeStatus::FAILURE; + } else { + return BT::NodeStatus::SUCCESS; + } + } else { // Negative = right side + if (std::abs(last_error_) > max_error_right_) { + return BT::NodeStatus::FAILURE; + } else { + return BT::NodeStatus::SUCCESS; + } } - return BT::NodeStatus::FAILURE; } } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_within_path_tracking_bounds.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_within_path_tracking_bounds.cpp index b48d329f45a..82e3525f0f8 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_within_path_tracking_bounds.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_within_path_tracking_bounds.cpp @@ -38,9 +38,7 @@ class IsWithinPathTrackingBoundsConditionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( - "node", - node_); + config_->blackboard->set("node", node_); config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); @@ -49,7 +47,7 @@ class IsWithinPathTrackingBoundsConditionTestFixture : public ::testing::Test "IsWithinPathTrackingBounds"); tracking_feedback_pub_ = node_->create_publisher( - "/tracking_feedback", + "tracking_feedback", rclcpp::SystemDefaultsQoS()); tracking_feedback_pub_->on_activate(); } @@ -64,6 +62,15 @@ class IsWithinPathTrackingBoundsConditionTestFixture : public ::testing::Test executor_.reset(); } + void publishAndSpin(float error_value) + { + nav2_msgs::msg::TrackingFeedback msg; + msg.tracking_error = error_value; + tracking_feedback_pub_->publish(msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor_->spin_some(); + } + protected: static nav2::LifecycleNode::SharedPtr node_; static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; @@ -81,129 +88,175 @@ std::shared_ptr IsWithinPathTrackingBoundsConditionTest nav2::Publisher::SharedPtr IsWithinPathTrackingBoundsConditionTestFixture::tracking_feedback_pub_ = nullptr; -TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_behavior_within_bounds) +TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_symmetric_bounds) { + // Test with equal bounds for left and right sides std::string xml_txt = R"( - + )"; auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); - // Test case 1: Error is within bounds (should return SUCCESS) - nav2_msgs::msg::TrackingFeedback tracking_feedback_msg; - tracking_feedback_msg.tracking_error = 0.5; - tracking_feedback_pub_->publish(tracking_feedback_msg); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - executor_->spin_some(); + publishAndSpin(0.5); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); - - // Test case 2: Error is exactly at the boundary (should return SUCCESS) - tracking_feedback_msg.tracking_error = 1.0; - tracking_feedback_pub_->publish(tracking_feedback_msg); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - executor_->spin_some(); + publishAndSpin(-0.5); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); - - // Test case 3: Error exceeds bounds (should return FAILURE) - tracking_feedback_msg.tracking_error = 1.5; - tracking_feedback_pub_->publish(tracking_feedback_msg); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - executor_->spin_some(); + publishAndSpin(1.0); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + publishAndSpin(-1.0); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + publishAndSpin(1.5); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); + publishAndSpin(-1.5); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); + publishAndSpin(0.0); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); +} - // Test case 4: Zero error (should return SUCCESS) - tracking_feedback_msg.tracking_error = 0.0; - tracking_feedback_pub_->publish(tracking_feedback_msg); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - executor_->spin_some(); +TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_asymmetric_bounds) +{ + // Test with different bounds for left and right sides + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + publishAndSpin(1.5); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + publishAndSpin(-0.3); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + publishAndSpin(2.0); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + publishAndSpin(-0.5); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + publishAndSpin(2.5); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); + publishAndSpin(-0.8); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); } -TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_behavior_different_max_error) +TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_left_side_only) { + // Test with very restrictive right bound std::string xml_txt = R"( - + )"; auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); - // Test case 1: Error is within smaller bounds (should return SUCCESS) - nav2_msgs::msg::TrackingFeedback tracking_feedback_msg; - tracking_feedback_msg.tracking_error = 0.1; - tracking_feedback_pub_->publish(tracking_feedback_msg); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - executor_->spin_some(); + publishAndSpin(4.9); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + publishAndSpin(5.0); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + publishAndSpin(5.1); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); + publishAndSpin(-0.001); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); + publishAndSpin(0.0); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); +} - // Test case 2: Error exceeds smaller bounds (should return FAILURE) - tracking_feedback_msg.tracking_error = 0.3; - tracking_feedback_pub_->publish(tracking_feedback_msg); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - executor_->spin_some(); +TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_right_side_only) +{ + // Test with very restrictive left bound + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + + publishAndSpin(-4.9); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + publishAndSpin(-5.0); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + publishAndSpin(-5.1); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); + publishAndSpin(0.001); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); + publishAndSpin(0.0); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); } -TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_behavior_large_error_values) +TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_very_tight_bounds) { + // Test with very small bounds std::string xml_txt = R"( - + )"; auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); - // Test case 1: Large error within bounds (should return SUCCESS) - nav2_msgs::msg::TrackingFeedback tracking_feedback_msg; - tracking_feedback_msg.tracking_error = 4.9; - tracking_feedback_pub_->publish(tracking_feedback_msg); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - executor_->spin_some(); + publishAndSpin(0.05); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + publishAndSpin(-0.05); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + publishAndSpin(0.15); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); + publishAndSpin(-0.15); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); +} + +TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_no_feedback_received) +{ + // Test behavior when no feedback has been received yet + std::string xml_txt = + R"( + + + + + )"; - // Test case 2: Very large error exceeding bounds (should return FAILURE) - tracking_feedback_msg.tracking_error = 10.0; - tracking_feedback_pub_->publish(tracking_feedback_msg); + // Create a fresh tree without publishing any feedback + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + + // Should return FAILURE when no feedback received + // Note: This test creates a new condition node, so last_error_ will be at max() std::this_thread::sleep_for(std::chrono::milliseconds(100)); executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); } -TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_behavior_edge_cases) +TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_sign_convention) { + // Verify sign convention: positive = left, negative = right std::string xml_txt = R"( - + )"; auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); - // Test case 1: Zero max_error with zero tracking error (should return SUCCESS) - nav2_msgs::msg::TrackingFeedback tracking_feedback_msg; - tracking_feedback_msg.tracking_error = 0.0; - tracking_feedback_pub_->publish(tracking_feedback_msg); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - executor_->spin_some(); + publishAndSpin(0.9); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); - - // Test case 2: Zero max_error with any positive tracking error (should return FAILURE) - tracking_feedback_msg.tracking_error = 0.001; - tracking_feedback_pub_->publish(tracking_feedback_msg); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - executor_->spin_some(); + publishAndSpin(1.5); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); + publishAndSpin(-1.5); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + publishAndSpin(-2.5); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); } From 9e50b942452473180121e2ccfbe23cdc3613722d Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 24 Oct 2025 13:16:49 +0300 Subject: [PATCH 7/7] minor changes Signed-off-by: silanus23 --- .../condition/is_within_path_tracking_bounds_condition.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp b/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp index 95ec65da639..e5ed1450a40 100644 --- a/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp @@ -39,7 +39,6 @@ IsWithinPathTrackingBoundsCondition::IsWithinPathTrackingBoundsCondition( bt_loop_duration_ = config().blackboard->template get("bt_loop_duration"); - RCLCPP_INFO_ONCE(logger_, "Waiting for tracking error"); RCLCPP_INFO(logger_, "Initialized IsWithinPathTrackingBoundsCondition BT node"); initialize(); } @@ -75,7 +74,7 @@ BT::NodeStatus IsWithinPathTrackingBoundsCondition::tick() } if (!getInput("max_error_right", max_error_right_)) { - RCLCPP_ERROR(logger_, "max_error parameter not provided"); + RCLCPP_ERROR(logger_, "max_error_right parameter not provided"); return BT::NodeStatus::FAILURE; }