diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 03339b19cdc..4059e6da15e 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -218,6 +218,12 @@ list(APPEND plugin_libs nav2_nonblocking_sequence_bt_node) add_library(nav2_round_robin_node_bt_node SHARED plugins/control/round_robin_node.cpp) list(APPEND plugin_libs nav2_round_robin_node_bt_node) +add_library(nav2_pause_resume_controller_bt_node SHARED plugins/control/pause_resume_controller.cpp) +list(APPEND plugin_libs nav2_pause_resume_controller_bt_node) + +add_library(nav2_persistent_sequence_bt_node SHARED plugins/control/persistent_sequence.cpp) +list(APPEND plugin_libs nav2_persistent_sequence_bt_node) + add_library(nav2_single_trigger_bt_node SHARED plugins/decorator/single_trigger_node.cpp) list(APPEND plugin_libs nav2_single_trigger_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp new file mode 100644 index 00000000000..a3f5333b19b --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp @@ -0,0 +1,152 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// 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__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_ + +// Other includes +#include +#include +#include + +// ROS includes +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/callback_group.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "behaviortree_cpp/control_node.h" +#include "nav2_ros_common/service_server.hpp" + +// Interface definitions +#include "std_srvs/srv/trigger.hpp" + + +namespace nav2_behavior_tree +{ + +using Trigger = std_srvs::srv::Trigger; + +enum state_t {RESUMED, PAUSED, PAUSE_REQUESTED, ON_PAUSE, RESUME_REQUESTED, ON_RESUME}; +const std::map state_names = { + {RESUMED, "RESUMED"}, + {PAUSED, "PAUSED"}, + {PAUSE_REQUESTED, "PAUSE_REQUESTED"}, + {ON_PAUSE, "ON_PAUSE"}, + {RESUME_REQUESTED, "RESUME_REQUESTED"}, + {ON_RESUME, "ON_RESUME"} +}; +const std::map child_indices = { + {RESUMED, 0}, + {PAUSED, 1}, + {ON_PAUSE, 2}, + {ON_RESUME, 3} +}; + +/* @brief Controlled through service calls to pause and resume the execution of the tree + * It has one mandatory child for the RESUMED, and three optional for the PAUSED state, + * the ON_PAUSE event and the ON_RESUME event. + * It has two input ports: + * - pause_service_name: name of the service to pause + * - resume_service_name: name of the service to resume + * + * Usage: + * + * + * + * + * + * + * + * + * + * + * The controller starts in RESUMED state, and ticks it until it returns success. + * When the pause service is called, ON_PAUSE is ticked until completion, + * then the controller switches to PAUSED state. + * When the resume service is called, ON_RESUME is ticked until completion, + * then the controller switches back to RESUMED state. + * + * The controller only returns success when the RESUMED child returns success. + * The controller returns failure if any child returns failure. + * In any other case, it returns running. + */ + + +class PauseResumeController : public BT::ControlNode +{ +public: + //! @brief Constructor + PauseResumeController( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + //! @brief Reset state and go to Idle + void halt() override; + + //! @brief Handle transitions if requested and tick child related to the actual state + BT::NodeStatus tick() override; + + //! @brief Declare ports + static BT::PortsList providedPorts() + { + return { + BT::InputPort( + "pause_service_name", + "Name of the service to pause"), + BT::InputPort( + "resume_service_name", + "Name of the service to resume"), + }; + } + + [[nodiscard]] inline state_t getState() const + { + return state_; + } + +private: + //! @brief Set state to PAUSE_REQUESTED + void pauseServiceCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response); + + //! @brief Set state to RESUME_REQUESTED + void resumeServiceCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response); + + /** @brief Switch to the next state based on the current state + * + * PAUSE_REQUESTED -> ON_PAUSE + * ON_PAUSE -> PAUSED + * + * RESUME_REQUESTED -> ON_RESUME + * ON_RESUME -> RESUMED + * + * Do nothing if in end state + */ + void switchToNextState(); + + rclcpp::Logger logger_{rclcpp::get_logger("PauseResumeController")}; + rclcpp::CallbackGroup::SharedPtr cb_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + nav2::ServiceServer::SharedPtr pause_srv_; + nav2::ServiceServer::SharedPtr resume_srv_; + state_t state_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/persistent_sequence.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/persistent_sequence.hpp new file mode 100644 index 00000000000..79d494eb8e6 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/persistent_sequence.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// 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__CONTROL__PERSISTENT_SEQUENCE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PERSISTENT_SEQUENCE_HPP_ + +#include +#include "behaviortree_cpp/control_node.h" +#include "behaviortree_cpp/bt_factory.h" + +namespace nav2_behavior_tree +{ +/** + * @brief The PersistentSequenceNode is similar to the SequenceNode, but it + * stores the index of the last running child in the blackboard (key: "current_child_idx"), + * and it does not reset the index when it got halted. + * It used to tick children in an ordered sequence. If any child returns RUNNING, previous + * children will NOT be ticked again. + * + * - If all the children return SUCCESS, this node returns SUCCESS. + * + * - If a child returns RUNNING, this node returns RUNNING. + * Loop is NOT restarted, the same running child will be ticked again. + * + * - If a child returns FAILURE, stop the loop and return FAILURE. + * Restart the loop only if (reset_on_failure == true) + * + */ +class PersistentSequenceNode : public BT::ControlNode +{ +public: + PersistentSequenceNode(const std::string & name, const BT::NodeConfiguration & conf); + + ~PersistentSequenceNode() override = default; + + //! @brief Declare ports + static BT::PortsList providedPorts() + { + return { + BT::BidirectionalPort("current_child_idx", "The index of the current child"), + }; + } + +private: + BT::NodeStatus tick() override; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PERSISTENT_SEQUENCE_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index a9798c0c64c..230ce02f70c 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -404,6 +404,16 @@ + + + Service to call to pause + Service to call to resume + + + + Index of the child to execute + + Rate diff --git a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp new file mode 100644 index 00000000000..0f039aad8e6 --- /dev/null +++ b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp @@ -0,0 +1,190 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// 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. + +// Other includes +#include + +// ROS includes +#include "nav2_behavior_tree/plugins/control/pause_resume_controller.hpp" + +// Other includes +#include "behaviortree_cpp/bt_factory.h" + +namespace nav2_behavior_tree +{ + +using namespace std::placeholders; + +PauseResumeController::PauseResumeController( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf) +: BT::ControlNode(xml_tag_name, conf) +{ + auto node = this->config().blackboard->get("node"); + state_ = RESUMED; + + // Create a separate cb group with a separate executor to spin + cb_group_ = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + + executor_ = + std::make_shared(); + + executor_->add_callback_group(cb_group_, node->get_node_base_interface()); + + std::string pause_service_name; + getInput("pause_service_name", pause_service_name); + pause_srv_ = std::make_shared>( + pause_service_name, + node, + std::bind(&PauseResumeController::pauseServiceCallback, this, _1, _2, _3), + cb_group_); + + std::string resume_service_name; + getInput("resume_service_name", resume_service_name); + resume_srv_ = std::make_shared>( + resume_service_name, + node, + std::bind(&PauseResumeController::resumeServiceCallback, this, _1, _2, _3), + cb_group_); +} + +BT::NodeStatus PauseResumeController::tick() +{ + unsigned int children_count = children_nodes_.size(); + if (children_count < 1 || children_count > 4) { + throw std::runtime_error( + "PauseNode must have at least one and at most four children " + "(currently has " + std::to_string(children_count) + ")"); + } + + if (status() == BT::NodeStatus::IDLE) { + state_ = RESUMED; + } + setStatus(BT::NodeStatus::RUNNING); + + executor_->spin_some(); + + // If pause / resume requested, reset children and switch to transient child + if (state_ == PAUSE_REQUESTED || state_ == RESUME_REQUESTED) { + resetChildren(); + switchToNextState(); + } + + // Return RUNNING and do nothing if specific child is not used + const uint16_t child_idx = child_indices.at(state_); + if (children_nodes_.size() <= child_idx) { + switchToNextState(); + return BT::NodeStatus::RUNNING; + } + + // If child is used, tick it + const BT::NodeStatus child_status = + children_nodes_[child_indices.at(state_)]->executeTick(); + + switch (child_status) { + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; + case BT::NodeStatus::SUCCESS: + case BT::NodeStatus::SKIPPED: + if (state_ == RESUMED) { + // Resumed child returned SUCCESS, we're done + return BT::NodeStatus::SUCCESS; + } + switchToNextState(); + // If any branch other than RESUMED returned SUCCESS, keep ticking + return BT::NodeStatus::RUNNING; + case BT::NodeStatus::FAILURE: + RCLCPP_ERROR( + logger_, "%s child returned FAILURE", state_names.at(state_).c_str()); + return BT::NodeStatus::FAILURE; + default: + throw std::runtime_error("A child node must never return IDLE"); + } +} + +void PauseResumeController::switchToNextState() +{ + static const std::map next_states = { + {PAUSE_REQUESTED, ON_PAUSE}, + {ON_PAUSE, PAUSED}, + {RESUME_REQUESTED, ON_RESUME}, + {ON_RESUME, RESUMED} + }; + + if (state_ == PAUSED || state_ == RESUMED) { + // No next state, do nothing + return; + } + + state_ = next_states.at(state_); + RCLCPP_INFO(logger_, "Switched to state: %s", state_names.at(state_).c_str()); +} + +void PauseResumeController::pauseServiceCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response) +{ + if (state_ != PAUSED) { + RCLCPP_INFO(logger_, "Received pause request"); + response->success = true; + state_ = PAUSE_REQUESTED; + return; + } + + std::string warn_message = "PauseResumeController BT node already in state PAUSED"; + RCLCPP_WARN(logger_, "%s", warn_message.c_str()); + response->success = false; + response->message = warn_message; +} + +void PauseResumeController::resumeServiceCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response) +{ + if (state_ == PAUSED) { + RCLCPP_INFO(logger_, "Received resume request"); + response->success = true; + state_ = RESUME_REQUESTED; + return; + } + + std::string warn_message = "PauseResumeController BT node not in state PAUSED"; + RCLCPP_WARN(logger_, "%s", warn_message.c_str()); + response->success = false; + response->message = warn_message; +} + +void PauseResumeController::halt() +{ + state_ = RESUMED; + ControlNode::halt(); +} + +} // namespace nav2_behavior_tree + +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory.registerBuilder( + "PauseResumeController", builder); +} diff --git a/nav2_behavior_tree/plugins/control/persistent_sequence.cpp b/nav2_behavior_tree/plugins/control/persistent_sequence.cpp new file mode 100644 index 00000000000..1886deb3d5c --- /dev/null +++ b/nav2_behavior_tree/plugins/control/persistent_sequence.cpp @@ -0,0 +1,84 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// 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/control/persistent_sequence.hpp" +#include "behaviortree_cpp/action_node.h" +#include "behaviortree_cpp/bt_factory.h" + +namespace nav2_behavior_tree +{ + +PersistentSequenceNode::PersistentSequenceNode( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ControlNode::ControlNode(name, conf) {} + +BT::NodeStatus PersistentSequenceNode::tick() +{ + const int children_count = children_nodes_.size(); + + int current_child_idx; + getInput("current_child_idx", current_child_idx); + + setStatus(BT::NodeStatus::RUNNING); + + while (current_child_idx < children_count) { + TreeNode * current_child_node = children_nodes_[current_child_idx]; + const BT::NodeStatus child_status = current_child_node->executeTick(); + + switch (child_status) { + case BT::NodeStatus::RUNNING: + return child_status; + + case BT::NodeStatus::FAILURE: + // Reset on failure + resetChildren(); + current_child_idx = 0; + setOutput("current_child_idx", 0); + return child_status; + + case BT::NodeStatus::SUCCESS: + case BT::NodeStatus::SKIPPED: + // Skip the child node + current_child_idx++; + setOutput("current_child_idx", current_child_idx); + break; + + case BT::NodeStatus::IDLE: + throw std::runtime_error("A child node must never return IDLE"); + } // end switch + } // end while loop + + // The entire while loop completed. This means that all the children returned SUCCESS. + if (current_child_idx >= children_count) { + resetChildren(); + setOutput("current_child_idx", 0); + } + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory.registerBuilder( + "PersistentSequence", builder); +} diff --git a/nav2_behavior_tree/test/plugins/control/CMakeLists.txt b/nav2_behavior_tree/test/plugins/control/CMakeLists.txt index 1a8b0cf781a..a625bdad792 100644 --- a/nav2_behavior_tree/test/plugins/control/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/control/CMakeLists.txt @@ -5,3 +5,8 @@ plugin_add_test(test_control_pipeline_sequence test_pipeline_sequence.cpp nav2_p plugin_add_test(test_control_round_robin_node test_round_robin_node.cpp nav2_round_robin_node_bt_node) plugin_add_test(test_control_nonblocking_sequence test_nonblocking_sequence.cpp nav2_nonblocking_sequence_bt_node) + +plugin_add_test(test_control_pause_resume_controller + test_pause_resume_controller.cpp nav2_pause_resume_controller_bt_node) + +plugin_add_test(test_persistent_sequence test_persistent_sequence.cpp nav2_persistent_sequence_bt_node) diff --git a/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp b/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp new file mode 100644 index 00000000000..8a78c37d89b --- /dev/null +++ b/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp @@ -0,0 +1,286 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// 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 "utils/test_behavior_tree_fixture.hpp" +#include "utils/test_dummy_tree_node.hpp" +#include "utils/get_node_from_tree.hpp" +#include "nav2_behavior_tree/plugins/control/pause_resume_controller.hpp" +#include "std_srvs/srv/trigger.hpp" + +class PauseResumeControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + static void SetUpTestCase() + { + auto node = std::make_shared("pause_resume_controller_test_fixture"); + executor_ = + std::make_shared(); + cb_group_ = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + executor_->add_callback_group(cb_group_, node->get_node_base_interface()); + pause_client_ = node->create_client( + "pause", rclcpp::ServicesQoS(), cb_group_); + resume_client_ = node->create_client( + "resume", rclcpp::ServicesQoS(), cb_group_); + + factory_ = std::make_shared(); + config_ = new BT::NodeConfiguration(); + config_->blackboard = BT::Blackboard::create(); + config_->blackboard->set("node", node); + + factory_->registerNodeType("PauseResumeController"); + + // Register dummy node for testing + factory_->registerNodeType("DummyNode"); + } + + static void TearDownTestCase() + { + if (config_) { + delete config_; + config_ = nullptr; + } + tree_.reset(); + } + +protected: + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + static rclcpp::CallbackGroup::SharedPtr cb_group_; + static rclcpp::Client::SharedPtr pause_client_; + static rclcpp::Client::SharedPtr resume_client_; +}; + +rclcpp::executors::SingleThreadedExecutor::SharedPtr +PauseResumeControllerTestFixture::executor_ = nullptr; +rclcpp::CallbackGroup::SharedPtr +PauseResumeControllerTestFixture::cb_group_ = nullptr; +rclcpp::Client::SharedPtr +PauseResumeControllerTestFixture::pause_client_ = nullptr; +rclcpp::Client::SharedPtr +PauseResumeControllerTestFixture::resume_client_ = nullptr; +BT::NodeConfiguration * PauseResumeControllerTestFixture::config_ = nullptr; +std::shared_ptr PauseResumeControllerTestFixture::factory_ = nullptr; +std::shared_ptr PauseResumeControllerTestFixture::tree_ = nullptr; + +TEST_F(PauseResumeControllerTestFixture, test_incorrect_num_children) +{ + // create tree with incorrect number of children + std::string xml_txt = + R"( + + + + + + )"; + EXPECT_THROW( + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard), + BT::RuntimeError); +} + +TEST_F(PauseResumeControllerTestFixture, test_unused_children) +{ + // create tree with only RESUMED child + std::string xml_txt = + R"( + + + + + + + )"; + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // get pause_resume_controller so we can check state + auto pause_bt_node = + nav2_behavior_tree::get_node_from_tree(tree_); + ASSERT_NE(pause_bt_node, nullptr); + using state_t = nav2_behavior_tree::state_t; + + // get dummy nodes so we can change their status + auto resumed_child = + nav2_behavior_tree::get_node_from_tree(tree_, 0); + ASSERT_NE(resumed_child, nullptr); + resumed_child->changeStatus(BT::NodeStatus::RUNNING); + + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + + const auto & check_request_succeeded = []( + rclcpp::Client::FutureAndRequestId & future) + { + executor_->spin_until_future_complete(future, std::chrono::seconds(1)); + ASSERT_EQ(future.wait_for(std::chrono::seconds(0)), std::future_status::ready); + EXPECT_EQ(future.get()->success, true); + }; + + // Call pause service, expect RUNNING and PAUSED + auto future = pause_client_->async_send_request( + std::make_shared()); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + check_request_succeeded(future); + + // Tick again, expect RUNNING and PAUSED + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + + // Call resume service, expect RUNNING and ON_RESUME + future = resume_client_->async_send_request( + std::make_shared()); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + check_request_succeeded(future); + + // Tick again, expect RUNNING and RESUMED + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); +} + +TEST_F(PauseResumeControllerTestFixture, test_behavior) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + + + + )"; + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // get pause_resume_controller so we can check state + auto pause_bt_node = + nav2_behavior_tree::get_node_from_tree(tree_); + ASSERT_NE(pause_bt_node, nullptr); + using state_t = nav2_behavior_tree::state_t; + + // get dummy nodes so we can change their status + auto resumed_child = + nav2_behavior_tree::get_node_from_tree(tree_, 0); + auto paused_child = + nav2_behavior_tree::get_node_from_tree(tree_, 1); + auto on_pause_child = + nav2_behavior_tree::get_node_from_tree(tree_, 2); + auto on_resume_child = + nav2_behavior_tree::get_node_from_tree(tree_, 3); + ASSERT_NE(resumed_child, nullptr); + ASSERT_NE(paused_child, nullptr); + ASSERT_NE(on_pause_child, nullptr); + ASSERT_NE(on_resume_child, nullptr); + + resumed_child->changeStatus(BT::NodeStatus::RUNNING); + paused_child->changeStatus(BT::NodeStatus::RUNNING); + on_pause_child->changeStatus(BT::NodeStatus::RUNNING); + on_resume_child->changeStatus(BT::NodeStatus::RUNNING); + + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + + const auto & check_future_result = []( + rclcpp::Client::FutureAndRequestId & future, bool success = true) + -> void + { + executor_->spin_until_future_complete(future, std::chrono::seconds(1)); + ASSERT_EQ(future.wait_for(std::chrono::seconds(0)), std::future_status::ready); + EXPECT_EQ(future.get()->success, success); + }; + + // Call pause service, set ON_PAUSE child to RUNNING, expect RUNNING and ON_PAUSE + auto future = pause_client_->async_send_request( + std::make_shared()); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::ON_PAUSE); + check_future_result(future); + + // Change ON_PAUSE child to SUCCESS, expect RUNNING and PAUSED + on_pause_child->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + + // Set PAUSED child to SUCCESS, expect RUNNING and PAUSED + // (should keep ticking unless RESUMED branch succeeds) + paused_child->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + + // Set PAUSED child to SKIPPED, expect RUNNING and PAUSED + paused_child->changeStatus(BT::NodeStatus::SKIPPED); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + + // Call pause service again, expect RUNNING and PAUSED + future = pause_client_->async_send_request( + std::make_shared()); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + check_future_result(future, false); + + // Call resume service, change ON_RESUME child to FAILURE, expect FAILURE + future = resume_client_->async_send_request( + std::make_shared()); + on_resume_child->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::FAILURE); + check_future_result(future); + + // Halt the tree, expect RUNNING and RESUMED + tree_->haltTree(); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + + // Set resumed child to SUCCESS, expect SUCCESS and RESUMED + resumed_child->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + + // Call resume service again, expect RUNNING and RESUMED + future = resume_client_->async_send_request( + std::make_shared()); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + check_future_result(future, false); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/control/test_persistent_sequence.cpp b/nav2_behavior_tree/test/plugins/control/test_persistent_sequence.cpp new file mode 100644 index 00000000000..ebf6aa0d6b8 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/control/test_persistent_sequence.cpp @@ -0,0 +1,157 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// 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 "utils/test_behavior_tree_fixture.hpp" +#include "utils/test_dummy_tree_node.hpp" +#include "utils/get_node_from_tree.hpp" +#include "nav2_behavior_tree/plugins/control/persistent_sequence.hpp" + +class PersistentSequenceTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + static void SetUpTestCase() + { + factory_ = std::make_shared(); + config_ = new BT::NodeConfiguration(); + config_->blackboard = BT::Blackboard::create(); + config_->blackboard->set("seq_child_idx", 0); + + factory_->registerNodeType( + "PersistentSequence"); + + // Register dummy node for testing + factory_->registerNodeType("DummyNode"); + } + + static void TearDownTestCase() + { + if (config_) { + delete config_; + config_ = nullptr; + } + tree_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +BT::NodeConfiguration * PersistentSequenceTestFixture::config_ = nullptr; +std::shared_ptr +PersistentSequenceTestFixture::factory_ = nullptr; +std::shared_ptr PersistentSequenceTestFixture::tree_ = nullptr; + +TEST_F(PersistentSequenceTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + )"; + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + config_->blackboard->set("seq_child_idx", 0); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); +} + +TEST_F(PersistentSequenceTestFixture, test_behavior) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + + + )"; + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // get dummy nodes so we can change their status + auto child0 = + nav2_behavior_tree::get_node_from_tree(tree_, 0); + auto child1 = + nav2_behavior_tree::get_node_from_tree(tree_, 1); + auto child2 = + nav2_behavior_tree::get_node_from_tree(tree_, 2); + ASSERT_NE(child0, nullptr); + ASSERT_NE(child1, nullptr); + ASSERT_NE(child2, nullptr); + child0->changeStatus(BT::NodeStatus::RUNNING); + child1->changeStatus(BT::NodeStatus::RUNNING); + child2->changeStatus(BT::NodeStatus::RUNNING); + + // Set child index to 0 and child0 to RUNNING, expect RUNNING and idx = 0 + config_->blackboard->set("seq_child_idx", 0); + child0->changeStatus(BT::NodeStatus::RUNNING); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(config_->blackboard->get("seq_child_idx"), 0); + + // Set child0 to SUCCESS, expect SUCCESS and idx = 1 + child0->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(config_->blackboard->get("seq_child_idx"), 1); + + // Set child1 to FAILURE, expect FAILURE and idx = 0 + child1->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_EQ(config_->blackboard->get("seq_child_idx"), 0); + + // Set idx to 1, child0 to FAILURE, child1 to SKIPPED and child2 to RUNNING, + // expect RUNNING and idx = 2 + config_->blackboard->set("seq_child_idx", 1); + child0->changeStatus(BT::NodeStatus::FAILURE); + child1->changeStatus(BT::NodeStatus::SKIPPED); + child2->changeStatus(BT::NodeStatus::RUNNING); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(config_->blackboard->get("seq_child_idx"), 2); + + // Set child2 to SUCCESS, expect SUCCESS and idx = 0 + child2->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(config_->blackboard->get("seq_child_idx"), 0); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/utils/get_node_from_tree.hpp b/nav2_behavior_tree/test/utils/get_node_from_tree.hpp new file mode 100644 index 00000000000..068fc924005 --- /dev/null +++ b/nav2_behavior_tree/test/utils/get_node_from_tree.hpp @@ -0,0 +1,66 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// 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 UTILS__GET_NODE_FROM_TREE_HPP_ +#define UTILS__GET_NODE_FROM_TREE_HPP_ + +#include +#include + +#include "behaviortree_cpp/bt_factory.h" +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief Get node from tree by type and index, casted to NodeT type, const casted away + * Returns true if the node was found, false otherwise + */ +template +NodeT * get_node_from_tree( + const std::shared_ptr tree, + const size_t index = 0) +{ + static rclcpp::Logger logger = rclcpp::get_logger("nav2_behavior_tree::get_node_from_tree"); + + std::vector nodes = tree->getNodesByPath("*"); + if (nodes.empty()) { + RCLCPP_ERROR(logger, "No nodes of given type found"); + return nullptr; + } + if (nodes.size() <= index) { + RCLCPP_ERROR(logger, "Out of bounds (found %zu < %zu nodes)", nodes.size(), index); + return nullptr; + } + + const NodeT * const_bt_node = + dynamic_cast(nodes[index]); + if (const_bt_node == nullptr) { + RCLCPP_ERROR(logger, "Failed to cast node to given type"); + return nullptr; + } + + NodeT * bt_node = const_cast(const_bt_node); + if (bt_node == nullptr) { + RCLCPP_ERROR(logger, "Failed to cast away const from node"); + return nullptr; + } + + return bt_node; +} + +} // namespace nav2_behavior_tree + +#endif // UTILS__GET_NODE_FROM_TREE_HPP_ diff --git a/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp b/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp index dcfa52927ac..b4a64d7c78f 100644 --- a/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp +++ b/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp @@ -18,6 +18,7 @@ #include #include +#include namespace nav2_behavior_tree { @@ -29,17 +30,20 @@ namespace nav2_behavior_tree class DummyNode : public BT::ActionNodeBase { public: - DummyNode() + DummyNode( + const std::string & /*xml_tag_name*/ = "dummy", + const BT::NodeConfiguration & /*conf*/ = BT::NodeConfiguration()) : BT::ActionNodeBase("dummy", {}) { } void changeStatus(BT::NodeStatus status) { - if (status == BT::NodeStatus::IDLE) { + requested_status = status; + if (requested_status == BT::NodeStatus::IDLE) { resetStatus(); } else { - setStatus(status); + setStatus(requested_status); } } @@ -50,12 +54,26 @@ class DummyNode : public BT::ActionNodeBase BT::NodeStatus tick() override { + if (requested_status == BT::NodeStatus::IDLE) { + resetStatus(); + } else { + setStatus(requested_status); + } return status(); } void halt() override { + resetStatus(); } + + static BT::PortsList providedPorts() + { + return {}; + } + +protected: + BT::NodeStatus requested_status = BT::NodeStatus::IDLE; }; } // namespace nav2_behavior_tree