diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index f2daaaeeeba..851a660ab66 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -120,6 +120,9 @@ list(APPEND plugin_libs nav2_goal_updated_condition_bt_node) add_library(nav2_is_path_valid_condition_bt_node SHARED plugins/condition/is_path_valid_condition.cpp) list(APPEND plugin_libs nav2_is_path_valid_condition_bt_node) +add_library(nav2_is_pose_occupied_condition_bt_node SHARED plugins/condition/is_pose_occupied_condition.cpp) +list(APPEND plugin_libs nav2_is_pose_occupied_condition_bt_node) + add_library(nav2_time_expired_condition_bt_node SHARED plugins/condition/time_expired_condition.cpp) list(APPEND plugin_libs nav2_time_expired_condition_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp new file mode 100644 index 00000000000..7ad6bb014a5 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp @@ -0,0 +1,101 @@ +// Copyright (c) 2025 Maurice Alexander Purnawan +// +// 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_POSE_OCCUPIED_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_POSE_OCCUPIED_CONDITION_HPP_ + +#include +#include + +#include "nav2_ros_common/lifecycle_node.hpp" +#include "behaviortree_cpp/condition_node.h" +#include "behaviortree_cpp/json_export.h" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/srv/get_costs.hpp" +#include "nav2_ros_common/service_client.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_behavior_tree/json_utils.hpp" + + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ConditionNode that returns SUCCESS when the IsPoseOccupied + * service returns true and FAILURE otherwise + */ +class IsPoseOccupiedCondition : public BT::ConditionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::IsPoseOccupiedCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ + IsPoseOccupiedCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + IsPoseOccupiedCondition() = delete; + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; + + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ + static BT::PortsList providedPorts() + { + // Register JSON definitions for the types used in the ports + BT::RegisterJsonDefinition(); + BT::RegisterJsonDefinition(); + + return { + BT::InputPort("pose", "Pose to check if occupied"), + BT::InputPort("service_name", "global_costmap/get_cost_global_costmap"), + BT::InputPort( + "cost_threshold", 254.0, + "Cost threshold for considering a pose occupied"), + BT::InputPort("use_footprint", true, "Whether to use footprint cost"), + BT::InputPort( + "consider_unknown_as_obstacle", false, + "Whether to consider unknown cost as obstacle"), + BT::InputPort("server_timeout"), + }; + } + +private: + nav2::LifecycleNode::SharedPtr node_; + nav2::ServiceClient::SharedPtr client_; + // The timeout value while waiting for a response from the + // get cost service + std::chrono::milliseconds server_timeout_; + bool use_footprint_; + bool consider_unknown_as_obstacle_; + double cost_threshold_; + std::string service_name_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_POSE_OCCUPIED_CONDITION_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index d555f0df948..7161f6419ce 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -394,6 +394,15 @@ If unknown cost is considered valid + + Pose to check whether it is occupied + Service name to call GetCosts + The cost threshold above which a pose is considered occupied + Whether to use the footprint cost or the point cost + If unknown cost is considered valid + Server timeout + + Error code diff --git a/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.cpp b/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.cpp new file mode 100644 index 00000000000..8d253e5e0a1 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.cpp @@ -0,0 +1,83 @@ +// Copyright (c) 2025 Maurice Alexander Purnawan +// +// 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_pose_occupied_condition.hpp" +#include +#include +#include + +namespace nav2_behavior_tree +{ + +IsPoseOccupiedCondition::IsPoseOccupiedCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf), + use_footprint_(true), consider_unknown_as_obstacle_(false), cost_threshold_(254), + service_name_("global_costmap/get_cost_global_costmap") +{ + node_ = config().blackboard->get("node"); + server_timeout_ = config().blackboard->template get("server_timeout"); +} + +void IsPoseOccupiedCondition::initialize() +{ + getInput("service_name", service_name_); + getInput("cost_threshold", cost_threshold_); + getInput("use_footprint", use_footprint_); + getInput("consider_unknown_as_obstacle", consider_unknown_as_obstacle_); + getInput("server_timeout", server_timeout_); + client_ = + node_->create_client( + service_name_, + false /* Does not create and spin an internal executor*/); +} + +BT::NodeStatus IsPoseOccupiedCondition::tick() +{ + if (!BT::isStatusActive(status())) { + initialize(); + } + geometry_msgs::msg::PoseStamped pose; + getInput("pose", pose); + + auto request = std::make_shared(); + request->use_footprint = use_footprint_; + request->poses.push_back(pose); + + auto response = client_->invoke(request, server_timeout_); + + if(!response->success) { + RCLCPP_ERROR( + node_->get_logger(), + "GetCosts service call failed"); + return BT::NodeStatus::FAILURE; + } + + if((response->costs[0] == 255 && !consider_unknown_as_obstacle_) || + response->costs[0] < cost_threshold_) + { + 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("IsPoseOccupied"); +} diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt index 51f7a908801..f916d1e7578 100644 --- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt @@ -26,6 +26,8 @@ plugin_add_test(test_condition_is_battery_low test_is_battery_low.cpp nav2_is_ba plugin_add_test(test_condition_is_path_valid test_is_path_valid.cpp nav2_is_path_valid_condition_bt_node) +plugin_add_test(test_condition_is_pose_occupied test_is_pose_occupied.cpp nav2_is_pose_occupied_condition_bt_node) + 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_would_a_controller_recovery_help diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_pose_occupied.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_pose_occupied.cpp new file mode 100644 index 00000000000..bb10d5dc4e4 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_is_pose_occupied.cpp @@ -0,0 +1,203 @@ +// Copyright (c) 2025 Maurice Alexander Purnawan +// +// 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 "behaviortree_cpp/bt_factory.h" + +#include "utils/test_service.hpp" +#include "nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp" +#include "utils/test_behavior_tree_fixture.hpp" + + +class IsPoseOccupiedSuccessService : public TestService +{ +public: + IsPoseOccupiedSuccessService() + : TestService("/global_costmap/get_cost_global_costmap") + {} + + virtual void handle_service( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + (void)request_header; + (void)request; + response->costs = {254}; + response->success = true; + } +}; + +class IsPoseOccupiedFailureService : public TestService +{ +public: + IsPoseOccupiedFailureService() + : TestService("/local_costmap/get_cost_local_costmap") + {} + + virtual void handle_service( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + (void)request_header; + (void)request; + response->costs = {255}; + response->success = false; + } +}; + +class IsPoseOccupiedTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("is_pose_occupied_test_fixture"); + 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( + "server_timeout", + std::chrono::milliseconds(10)); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "IsPoseOccupied", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + success_server_.reset(); + failure_server_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + static std::shared_ptr success_server_; + static std::shared_ptr failure_server_; + +protected: + static nav2::LifecycleNode::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +nav2::LifecycleNode::SharedPtr IsPoseOccupiedTestFixture::node_ = nullptr; + +BT::NodeConfiguration * IsPoseOccupiedTestFixture::config_ = nullptr; +std::shared_ptr +IsPoseOccupiedTestFixture::success_server_ = nullptr; +std::shared_ptr +IsPoseOccupiedTestFixture::failure_server_ = nullptr; +std::shared_ptr IsPoseOccupiedTestFixture::factory_ = nullptr; +std::shared_ptr IsPoseOccupiedTestFixture::tree_ = nullptr; + +TEST_F(IsPoseOccupiedTestFixture, test_tick_is_pose_occupied_success) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 5.0; + pose.pose.position.y = 0.0; + + config_->blackboard->set("pose", pose); + + tree_->rootNode()->executeTick(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); +} + +TEST_F(IsPoseOccupiedTestFixture, test_tick_is_pose_occupied_fail) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 5.0; + pose.pose.position.y = 0.0; + + config_->blackboard->set("pose", pose); + + tree_->rootNode()->executeTick(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize service and spin on new thread + IsPoseOccupiedTestFixture::success_server_ = + std::make_shared(); + std::thread success_server_thread([]() { + rclcpp::spin(IsPoseOccupiedTestFixture::success_server_); + }); + + IsPoseOccupiedTestFixture::failure_server_ = + std::make_shared(); + std::thread failure_server_thread([]() { + rclcpp::spin(IsPoseOccupiedTestFixture::failure_server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + success_server_thread.join(); + failure_server_thread.join(); + + return all_successful; +}