diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 1827fb5665..575337c61a 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/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 0000000000..67a51cc72a --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp @@ -0,0 +1,75 @@ +// 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 + +#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_right", "Maximum allowed tracking error on the right side"), + BT::InputPort("max_error_left", "Maximum allowed tracking error left side") + }; + } + +private: + 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_; + double last_error_{0.0}; + 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); +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_WITHIN_PATH_TRACKING_BOUNDS_CONDITION_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 7161f6419c..62867787e5 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -367,6 +367,11 @@ Topic for battery info + + 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 + + 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 new file mode 100644 index 0000000000..e5ed1450a4 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp @@ -0,0 +1,113 @@ +// 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_(std::numeric_limits::max()) +{ + 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()); + + 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_INFO(logger_, "Initialized IsWithinPathTrackingBoundsCondition BT node"); + initialize(); +} + +void IsWithinPathTrackingBoundsCondition::trackingFeedbackCallback( + const nav2_msgs::msg::TrackingFeedback::SharedPtr msg) +{ + last_error_ = msg->tracking_error; +} + +void IsWithinPathTrackingBoundsCondition::initialize() +{ + getInput("max_error_left", max_error_left_); + getInput("max_error_right", max_error_right_); +} + +BT::NodeStatus IsWithinPathTrackingBoundsCondition::tick() +{ + if (!BT::isStatusActive(status())) { + initialize(); + } + + callback_group_executor_.spin_all(bt_loop_duration_); + + 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_); + } + + if (last_error_ == std::numeric_limits::max()) { + RCLCPP_WARN(logger_, "No tracking feedback received yet."); + return BT::NodeStatus::FAILURE; + } + + 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; + } + } +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType( + "IsWithinPathTrackingBounds"); +} diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt index f916d1e757..259aad4e01 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) 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 0000000000..82e3525f0f --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_is_within_path_tracking_bounds.cpp @@ -0,0 +1,276 @@ +// 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(); + } + + 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_; + 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_symmetric_bounds) +{ + // Test with equal bounds for left and right sides + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + + publishAndSpin(0.5); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + publishAndSpin(-0.5); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); + 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_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_left_side_only) +{ + // Test with very restrictive right 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_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_very_tight_bounds) +{ + // Test with very small bounds + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + + 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"( + + + + + )"; + + // 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_sign_convention) +{ + // Verify sign convention: positive = left, negative = right + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + + publishAndSpin(0.9); + 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::SUCCESS); + publishAndSpin(-2.5); + 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; +}