From 37da4eed83ebd58bcc84981dbd72ff51ebcdd1a2 Mon Sep 17 00:00:00 2001 From: redvinaa Date: Mon, 9 Jun 2025 11:46:49 +0200 Subject: [PATCH 01/16] Add pause and seq with bb memory BT nodes Signed-off-by: redvinaa --- nav2_behavior_tree/CMakeLists.txt | 6 + .../plugins/control/pause.hpp | 98 ++++++ .../sequence_with_blackboard_memory.hpp | 63 ++++ nav2_behavior_tree/nav2_tree_nodes.xml | 10 + nav2_behavior_tree/plugins/control/pause.cpp | 279 ++++++++++++++++++ .../sequence_with_blackboard_memory.cpp | 100 +++++++ 6 files changed, 556 insertions(+) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause.hpp create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp create mode 100644 nav2_behavior_tree/plugins/control/pause.cpp create mode 100644 nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 03339b19cdc..1a10f0d6993 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_bt_node SHARED plugins/control/pause.cpp) +list(APPEND plugin_libs nav2_pause_bt_node) + +add_library(nav2_sequence_with_blackboard_memory_bt_node SHARED plugins/control/sequence_with_blackboard_memory.cpp) +list(APPEND plugin_libs nav2_sequence_with_blackboard_memory_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.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause.hpp new file mode 100644 index 00000000000..67681ef741a --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause.hpp @@ -0,0 +1,98 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_HPP_ + +// Other includes +#include +#include +#include + +// ROS includes +#include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp/control_node.h" + +// Interface definitions +#include "std_srvs/srv/trigger.hpp" + + +namespace nav2_behavior_tree +{ + +using Trigger = std_srvs::srv::Trigger; + +enum state_t {UNPAUSED, PAUSED, PAUSE_REQUESTED, ON_PAUSE, RESUME_REQUESTED, ON_RESUME}; + +/* @brief Controlled through service calls to pause and resume the execution of the tree + * It has one mandatory child for the UNPAUSED, 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 + */ +class Pause : public BT::ControlNode +{ +public: + //! @brief Constructor + Pause( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + //! @brief Destructor + ~Pause(); + + //! @brief Reset state and go to Idle + void halt() override; + + //! @brief Return a NodeStatus according to the children's status + 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"), + }; + } + +private: + //! @brief Service callback to pause + void pause_service_callback( + const std::shared_ptr request, + std::shared_ptr response); + + //! @brief Service callback to resume + void resume_service_callback( + const std::shared_ptr request, + std::shared_ptr response); + + rclcpp::Node::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr cb_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + std::unique_ptr spinner_thread_; + rclcpp::Service::SharedPtr pause_srv_; + rclcpp::Service::SharedPtr resume_srv_; + state_t state_; + std::mutex state_mutex_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp new file mode 100644 index 00000000000..9c46b3e8c5a --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp @@ -0,0 +1,63 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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__SEQUENCE_WITH_BLACKBOARD_MEMORY_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__SEQUENCE_WITH_BLACKBOARD_MEMORY_HPP_ + +#include +#include "behaviortree_cpp/control_node.h" +#include "behaviortree_cpp/bt_factory.h" + +namespace nav2_behavior_tree +{ +/** + * @brief The SequenceWithBlackboardMemoryNode 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 SequenceWithBlackboardMemoryNode : public BT::ControlNode +{ +public: + SequenceWithBlackboardMemoryNode(const std::string & name, const BT::NodeConfiguration & conf); + + ~SequenceWithBlackboardMemoryNode() override = default; + + void halt() override; + + //! @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__SEQUENCE_WITH_BLACKBOARD_MEMORY_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index a9798c0c64c..8860ac560f3 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.cpp b/nav2_behavior_tree/plugins/control/pause.cpp new file mode 100644 index 00000000000..241118dc844 --- /dev/null +++ b/nav2_behavior_tree/plugins/control/pause.cpp @@ -0,0 +1,279 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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 + +// ROS includes +#include "nav2_behavior_tree/plugins/control/pause.hpp" +#include "rclcpp/callback_group.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" + +// Other includes +#include "behaviortree_cpp/bt_factory.h" + +namespace nav2_behavior_tree +{ + +using namespace std::placeholders; + +Pause::Pause( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf) +: BT::ControlNode(xml_tag_name, conf) +{ + node_ = this->config().blackboard->get("node"); + state_ = UNPAUSED; + + // 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_ = node_->create_service( + pause_service_name, + std::bind(&Pause::pause_service_callback, this, _1, _2), + rclcpp::ServicesQoS(), cb_group_); + + std::string resume_service_name; + getInput("resume_service_name", resume_service_name); + resume_srv_ = node_->create_service( + resume_service_name, + std::bind(&Pause::resume_service_callback, this, _1, _2), + rclcpp::ServicesQoS(), cb_group_); + + spinner_thread_ = std::make_unique( + [&]() { + executor_->spin(); + }); + spinner_thread_->detach(); +} + +Pause::~Pause() +{ + RCLCPP_DEBUG(node_->get_logger(), "Shutting down Pause BT node"); + executor_->cancel(); +} + +BT::NodeStatus Pause::tick() +{ + unsigned int children_count = children_nodes_.size(); + if (children_count < 1 || children_count > 4) { + throw BT::LogicError( + "PauseNode must have at least one and at most four children " + "(currently has " + std::to_string(children_count) + ")"); + } + + std::lock_guard lock(state_mutex_); + if (status() == BT::NodeStatus::IDLE) { + state_ = UNPAUSED; + } + if (state_ == PAUSE_REQUESTED) { + resetChildren(); + state_ = ON_PAUSE; + RCLCPP_INFO(node_->get_logger(), "Switched to state: ON_PAUSE"); + } + if (state_ == RESUME_REQUESTED) { + resetChildren(); + state_ = ON_RESUME; + RCLCPP_INFO(node_->get_logger(), "Switched to state: ON_RESUME"); + } + setStatus(BT::NodeStatus::RUNNING); + + if (state_ == ON_PAUSE) { + if (children_count < 3) { + // Event not handled, go directly to PAUSED state + RCLCPP_INFO(node_->get_logger(), "Switched to state: PAUSED"); + state_ = PAUSED; + } else { + // Tick ON_PAUSE child + const BT::NodeStatus child_status = children_nodes_[2]->executeTick(); + switch (child_status) { + case BT::NodeStatus::RUNNING: + break; + case BT::NodeStatus::SUCCESS: + RCLCPP_INFO(node_->get_logger(), "Switched to state: PAUSED"); + state_ = PAUSED; + break; + case BT::NodeStatus::SKIPPED: + // Same as SUCCESS + RCLCPP_INFO(node_->get_logger(), "Switched to state: PAUSED"); + state_ = PAUSED; + break; + case BT::NodeStatus::FAILURE: + RCLCPP_ERROR(node_->get_logger(), "ON_PAUSE child returned FAILURE"); + setStatus(BT::NodeStatus::FAILURE); + break; + default: + throw BT::LogicError("A child node must never return IDLE"); + } + } + } + + if (state_ == ON_RESUME) { + if (children_count < 4) { + // Event not handled, go directly to UNPAUSED state + RCLCPP_INFO(node_->get_logger(), "Switched to state: UNPAUSED"); + state_ = UNPAUSED; + } else { + // Tick ON_RESUME child + const BT::NodeStatus child_status = children_nodes_[3]->executeTick(); + switch (child_status) { + case BT::NodeStatus::RUNNING: + break; + case BT::NodeStatus::SUCCESS: + RCLCPP_INFO(node_->get_logger(), "Switched to state: UNPAUSED"); + state_ = UNPAUSED; + break; + case BT::NodeStatus::SKIPPED: + // Same as SUCCESS + RCLCPP_INFO(node_->get_logger(), "Switched to state: UNPAUSED"); + state_ = UNPAUSED; + break; + case BT::NodeStatus::FAILURE: + RCLCPP_ERROR(node_->get_logger(), "ON_RESUME child returned FAILURE"); + setStatus(BT::NodeStatus::FAILURE); + break; + default: + throw BT::LogicError("A child node must never return IDLE"); + } + } + } + + if (state_ == PAUSED) { + if (children_count < 2) { + // State not handled, do nothing until unpaused + } else { + // Tick PAUSED child + const BT::NodeStatus child_status = children_nodes_[1]->executeTick(); + switch (child_status) { + case BT::NodeStatus::RUNNING: + break; + case BT::NodeStatus::SUCCESS: + break; + case BT::NodeStatus::SKIPPED: + // Same as SUCCESS + break; + case BT::NodeStatus::FAILURE: + RCLCPP_ERROR(node_->get_logger(), "PAUSED child returned FAILURE"); + setStatus(BT::NodeStatus::FAILURE); + break; + default: + throw BT::LogicError("A child node must never return IDLE"); + } + } + } + + if (state_ == UNPAUSED) { + // Tick UNPAUSED child + const BT::NodeStatus child_status = children_nodes_[0]->executeTick(); + switch (child_status) { + case BT::NodeStatus::RUNNING: + break; + case BT::NodeStatus::SUCCESS: + setStatus(BT::NodeStatus::SUCCESS); + break; + case BT::NodeStatus::SKIPPED: + // Same as SUCCESS + setStatus(BT::NodeStatus::SUCCESS); + break; + case BT::NodeStatus::FAILURE: + RCLCPP_ERROR(node_->get_logger(), "UNPAUSED child returned FAILURE"); + setStatus(BT::NodeStatus::FAILURE); + break; + default: + throw BT::LogicError("A child node must never return IDLE"); + } + } + + return status(); +} + +void Pause::pause_service_callback( + const std::shared_ptr/*request*/, + std::shared_ptr response) +{ + if (status() == BT::NodeStatus::IDLE) { + std::string warn_msg = "Pause BT node has not been ticked yet"; + response->success = false; + response->message = warn_msg; + RCLCPP_ERROR(node_->get_logger(), "%s", warn_msg.c_str()); + return; + } + + std::lock_guard lock(state_mutex_); + if (state_ != PAUSED) { + RCLCPP_INFO(node_->get_logger(), "PAUSE_REQUESTED"); + response->success = true; + state_ = PAUSE_REQUESTED; + return; + } + + std::string warn_message = "Pause BT node already in state PAUSED"; + RCLCPP_WARN(node_->get_logger(), "%s", warn_message.c_str()); + response->success = false; + response->message = warn_message; +} + +void Pause::resume_service_callback( + const std::shared_ptr/*request*/, + std::shared_ptr response) +{ + if (status() == BT::NodeStatus::IDLE) { + std::string warn_msg = "Pause BT node has not been ticked yet"; + response->success = false; + response->message = warn_msg; + RCLCPP_ERROR(node_->get_logger(), "%s", warn_msg.c_str()); + return; + } + + std::lock_guard lock(state_mutex_); + if (state_ == PAUSED) { + RCLCPP_INFO(node_->get_logger(), "RESUME_REQUESTED"); + response->success = true; + state_ = RESUME_REQUESTED; + return; + } + + std::string warn_message = "You can't resume a node that is not paused"; + RCLCPP_WARN(node_->get_logger(), "%s", warn_message.c_str()); + response->success = false; + response->message = warn_message; +} + +void Pause::halt() +{ + std::lock_guard lock(state_mutex_); + state_ = UNPAUSED; + 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("Pause", builder); +} diff --git a/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp b/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp new file mode 100644 index 00000000000..3c85e705c76 --- /dev/null +++ b/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp @@ -0,0 +1,100 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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/sequence_with_blackboard_memory.hpp" +#include "behaviortree_cpp/action_node.h" +#include "behaviortree_cpp/bt_factory.h" + +namespace nav2_behavior_tree +{ + +SequenceWithBlackboardMemoryNode::SequenceWithBlackboardMemoryNode( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ControlNode::ControlNode(name, conf) +{ + setRegistrationID("SequenceWithBlackboardMemory"); +} + +void SequenceWithBlackboardMemoryNode::halt() +{ + ControlNode::halt(); +} + +BT::NodeStatus SequenceWithBlackboardMemoryNode::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: { + current_child_idx++; + setOutput("current_child_idx", current_child_idx); + } + break; + + case BT::NodeStatus::SKIPPED: { + // Same as SUCCESS + // Skip the child node + current_child_idx++; + setOutput("current_child_idx", current_child_idx); + } + break; + + case BT::NodeStatus::IDLE: { + throw BT::LogicError("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( + "SequenceWithBlackboardMemory", builder); +} From 752eded6c701c42e29aea64cc3deda4619a7e40b Mon Sep 17 00:00:00 2001 From: redvinaa Date: Tue, 10 Jun 2025 13:58:15 +0200 Subject: [PATCH 02/16] Requested changes Signed-off-by: redvinaa --- nav2_behavior_tree/CMakeLists.txt | 4 +- ...{pause.hpp => pause_resume_controller.hpp} | 57 +++- .../sequence_with_blackboard_memory.hpp | 4 +- nav2_behavior_tree/nav2_tree_nodes.xml | 2 +- nav2_behavior_tree/plugins/control/pause.cpp | 279 ------------------ .../control/pause_resume_controller.cpp | 211 +++++++++++++ .../sequence_with_blackboard_memory.cpp | 38 +-- 7 files changed, 270 insertions(+), 325 deletions(-) rename nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/{pause.hpp => pause_resume_controller.hpp} (66%) delete mode 100644 nav2_behavior_tree/plugins/control/pause.cpp create mode 100644 nav2_behavior_tree/plugins/control/pause_resume_controller.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 1a10f0d6993..93359bf8738 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -218,8 +218,8 @@ 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_bt_node SHARED plugins/control/pause.cpp) -list(APPEND plugin_libs nav2_pause_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_sequence_with_blackboard_memory_bt_node SHARED plugins/control/sequence_with_blackboard_memory.cpp) list(APPEND plugin_libs nav2_sequence_with_blackboard_memory_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp similarity index 66% rename from nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause.hpp rename to nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp index 67681ef741a..a7a0193c5d8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2025 Intel Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -18,11 +18,14 @@ // Other includes #include #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_util/service_server.hpp" // Interface definitions #include "std_srvs/srv/trigger.hpp" @@ -34,6 +37,20 @@ namespace nav2_behavior_tree using Trigger = std_srvs::srv::Trigger; enum state_t {UNPAUSED, PAUSED, PAUSE_REQUESTED, ON_PAUSE, RESUME_REQUESTED, ON_RESUME}; +const std::map state_names = { + {UNPAUSED, "UNPAUSED"}, + {PAUSED, "PAUSED"}, + {PAUSE_REQUESTED, "PAUSE_REQUESTED"}, + {ON_PAUSE, "ON_PAUSE"}, + {RESUME_REQUESTED, "RESUME_REQUESTED"}, + {ON_RESUME, "ON_RESUME"} +}; +const std::map child_indices = { + {UNPAUSED, 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 UNPAUSED, and three optional for the PAUSED state, @@ -41,18 +58,28 @@ enum state_t {UNPAUSED, PAUSED, PAUSE_REQUESTED, ON_PAUSE, RESUME_REQUESTED, ON_ * It has two input ports: * - pause_service_name: name of the service to pause * - resume_service_name: name of the service to resume + * + * Usage: + * + * + * + * + * + * + * + * + * */ -class Pause : public BT::ControlNode + + +class PauseResumeController : public BT::ControlNode { public: //! @brief Constructor - Pause( + PauseResumeController( const std::string & xml_tag_name, const BT::NodeConfiguration & conf); - //! @brief Destructor - ~Pause(); - //! @brief Reset state and go to Idle void halt() override; @@ -74,23 +101,27 @@ class Pause : public BT::ControlNode private: //! @brief Service callback to pause - void pause_service_callback( + void pauseServiceCallback( + const std::shared_ptr/*request_header*/, const std::shared_ptr request, std::shared_ptr response); //! @brief Service callback to resume - void resume_service_callback( + void resumeServiceCallback( + const std::shared_ptr/*request_header*/, const std::shared_ptr request, std::shared_ptr response); + BT::NodeStatus tickChildAndTransition(); + + void switchState(const state_t new_state); + rclcpp::Node::SharedPtr node_; rclcpp::CallbackGroup::SharedPtr cb_group_; rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; - std::unique_ptr spinner_thread_; - rclcpp::Service::SharedPtr pause_srv_; - rclcpp::Service::SharedPtr resume_srv_; + nav2_util::ServiceServer::SharedPtr pause_srv_; + nav2_util::ServiceServer::SharedPtr resume_srv_; state_t state_; - std::mutex state_mutex_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp index 9c46b3e8c5a..7e995076473 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2025 Intel Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -44,8 +44,6 @@ class SequenceWithBlackboardMemoryNode : public BT::ControlNode ~SequenceWithBlackboardMemoryNode() override = default; - void halt() override; - //! @brief Declare ports static BT::PortsList providedPorts() { diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 8860ac560f3..8456c2133da 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -405,7 +405,7 @@ - + Service to call to pause Service to call to resume diff --git a/nav2_behavior_tree/plugins/control/pause.cpp b/nav2_behavior_tree/plugins/control/pause.cpp deleted file mode 100644 index 241118dc844..00000000000 --- a/nav2_behavior_tree/plugins/control/pause.cpp +++ /dev/null @@ -1,279 +0,0 @@ -// Copyright (c) 2019 Intel Corporation -// -// 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 - -// ROS includes -#include "nav2_behavior_tree/plugins/control/pause.hpp" -#include "rclcpp/callback_group.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" - -// Other includes -#include "behaviortree_cpp/bt_factory.h" - -namespace nav2_behavior_tree -{ - -using namespace std::placeholders; - -Pause::Pause( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) -: BT::ControlNode(xml_tag_name, conf) -{ - node_ = this->config().blackboard->get("node"); - state_ = UNPAUSED; - - // 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_ = node_->create_service( - pause_service_name, - std::bind(&Pause::pause_service_callback, this, _1, _2), - rclcpp::ServicesQoS(), cb_group_); - - std::string resume_service_name; - getInput("resume_service_name", resume_service_name); - resume_srv_ = node_->create_service( - resume_service_name, - std::bind(&Pause::resume_service_callback, this, _1, _2), - rclcpp::ServicesQoS(), cb_group_); - - spinner_thread_ = std::make_unique( - [&]() { - executor_->spin(); - }); - spinner_thread_->detach(); -} - -Pause::~Pause() -{ - RCLCPP_DEBUG(node_->get_logger(), "Shutting down Pause BT node"); - executor_->cancel(); -} - -BT::NodeStatus Pause::tick() -{ - unsigned int children_count = children_nodes_.size(); - if (children_count < 1 || children_count > 4) { - throw BT::LogicError( - "PauseNode must have at least one and at most four children " - "(currently has " + std::to_string(children_count) + ")"); - } - - std::lock_guard lock(state_mutex_); - if (status() == BT::NodeStatus::IDLE) { - state_ = UNPAUSED; - } - if (state_ == PAUSE_REQUESTED) { - resetChildren(); - state_ = ON_PAUSE; - RCLCPP_INFO(node_->get_logger(), "Switched to state: ON_PAUSE"); - } - if (state_ == RESUME_REQUESTED) { - resetChildren(); - state_ = ON_RESUME; - RCLCPP_INFO(node_->get_logger(), "Switched to state: ON_RESUME"); - } - setStatus(BT::NodeStatus::RUNNING); - - if (state_ == ON_PAUSE) { - if (children_count < 3) { - // Event not handled, go directly to PAUSED state - RCLCPP_INFO(node_->get_logger(), "Switched to state: PAUSED"); - state_ = PAUSED; - } else { - // Tick ON_PAUSE child - const BT::NodeStatus child_status = children_nodes_[2]->executeTick(); - switch (child_status) { - case BT::NodeStatus::RUNNING: - break; - case BT::NodeStatus::SUCCESS: - RCLCPP_INFO(node_->get_logger(), "Switched to state: PAUSED"); - state_ = PAUSED; - break; - case BT::NodeStatus::SKIPPED: - // Same as SUCCESS - RCLCPP_INFO(node_->get_logger(), "Switched to state: PAUSED"); - state_ = PAUSED; - break; - case BT::NodeStatus::FAILURE: - RCLCPP_ERROR(node_->get_logger(), "ON_PAUSE child returned FAILURE"); - setStatus(BT::NodeStatus::FAILURE); - break; - default: - throw BT::LogicError("A child node must never return IDLE"); - } - } - } - - if (state_ == ON_RESUME) { - if (children_count < 4) { - // Event not handled, go directly to UNPAUSED state - RCLCPP_INFO(node_->get_logger(), "Switched to state: UNPAUSED"); - state_ = UNPAUSED; - } else { - // Tick ON_RESUME child - const BT::NodeStatus child_status = children_nodes_[3]->executeTick(); - switch (child_status) { - case BT::NodeStatus::RUNNING: - break; - case BT::NodeStatus::SUCCESS: - RCLCPP_INFO(node_->get_logger(), "Switched to state: UNPAUSED"); - state_ = UNPAUSED; - break; - case BT::NodeStatus::SKIPPED: - // Same as SUCCESS - RCLCPP_INFO(node_->get_logger(), "Switched to state: UNPAUSED"); - state_ = UNPAUSED; - break; - case BT::NodeStatus::FAILURE: - RCLCPP_ERROR(node_->get_logger(), "ON_RESUME child returned FAILURE"); - setStatus(BT::NodeStatus::FAILURE); - break; - default: - throw BT::LogicError("A child node must never return IDLE"); - } - } - } - - if (state_ == PAUSED) { - if (children_count < 2) { - // State not handled, do nothing until unpaused - } else { - // Tick PAUSED child - const BT::NodeStatus child_status = children_nodes_[1]->executeTick(); - switch (child_status) { - case BT::NodeStatus::RUNNING: - break; - case BT::NodeStatus::SUCCESS: - break; - case BT::NodeStatus::SKIPPED: - // Same as SUCCESS - break; - case BT::NodeStatus::FAILURE: - RCLCPP_ERROR(node_->get_logger(), "PAUSED child returned FAILURE"); - setStatus(BT::NodeStatus::FAILURE); - break; - default: - throw BT::LogicError("A child node must never return IDLE"); - } - } - } - - if (state_ == UNPAUSED) { - // Tick UNPAUSED child - const BT::NodeStatus child_status = children_nodes_[0]->executeTick(); - switch (child_status) { - case BT::NodeStatus::RUNNING: - break; - case BT::NodeStatus::SUCCESS: - setStatus(BT::NodeStatus::SUCCESS); - break; - case BT::NodeStatus::SKIPPED: - // Same as SUCCESS - setStatus(BT::NodeStatus::SUCCESS); - break; - case BT::NodeStatus::FAILURE: - RCLCPP_ERROR(node_->get_logger(), "UNPAUSED child returned FAILURE"); - setStatus(BT::NodeStatus::FAILURE); - break; - default: - throw BT::LogicError("A child node must never return IDLE"); - } - } - - return status(); -} - -void Pause::pause_service_callback( - const std::shared_ptr/*request*/, - std::shared_ptr response) -{ - if (status() == BT::NodeStatus::IDLE) { - std::string warn_msg = "Pause BT node has not been ticked yet"; - response->success = false; - response->message = warn_msg; - RCLCPP_ERROR(node_->get_logger(), "%s", warn_msg.c_str()); - return; - } - - std::lock_guard lock(state_mutex_); - if (state_ != PAUSED) { - RCLCPP_INFO(node_->get_logger(), "PAUSE_REQUESTED"); - response->success = true; - state_ = PAUSE_REQUESTED; - return; - } - - std::string warn_message = "Pause BT node already in state PAUSED"; - RCLCPP_WARN(node_->get_logger(), "%s", warn_message.c_str()); - response->success = false; - response->message = warn_message; -} - -void Pause::resume_service_callback( - const std::shared_ptr/*request*/, - std::shared_ptr response) -{ - if (status() == BT::NodeStatus::IDLE) { - std::string warn_msg = "Pause BT node has not been ticked yet"; - response->success = false; - response->message = warn_msg; - RCLCPP_ERROR(node_->get_logger(), "%s", warn_msg.c_str()); - return; - } - - std::lock_guard lock(state_mutex_); - if (state_ == PAUSED) { - RCLCPP_INFO(node_->get_logger(), "RESUME_REQUESTED"); - response->success = true; - state_ = RESUME_REQUESTED; - return; - } - - std::string warn_message = "You can't resume a node that is not paused"; - RCLCPP_WARN(node_->get_logger(), "%s", warn_message.c_str()); - response->success = false; - response->message = warn_message; -} - -void Pause::halt() -{ - std::lock_guard lock(state_mutex_); - state_ = UNPAUSED; - 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("Pause", builder); -} 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..e468d00ad85 --- /dev/null +++ b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp @@ -0,0 +1,211 @@ +// Copyright (c) 2025 Intel Corporation +// +// 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) +{ + node_ = this->config().blackboard->get("node"); + state_ = UNPAUSED; + + // 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), + rclcpp::ServicesQoS(), 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), + rclcpp::ServicesQoS(), 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_ = UNPAUSED; + } + setStatus(BT::NodeStatus::RUNNING); + + executor_->spin_some(); + + if (state_ == PAUSE_REQUESTED) { + resetChildren(); + switchState(ON_PAUSE); + } + if (state_ == RESUME_REQUESTED) { + resetChildren(); + switchState(ON_RESUME); + } + + tickChildAndTransition(); + + return status(); +} + +void PauseResumeController::switchState(const state_t new_state) +{ + if (state_ == new_state) { + RCLCPP_WARN(node_->get_logger(), "Already in state: %s", state_names.at(state_).c_str()); + return; + } + + state_ = new_state; + RCLCPP_INFO(node_->get_logger(), "Switched to state: %s", state_names.at(state_).c_str()); +} + +BT::NodeStatus PauseResumeController::tickChildAndTransition() +{ + // Return RUNNING and do nothing if specific child is not used + const uint child_idx = child_indices.at(state_); + if (children_nodes_.size() <= child_idx) { + if (state_ == ON_PAUSE) { + switchState(PAUSED); + } else if (state_ == ON_RESUME) { + switchState(UNPAUSED); + } + + 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_ == ON_PAUSE) { + switchState(PAUSED); + } else if (state_ == ON_RESUME) { + switchState(UNPAUSED); + } + return BT::NodeStatus::SUCCESS; + case BT::NodeStatus::FAILURE: + RCLCPP_ERROR( + node_->get_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::pauseServiceCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response) +{ + if (status() == BT::NodeStatus::IDLE) { + std::string warn_msg = "PauseResumeController BT node has not been ticked yet"; + response->success = false; + response->message = warn_msg; + RCLCPP_ERROR(node_->get_logger(), "%s", warn_msg.c_str()); + return; + } + + if (state_ != PAUSED) { + RCLCPP_INFO(node_->get_logger(), "PAUSE_REQUESTED"); + response->success = true; + state_ = PAUSE_REQUESTED; + return; + } + + std::string warn_message = "PauseResumeController BT node already in state PAUSED"; + RCLCPP_WARN(node_->get_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 (status() == BT::NodeStatus::IDLE) { + std::string warn_msg = "PauseResumeController BT node has not been ticked yet"; + response->success = false; + response->message = warn_msg; + RCLCPP_ERROR(node_->get_logger(), "%s", warn_msg.c_str()); + return; + } + + if (state_ == PAUSED) { + RCLCPP_INFO(node_->get_logger(), "RESUME_REQUESTED"); + response->success = true; + state_ = RESUME_REQUESTED; + return; + } + + std::string warn_message = "PauseResumeController BT node not in state PAUSED"; + RCLCPP_WARN(node_->get_logger(), "%s", warn_message.c_str()); + response->success = false; + response->message = warn_message; +} + +void PauseResumeController::halt() +{ + state_ = UNPAUSED; + 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/sequence_with_blackboard_memory.cpp b/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp index 3c85e705c76..2ff879a3cf6 100644 --- a/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp +++ b/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2025 Intel Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -22,15 +22,7 @@ namespace nav2_behavior_tree SequenceWithBlackboardMemoryNode::SequenceWithBlackboardMemoryNode( const std::string & name, const BT::NodeConfiguration & conf) -: BT::ControlNode::ControlNode(name, conf) -{ - setRegistrationID("SequenceWithBlackboardMemory"); -} - -void SequenceWithBlackboardMemoryNode::halt() -{ - ControlNode::halt(); -} +: BT::ControlNode::ControlNode(name, conf) {} BT::NodeStatus SequenceWithBlackboardMemoryNode::tick() { @@ -46,35 +38,27 @@ BT::NodeStatus SequenceWithBlackboardMemoryNode::tick() const BT::NodeStatus child_status = current_child_node->executeTick(); switch (child_status) { - case BT::NodeStatus::RUNNING: { + case BT::NodeStatus::RUNNING: return child_status; - } - case BT::NodeStatus::FAILURE: { + + case BT::NodeStatus::FAILURE: // Reset on failure resetChildren(); current_child_idx = 0; setOutput("current_child_idx", 0); return child_status; - } - case BT::NodeStatus::SUCCESS: { - current_child_idx++; - setOutput("current_child_idx", current_child_idx); - } - break; - case BT::NodeStatus::SKIPPED: { - // Same as SUCCESS + 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 BT::LogicError("A child node must never return IDLE"); - } - } // end switch - } // end while loop + 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) { From 0aeea3d1499bc9ed830cc2b544621dae2e9f3063 Mon Sep 17 00:00:00 2001 From: redvinaa Date: Tue, 10 Jun 2025 12:54:26 +0000 Subject: [PATCH 03/16] Lint Signed-off-by: redvinaa --- .../control/pause_resume_controller.hpp | 6 +++--- .../control/pause_resume_controller.cpp | 3 ++- .../sequence_with_blackboard_memory.cpp | 20 +++++++++---------- 3 files changed, 15 insertions(+), 14 deletions(-) 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 index a7a0193c5d8..f6b469418f1 100644 --- 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 @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_HPP_ -#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_HPP_ +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_ // Other includes #include @@ -126,4 +126,4 @@ class PauseResumeController : public BT::ControlNode } // namespace nav2_behavior_tree -#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_HPP_ +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_ diff --git a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp index e468d00ad85..57cdbdd503c 100644 --- a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp +++ b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp @@ -207,5 +207,6 @@ BT_REGISTER_NODES(factory) name, config); }; - factory.registerBuilder("PauseResumeController", builder); + factory.registerBuilder( + "PauseResumeController", builder); } diff --git a/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp b/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp index 2ff879a3cf6..d751558b323 100644 --- a/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp +++ b/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp @@ -39,24 +39,24 @@ BT::NodeStatus SequenceWithBlackboardMemoryNode::tick() switch (child_status) { case BT::NodeStatus::RUNNING: - return child_status; + return child_status; case BT::NodeStatus::FAILURE: - // Reset on failure - resetChildren(); - current_child_idx = 0; - setOutput("current_child_idx", 0); - return child_status; + // 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); + // 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"); + throw std::runtime_error("A child node must never return IDLE"); } // end switch } // end while loop From bf1de2a76fc28ec62478abaf48606b9b0ca13df0 Mon Sep 17 00:00:00 2001 From: redvinaa Date: Fri, 20 Jun 2025 15:25:26 +0200 Subject: [PATCH 04/16] Restructure pause, rename unpaused state to resumed Signed-off-by: redvinaa --- .../control/pause_resume_controller.hpp | 38 ++++---- .../control/pause_resume_controller.cpp | 90 ++++++++----------- 2 files changed, 62 insertions(+), 66 deletions(-) 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 index f6b469418f1..652e33f95d5 100644 --- 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 @@ -36,32 +36,32 @@ namespace nav2_behavior_tree using Trigger = std_srvs::srv::Trigger; -enum state_t {UNPAUSED, PAUSED, PAUSE_REQUESTED, ON_PAUSE, RESUME_REQUESTED, ON_RESUME}; +enum state_t {RESUMED, PAUSED, PAUSE_REQUESTED, ON_PAUSE, RESUME_REQUESTED, ON_RESUME}; const std::map state_names = { - {UNPAUSED, "UNPAUSED"}, + {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 = { - {UNPAUSED, 0}, +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 UNPAUSED, and three optional for the PAUSED state, + * 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: - * - * + * + * * * * @@ -83,7 +83,7 @@ class PauseResumeController : public BT::ControlNode //! @brief Reset state and go to Idle void halt() override; - //! @brief Return a NodeStatus according to the children's status + //! @brief Handle transitions if requested and tick child related to the actual state BT::NodeStatus tick() override; //! @brief Declare ports @@ -100,23 +100,31 @@ class PauseResumeController : public BT::ControlNode } private: - //! @brief Service callback to pause + //! @brief Set state to PAUSE_REQUESTED void pauseServiceCallback( const std::shared_ptr/*request_header*/, const std::shared_ptr request, std::shared_ptr response); - //! @brief Service callback to resume + //! @brief Set state to RESUME_REQUESTED void resumeServiceCallback( const std::shared_ptr/*request_header*/, const std::shared_ptr request, std::shared_ptr response); - BT::NodeStatus tickChildAndTransition(); - - void switchState(const state_t new_state); - - rclcpp::Node::SharedPtr node_; + /** @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_util::ServiceServer::SharedPtr pause_srv_; diff --git a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp index 57cdbdd503c..aef23651226 100644 --- a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp +++ b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp @@ -31,23 +31,23 @@ PauseResumeController::PauseResumeController( const BT::NodeConfiguration & conf) : BT::ControlNode(xml_tag_name, conf) { - node_ = this->config().blackboard->get("node"); - state_ = UNPAUSED; + 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( + 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()); + 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_, + node, std::bind(&PauseResumeController::pauseServiceCallback, this, _1, _2, _3), rclcpp::ServicesQoS(), cb_group_); @@ -55,7 +55,7 @@ PauseResumeController::PauseResumeController( getInput("resume_service_name", resume_service_name); resume_srv_ = std::make_shared>( resume_service_name, - node_, + node, std::bind(&PauseResumeController::resumeServiceCallback, this, _1, _2, _3), rclcpp::ServicesQoS(), cb_group_); } @@ -70,48 +70,22 @@ BT::NodeStatus PauseResumeController::tick() } if (status() == BT::NodeStatus::IDLE) { - state_ = UNPAUSED; + state_ = RESUMED; } setStatus(BT::NodeStatus::RUNNING); executor_->spin_some(); - if (state_ == PAUSE_REQUESTED) { + // If pause / resume requested, reset children and switch to transient child + if (state_ == PAUSE_REQUESTED || state_ == RESUME_REQUESTED) { resetChildren(); - switchState(ON_PAUSE); + switchToNextState(); } - if (state_ == RESUME_REQUESTED) { - resetChildren(); - switchState(ON_RESUME); - } - - tickChildAndTransition(); - - return status(); -} -void PauseResumeController::switchState(const state_t new_state) -{ - if (state_ == new_state) { - RCLCPP_WARN(node_->get_logger(), "Already in state: %s", state_names.at(state_).c_str()); - return; - } - - state_ = new_state; - RCLCPP_INFO(node_->get_logger(), "Switched to state: %s", state_names.at(state_).c_str()); -} - -BT::NodeStatus PauseResumeController::tickChildAndTransition() -{ // Return RUNNING and do nothing if specific child is not used - const uint child_idx = child_indices.at(state_); + const uint16_t child_idx = child_indices.at(state_); if (children_nodes_.size() <= child_idx) { - if (state_ == ON_PAUSE) { - switchState(PAUSED); - } else if (state_ == ON_RESUME) { - switchState(UNPAUSED); - } - + switchToNextState(); return BT::NodeStatus::RUNNING; } @@ -123,21 +97,35 @@ BT::NodeStatus PauseResumeController::tickChildAndTransition() return BT::NodeStatus::RUNNING; case BT::NodeStatus::SUCCESS: case BT::NodeStatus::SKIPPED: - if (state_ == ON_PAUSE) { - switchState(PAUSED); - } else if (state_ == ON_RESUME) { - switchState(UNPAUSED); - } + switchToNextState(); return BT::NodeStatus::SUCCESS; case BT::NodeStatus::FAILURE: RCLCPP_ERROR( - node_->get_logger(), "%s child returned FAILURE", state_names.at(state_).c_str()); + 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*/, @@ -147,19 +135,19 @@ void PauseResumeController::pauseServiceCallback( std::string warn_msg = "PauseResumeController BT node has not been ticked yet"; response->success = false; response->message = warn_msg; - RCLCPP_ERROR(node_->get_logger(), "%s", warn_msg.c_str()); + RCLCPP_ERROR(logger_, "%s", warn_msg.c_str()); return; } if (state_ != PAUSED) { - RCLCPP_INFO(node_->get_logger(), "PAUSE_REQUESTED"); + 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(node_->get_logger(), "%s", warn_message.c_str()); + RCLCPP_WARN(logger_, "%s", warn_message.c_str()); response->success = false; response->message = warn_message; } @@ -173,26 +161,26 @@ void PauseResumeController::resumeServiceCallback( std::string warn_msg = "PauseResumeController BT node has not been ticked yet"; response->success = false; response->message = warn_msg; - RCLCPP_ERROR(node_->get_logger(), "%s", warn_msg.c_str()); + RCLCPP_ERROR(logger_, "%s", warn_msg.c_str()); return; } if (state_ == PAUSED) { - RCLCPP_INFO(node_->get_logger(), "RESUME_REQUESTED"); + 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(node_->get_logger(), "%s", warn_message.c_str()); + RCLCPP_WARN(logger_, "%s", warn_message.c_str()); response->success = false; response->message = warn_message; } void PauseResumeController::halt() { - state_ = UNPAUSED; + state_ = RESUMED; ControlNode::halt(); } From 78dbcf5ae432fde9be99001499e5889bf75428e5 Mon Sep 17 00:00:00 2001 From: redvinaa Date: Fri, 20 Jun 2025 17:30:08 +0200 Subject: [PATCH 05/16] Add PauseResumeController test Signed-off-by: redvinaa --- .../test/plugins/control/CMakeLists.txt | 3 + .../control/test_pause_resume_controller.cpp | 150 ++++++++++++++++++ 2 files changed, 153 insertions(+) create mode 100644 nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp diff --git a/nav2_behavior_tree/test/plugins/control/CMakeLists.txt b/nav2_behavior_tree/test/plugins/control/CMakeLists.txt index 1a8b0cf781a..dfa95019d5d 100644 --- a/nav2_behavior_tree/test/plugins/control/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/control/CMakeLists.txt @@ -5,3 +5,6 @@ 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) 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..a906750900c --- /dev/null +++ b/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp @@ -0,0 +1,150 @@ +// Copyright (c) 2025 Intel Corporation +// +// 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 "nav2_behavior_tree/plugins/control/pause_resume_controller.hpp" +#include "std_srvs/srv/trigger.hpp" + +class PauseResumeControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + void SetUp() override + { + 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_); + + config_->input_ports["pause_service_name"] = "pause"; + config_->input_ports["resume_service_name"] = "resume"; + config_->blackboard = BT::Blackboard::create(); + config_->blackboard->set("node", node_); + + bt_node_ = std::make_shared( + "pause_resume_controller", *config_); + resumed_child_ = std::make_shared(); + paused_child_ = std::make_shared(); + on_pause_child_ = std::make_shared(); + on_resume_child_ = std::make_shared(); + resumed_child_->changeStatus(BT::NodeStatus::SUCCESS); + paused_child_->changeStatus(BT::NodeStatus::SUCCESS); + on_pause_child_->changeStatus(BT::NodeStatus::SUCCESS); + on_resume_child_->changeStatus(BT::NodeStatus::SUCCESS); + bt_node_->addChild(resumed_child_.get()); + bt_node_->addChild(paused_child_.get()); + bt_node_->addChild(on_pause_child_.get()); + bt_node_->addChild(on_resume_child_.get()); + } + +protected: + static std::shared_ptr bt_node_; + static std::shared_ptr paused_child_; + static std::shared_ptr resumed_child_; + static std::shared_ptr on_pause_child_; + static std::shared_ptr on_resume_child_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + static rclcpp::CallbackGroup::SharedPtr cb_group_; + static rclcpp::Client::SharedPtr pause_client_; + static rclcpp::Client::SharedPtr resume_client_; +}; + +std::shared_ptr +PauseResumeControllerTestFixture::bt_node_ = nullptr; +std::shared_ptr +PauseResumeControllerTestFixture::paused_child_ = nullptr; +std::shared_ptr +PauseResumeControllerTestFixture::resumed_child_ = nullptr; +std::shared_ptr +PauseResumeControllerTestFixture::on_pause_child_ = nullptr; +std::shared_ptr +PauseResumeControllerTestFixture::on_resume_child_ = nullptr; +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; + +TEST_F(PauseResumeControllerTestFixture, test_behavior) +{ + resumed_child_->changeStatus(BT::NodeStatus::SUCCESS); + paused_child_->changeStatus(BT::NodeStatus::SUCCESS); + on_pause_child_->changeStatus(BT::NodeStatus::SUCCESS); + on_resume_child_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + + // Set on_pause to RUNNING, call pause service, expect RUNNING + on_pause_child_->changeStatus(BT::NodeStatus::RUNNING); + auto res = pause_client_->async_send_request( + std::make_shared()); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); + executor_->spin_until_future_complete(res, std::chrono::seconds(1)); + ASSERT_EQ(res.wait_for(std::chrono::seconds(1)), std::future_status::ready); + EXPECT_EQ(res.get()->success, true); + + // Change on_pause to SUCCESS, paused child to FAILURE + // Expect SUCCESS (from on_pause), then FAILURE (from paused child) + on_pause_child_->changeStatus(BT::NodeStatus::SUCCESS); + paused_child_->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + + // Set paused to SUCCESS, tick again, expect SUCCESS + paused_child_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + + // Set on_resume to SKIPPED, call resume service, expect SUCCESS (treated same as SKIPPED) + on_resume_child_->changeStatus(BT::NodeStatus::SKIPPED); + res = resume_client_->async_send_request( + std::make_shared()); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + executor_->spin_until_future_complete(res, std::chrono::seconds(1)); + ASSERT_EQ(res.wait_for(std::chrono::seconds(1)), std::future_status::ready); + EXPECT_EQ(res.get()->success, true); + + // Set resumed to RUNNING, expect RUNNING + resumed_child_->changeStatus(BT::NodeStatus::RUNNING); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); + + // Change resumed to SUCCESS, expect SUCCESS + resumed_child_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); +} + +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; +} From 223669cc77c96e57b9d75df70b52cb3b11325ebe Mon Sep 17 00:00:00 2001 From: redvinaa Date: Fri, 4 Jul 2025 13:26:20 +0000 Subject: [PATCH 06/16] Implement tests using xml_txt trees and dummy nodes Signed-off-by: redvinaa --- .../control/pause_resume_controller.hpp | 15 ++ .../control/pause_resume_controller.cpp | 8 +- .../sequence_with_blackboard_memory.cpp | 2 +- .../test/plugins/control/CMakeLists.txt | 3 + .../control/test_pause_resume_controller.cpp | 185 +++++++++++------- .../test_sequence_with_blackboard_memory.cpp | 157 +++++++++++++++ .../test/utils/get_node_from_tree.hpp | 67 +++++++ .../test/utils/test_dummy_tree_node.hpp | 24 ++- 8 files changed, 384 insertions(+), 77 deletions(-) create mode 100644 nav2_behavior_tree/test/plugins/control/test_sequence_with_blackboard_memory.cpp create mode 100644 nav2_behavior_tree/test/utils/get_node_from_tree.hpp 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 index 652e33f95d5..b5ac1230f3a 100644 --- 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 @@ -69,6 +69,16 @@ const std::map child_indices = { * * * + * + * 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. */ @@ -99,6 +109,11 @@ class PauseResumeController : public BT::ControlNode }; } + [[nodiscard]] inline state_t getState() const + { + return state_; + } + private: //! @brief Set state to PAUSE_REQUESTED void pauseServiceCallback( diff --git a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp index aef23651226..82ae572dbe7 100644 --- a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp +++ b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp @@ -92,13 +92,19 @@ BT::NodeStatus PauseResumeController::tick() // 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(); - return BT::NodeStatus::SUCCESS; + // 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()); diff --git a/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp b/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp index d751558b323..beeeddb40df 100644 --- a/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp +++ b/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp @@ -61,7 +61,7 @@ BT::NodeStatus SequenceWithBlackboardMemoryNode::tick() } // end while loop // The entire while loop completed. This means that all the children returned SUCCESS. - if (current_child_idx == children_count) { + if (current_child_idx >= children_count) { resetChildren(); setOutput("current_child_idx", 0); } diff --git a/nav2_behavior_tree/test/plugins/control/CMakeLists.txt b/nav2_behavior_tree/test/plugins/control/CMakeLists.txt index dfa95019d5d..8c8397c89b6 100644 --- a/nav2_behavior_tree/test/plugins/control/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/control/CMakeLists.txt @@ -8,3 +8,6 @@ plugin_add_test(test_control_nonblocking_sequence test_nonblocking_sequence.cpp plugin_add_test(test_control_pause_resume_controller test_pause_resume_controller.cpp nav2_pause_resume_controller_bt_node) + +plugin_add_test(test_sequence_with_blackboard_memory + test_sequence_with_blackboard_memory.cpp nav2_sequence_with_blackboard_memory_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 index a906750900c..ee1f7ecfd1e 100644 --- a/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp @@ -17,13 +17,14 @@ #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: - void SetUp() override + static void SetUpTestCase() { node_ = std::make_shared("pause_resume_controller_test_fixture"); executor_ = @@ -36,49 +37,36 @@ class PauseResumeControllerTestFixture : public nav2_behavior_tree::BehaviorTree resume_client_ = node_->create_client( "resume", rclcpp::ServicesQoS(), cb_group_); - config_->input_ports["pause_service_name"] = "pause"; - config_->input_ports["resume_service_name"] = "resume"; + factory_ = std::make_shared(); + config_ = new BT::NodeConfiguration(); config_->blackboard = BT::Blackboard::create(); config_->blackboard->set("node", node_); - bt_node_ = std::make_shared( - "pause_resume_controller", *config_); - resumed_child_ = std::make_shared(); - paused_child_ = std::make_shared(); - on_pause_child_ = std::make_shared(); - on_resume_child_ = std::make_shared(); - resumed_child_->changeStatus(BT::NodeStatus::SUCCESS); - paused_child_->changeStatus(BT::NodeStatus::SUCCESS); - on_pause_child_->changeStatus(BT::NodeStatus::SUCCESS); - on_resume_child_->changeStatus(BT::NodeStatus::SUCCESS); - bt_node_->addChild(resumed_child_.get()); - bt_node_->addChild(paused_child_.get()); - bt_node_->addChild(on_pause_child_.get()); - bt_node_->addChild(on_resume_child_.get()); + factory_->registerNodeType("PauseResumeController"); + + // Register dummy node for testing + factory_->registerNodeType("DummyNode"); + } + + static void TearDownTestCase() + { + if (config_) { + delete config_; + config_ = nullptr; + } + tree_.reset(); } protected: - static std::shared_ptr bt_node_; - static std::shared_ptr paused_child_; - static std::shared_ptr resumed_child_; - static std::shared_ptr on_pause_child_; - static std::shared_ptr on_resume_child_; + 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_; }; -std::shared_ptr -PauseResumeControllerTestFixture::bt_node_ = nullptr; -std::shared_ptr -PauseResumeControllerTestFixture::paused_child_ = nullptr; -std::shared_ptr -PauseResumeControllerTestFixture::resumed_child_ = nullptr; -std::shared_ptr -PauseResumeControllerTestFixture::on_pause_child_ = nullptr; -std::shared_ptr -PauseResumeControllerTestFixture::on_resume_child_ = nullptr; rclcpp::executors::SingleThreadedExecutor::SharedPtr PauseResumeControllerTestFixture::executor_ = nullptr; rclcpp::CallbackGroup::SharedPtr @@ -87,51 +75,104 @@ 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_behavior) { - resumed_child_->changeStatus(BT::NodeStatus::SUCCESS); - paused_child_->changeStatus(BT::NodeStatus::SUCCESS); - on_pause_child_->changeStatus(BT::NodeStatus::SUCCESS); - on_resume_child_->changeStatus(BT::NodeStatus::SUCCESS); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); - - // Set on_pause to RUNNING, call pause service, expect RUNNING - on_pause_child_->changeStatus(BT::NodeStatus::RUNNING); - auto res = pause_client_->async_send_request( + // 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_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, set ON_PAUSE child to RUNNING, expect RUNNING and ON_PAUSE + auto future = pause_client_->async_send_request( std::make_shared()); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); - executor_->spin_until_future_complete(res, std::chrono::seconds(1)); - ASSERT_EQ(res.wait_for(std::chrono::seconds(1)), std::future_status::ready); - EXPECT_EQ(res.get()->success, true); - - // Change on_pause to SUCCESS, paused child to FAILURE - // Expect SUCCESS (from on_pause), then FAILURE (from paused child) - on_pause_child_->changeStatus(BT::NodeStatus::SUCCESS); - paused_child_->changeStatus(BT::NodeStatus::FAILURE); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); - - // Set paused to SUCCESS, tick again, expect SUCCESS - paused_child_->changeStatus(BT::NodeStatus::SUCCESS); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); - - // Set on_resume to SKIPPED, call resume service, expect SUCCESS (treated same as SKIPPED) - on_resume_child_->changeStatus(BT::NodeStatus::SKIPPED); - res = resume_client_->async_send_request( + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::ON_PAUSE); + check_request_succeeded(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 resume service, change ON_RESUME child to FAILURE, expect FAILURE + future = resume_client_->async_send_request( std::make_shared()); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); - executor_->spin_until_future_complete(res, std::chrono::seconds(1)); - ASSERT_EQ(res.wait_for(std::chrono::seconds(1)), std::future_status::ready); - EXPECT_EQ(res.get()->success, true); - - // Set resumed to RUNNING, expect RUNNING - resumed_child_->changeStatus(BT::NodeStatus::RUNNING); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); - - // Change resumed to SUCCESS, expect SUCCESS - resumed_child_->changeStatus(BT::NodeStatus::SUCCESS); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + on_resume_child->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::FAILURE); + check_request_succeeded(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); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/control/test_sequence_with_blackboard_memory.cpp b/nav2_behavior_tree/test/plugins/control/test_sequence_with_blackboard_memory.cpp new file mode 100644 index 00000000000..67a8c89ad7b --- /dev/null +++ b/nav2_behavior_tree/test/plugins/control/test_sequence_with_blackboard_memory.cpp @@ -0,0 +1,157 @@ +// Copyright (c) 2025 Intel Corporation +// +// 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/sequence_with_blackboard_memory.hpp" + +class SequenceWithBlackboardMemoryTestFixture : 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( + "SequenceWithBlackboardMemory"); + + // 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 * SequenceWithBlackboardMemoryTestFixture::config_ = nullptr; +std::shared_ptr +SequenceWithBlackboardMemoryTestFixture::factory_ = nullptr; +std::shared_ptr SequenceWithBlackboardMemoryTestFixture::tree_ = nullptr; + +TEST_F(SequenceWithBlackboardMemoryTestFixture, 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(SequenceWithBlackboardMemoryTestFixture, 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..e5eb97fcae7 --- /dev/null +++ b/nav2_behavior_tree/test/utils/get_node_from_tree.hpp @@ -0,0 +1,67 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// 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() + 1, 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 From 1042042311384ec4d9c7dfa14c92165e4034cd5b Mon Sep 17 00:00:00 2001 From: redvinaa Date: Tue, 8 Jul 2025 07:54:50 +0000 Subject: [PATCH 07/16] Update include Signed-off-by: redvinaa --- .../plugins/control/pause_resume_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index b5ac1230f3a..517a3083f01 100644 --- 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 @@ -25,7 +25,7 @@ #include "rclcpp/callback_group.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" #include "behaviortree_cpp/control_node.h" -#include "nav2_util/service_server.hpp" +#include "nav2_ros_common/service_server.hpp" // Interface definitions #include "std_srvs/srv/trigger.hpp" From b48ea777a0a12f2ec03a823793973cb89b50e3c5 Mon Sep 17 00:00:00 2001 From: redvinaa Date: Tue, 8 Jul 2025 12:23:30 +0000 Subject: [PATCH 08/16] One more fix because of sync Signed-off-by: redvinaa --- .../plugins/control/pause_resume_controller.hpp | 4 ++-- .../plugins/control/pause_resume_controller.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) 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 index 517a3083f01..4daaaa2aac4 100644 --- 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 @@ -142,8 +142,8 @@ class PauseResumeController : public BT::ControlNode rclcpp::Logger logger_{rclcpp::get_logger("PauseResumeController")}; rclcpp::CallbackGroup::SharedPtr cb_group_; rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; - nav2_util::ServiceServer::SharedPtr pause_srv_; - nav2_util::ServiceServer::SharedPtr resume_srv_; + nav2::ServiceServer::SharedPtr pause_srv_; + nav2::ServiceServer::SharedPtr resume_srv_; state_t state_; }; diff --git a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp index 82ae572dbe7..c1c130ee397 100644 --- a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp +++ b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp @@ -45,19 +45,19 @@ PauseResumeController::PauseResumeController( std::string pause_service_name; getInput("pause_service_name", pause_service_name); - pause_srv_ = std::make_shared>( + pause_srv_ = std::make_shared>( pause_service_name, node, std::bind(&PauseResumeController::pauseServiceCallback, this, _1, _2, _3), - rclcpp::ServicesQoS(), cb_group_); + cb_group_); std::string resume_service_name; getInput("resume_service_name", resume_service_name); - resume_srv_ = std::make_shared>( + resume_srv_ = std::make_shared>( resume_service_name, node, std::bind(&PauseResumeController::resumeServiceCallback, this, _1, _2, _3), - rclcpp::ServicesQoS(), cb_group_); + cb_group_); } BT::NodeStatus PauseResumeController::tick() From c1632c7173c7d7257f1825ec80115610560bd2c6 Mon Sep 17 00:00:00 2001 From: redvinaa Date: Wed, 9 Jul 2025 10:14:55 +0200 Subject: [PATCH 09/16] Fix build Signed-off-by: redvinaa --- .../plugins/control/test_pause_resume_controller.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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 index ee1f7ecfd1e..56f879aa739 100644 --- a/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp @@ -26,21 +26,21 @@ class PauseResumeControllerTestFixture : public nav2_behavior_tree::BehaviorTree public: static void SetUpTestCase() { - node_ = std::make_shared("pause_resume_controller_test_fixture"); + auto node = std::make_shared("pause_resume_controller_test_fixture"); executor_ = std::make_shared(); - cb_group_ = node_->create_callback_group( + 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( + 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_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_); + config_->blackboard->set("node", node); factory_->registerNodeType("PauseResumeController"); From f844ee3fdeb4453f6da1b9b3eff08d0947350e5e Mon Sep 17 00:00:00 2001 From: redvinaa Date: Wed, 9 Jul 2025 11:27:39 +0200 Subject: [PATCH 10/16] Add tests Signed-off-by: redvinaa --- .../control/test_pause_resume_controller.cpp | 94 +++++++++++++++++++ 1 file changed, 94 insertions(+) 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 index 56f879aa739..71883c4b325 100644 --- a/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp @@ -79,6 +79,86 @@ 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), + std::runtime_error); +} + +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 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_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::ON_RESUME); + 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 @@ -157,6 +237,13 @@ TEST_F(PauseResumeControllerTestFixture, test_behavior) 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_request_succeeded(future); + // Call resume service, change ON_RESUME child to FAILURE, expect FAILURE future = resume_client_->async_send_request( std::make_shared()); @@ -173,6 +260,13 @@ TEST_F(PauseResumeControllerTestFixture, test_behavior) 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::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + check_request_succeeded(future); } int main(int argc, char ** argv) From 59c2494a4be1fbb3ef13c55afd94b7d6c0fadd43 Mon Sep 17 00:00:00 2001 From: redvinaa Date: Wed, 9 Jul 2025 11:33:17 +0200 Subject: [PATCH 11/16] Remove unreachable code Signed-off-by: redvinaa --- .../plugins/control/pause_resume_controller.cpp | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp index c1c130ee397..627b7a55445 100644 --- a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp +++ b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp @@ -137,14 +137,6 @@ void PauseResumeController::pauseServiceCallback( const std::shared_ptr/*request*/, std::shared_ptr response) { - if (status() == BT::NodeStatus::IDLE) { - std::string warn_msg = "PauseResumeController BT node has not been ticked yet"; - response->success = false; - response->message = warn_msg; - RCLCPP_ERROR(logger_, "%s", warn_msg.c_str()); - return; - } - if (state_ != PAUSED) { RCLCPP_INFO(logger_, "Received pause request"); response->success = true; @@ -163,14 +155,6 @@ void PauseResumeController::resumeServiceCallback( const std::shared_ptr/*request*/, std::shared_ptr response) { - if (status() == BT::NodeStatus::IDLE) { - std::string warn_msg = "PauseResumeController BT node has not been ticked yet"; - response->success = false; - response->message = warn_msg; - RCLCPP_ERROR(logger_, "%s", warn_msg.c_str()); - return; - } - if (state_ == PAUSED) { RCLCPP_INFO(logger_, "Received resume request"); response->success = true; From 03a79e08e28be1c4f3d022b912460ec170ddd998 Mon Sep 17 00:00:00 2001 From: redvinaa Date: Wed, 9 Jul 2025 13:55:09 +0200 Subject: [PATCH 12/16] Fix tests Signed-off-by: redvinaa --- .../control/test_pause_resume_controller.cpp | 25 ++++++++++--------- 1 file changed, 13 insertions(+), 12 deletions(-) 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 index 71883c4b325..8ba1c1beccb 100644 --- a/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp @@ -94,7 +94,7 @@ TEST_F(PauseResumeControllerTestFixture, test_incorrect_num_children) )"; EXPECT_THROW( auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard), - std::runtime_error); + BT::RuntimeError); } TEST_F(PauseResumeControllerTestFixture, test_unused_children) @@ -136,11 +136,11 @@ TEST_F(PauseResumeControllerTestFixture, test_unused_children) EXPECT_EQ(future.get()->success, true); }; - // Call pause service, expect RUNNING and ON_PAUSE + // 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::ON_PAUSE); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); check_request_succeeded(future); // Tick again, expect RUNNING and PAUSED @@ -151,7 +151,7 @@ TEST_F(PauseResumeControllerTestFixture, test_unused_children) 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::ON_RESUME); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); check_request_succeeded(future); // Tick again, expect RUNNING and RESUMED @@ -206,12 +206,13 @@ TEST_F(PauseResumeControllerTestFixture, test_behavior) 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) + 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, true); + EXPECT_EQ(future.get()->success, success); }; // Call pause service, set ON_PAUSE child to RUNNING, expect RUNNING and ON_PAUSE @@ -219,7 +220,7 @@ TEST_F(PauseResumeControllerTestFixture, test_behavior) std::make_shared()); EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(pause_bt_node->getState(), state_t::ON_PAUSE); - check_request_succeeded(future); + check_future_result(future); // Change ON_PAUSE child to SUCCESS, expect RUNNING and PAUSED on_pause_child->changeStatus(BT::NodeStatus::SUCCESS); @@ -242,14 +243,14 @@ TEST_F(PauseResumeControllerTestFixture, test_behavior) std::make_shared()); EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); - check_request_succeeded(future); + 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_request_succeeded(future); + check_future_result(future); // Halt the tree, expect RUNNING and RESUMED tree_->haltTree(); @@ -264,9 +265,9 @@ TEST_F(PauseResumeControllerTestFixture, test_behavior) // Call resume service again, expect RUNNING and RESUMED future = resume_client_->async_send_request( std::make_shared()); - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); - check_request_succeeded(future); + check_future_result(future, false); } int main(int argc, char ** argv) From 539407a796a70626129309123a1ebf4370a2ff72 Mon Sep 17 00:00:00 2001 From: redvinaa Date: Tue, 15 Jul 2025 15:41:27 +0200 Subject: [PATCH 13/16] Update copyrights Signed-off-by: redvinaa --- .../plugins/control/pause_resume_controller.hpp | 2 +- .../plugins/control/sequence_with_blackboard_memory.hpp | 2 +- nav2_behavior_tree/plugins/control/pause_resume_controller.cpp | 2 +- .../plugins/control/sequence_with_blackboard_memory.cpp | 2 +- .../test/plugins/control/test_pause_resume_controller.cpp | 2 +- .../plugins/control/test_sequence_with_blackboard_memory.cpp | 2 +- nav2_behavior_tree/test/utils/get_node_from_tree.hpp | 3 +-- 7 files changed, 7 insertions(+), 8 deletions(-) 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 index 4daaaa2aac4..3746b01d342 100644 --- 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 @@ -1,4 +1,4 @@ -// Copyright (c) 2025 Intel Corporation +// 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. diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp index 7e995076473..f6bfc497f2e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2025 Intel Corporation +// 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. diff --git a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp index 627b7a55445..0f039aad8e6 100644 --- a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp +++ b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2025 Intel Corporation +// 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. diff --git a/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp b/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp index beeeddb40df..d3a0984f54e 100644 --- a/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp +++ b/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2025 Intel Corporation +// 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. 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 index 8ba1c1beccb..8a78c37d89b 100644 --- a/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2025 Intel Corporation +// 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. diff --git a/nav2_behavior_tree/test/plugins/control/test_sequence_with_blackboard_memory.cpp b/nav2_behavior_tree/test/plugins/control/test_sequence_with_blackboard_memory.cpp index 67a8c89ad7b..16e469e2917 100644 --- a/nav2_behavior_tree/test/plugins/control/test_sequence_with_blackboard_memory.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_sequence_with_blackboard_memory.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2025 Intel Corporation +// 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. diff --git a/nav2_behavior_tree/test/utils/get_node_from_tree.hpp b/nav2_behavior_tree/test/utils/get_node_from_tree.hpp index e5eb97fcae7..b6574511c06 100644 --- a/nav2_behavior_tree/test/utils/get_node_from_tree.hpp +++ b/nav2_behavior_tree/test/utils/get_node_from_tree.hpp @@ -1,5 +1,4 @@ -// Copyright (c) 2018 Intel Corporation -// Copyright (c) 2020 Sarthak Mittal +// 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. From 580f182867012b0b1469ba042088bb2de4463fde Mon Sep 17 00:00:00 2001 From: redvinaa Date: Tue, 15 Jul 2025 15:42:41 +0200 Subject: [PATCH 14/16] Fix size calc Signed-off-by: redvinaa --- nav2_behavior_tree/test/utils/get_node_from_tree.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/test/utils/get_node_from_tree.hpp b/nav2_behavior_tree/test/utils/get_node_from_tree.hpp index b6574511c06..068fc924005 100644 --- a/nav2_behavior_tree/test/utils/get_node_from_tree.hpp +++ b/nav2_behavior_tree/test/utils/get_node_from_tree.hpp @@ -41,7 +41,7 @@ NodeT * get_node_from_tree( return nullptr; } if (nodes.size() <= index) { - RCLCPP_ERROR(logger, "Out of bounds (found %zu < %zu nodes)", nodes.size() + 1, index); + RCLCPP_ERROR(logger, "Out of bounds (found %zu < %zu nodes)", nodes.size(), index); return nullptr; } From 84ce99958fe2d6734e050b29589bc62b96aa2309 Mon Sep 17 00:00:00 2001 From: redvinaa Date: Tue, 29 Jul 2025 14:54:31 +0200 Subject: [PATCH 15/16] Rename to PersistentSequence Signed-off-by: redvinaa --- nav2_behavior_tree/CMakeLists.txt | 4 +-- ...ard_memory.hpp => persistent_sequence.hpp} | 14 +++++----- nav2_behavior_tree/nav2_tree_nodes.xml | 2 +- ...ard_memory.cpp => persistent_sequence.cpp} | 12 ++++----- .../test/plugins/control/CMakeLists.txt | 3 +-- ...emory.cpp => test_persistent_sequence.cpp} | 26 +++++++++---------- 6 files changed, 30 insertions(+), 31 deletions(-) rename nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/{sequence_with_blackboard_memory.hpp => persistent_sequence.hpp} (74%) rename nav2_behavior_tree/plugins/control/{sequence_with_blackboard_memory.cpp => persistent_sequence.cpp} (84%) rename nav2_behavior_tree/test/plugins/control/{test_sequence_with_blackboard_memory.cpp => test_persistent_sequence.cpp} (83%) diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 93359bf8738..4059e6da15e 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -221,8 +221,8 @@ 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_sequence_with_blackboard_memory_bt_node SHARED plugins/control/sequence_with_blackboard_memory.cpp) -list(APPEND plugin_libs nav2_sequence_with_blackboard_memory_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/sequence_with_blackboard_memory.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/persistent_sequence.hpp similarity index 74% rename from nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp rename to nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/persistent_sequence.hpp index f6bfc497f2e..79d494eb8e6 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/persistent_sequence.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__SEQUENCE_WITH_BLACKBOARD_MEMORY_HPP_ -#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__SEQUENCE_WITH_BLACKBOARD_MEMORY_HPP_ +#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" @@ -22,7 +22,7 @@ namespace nav2_behavior_tree { /** - * @brief The SequenceWithBlackboardMemoryNode is similar to the SequenceNode, but it + * @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 @@ -37,12 +37,12 @@ namespace nav2_behavior_tree * Restart the loop only if (reset_on_failure == true) * */ -class SequenceWithBlackboardMemoryNode : public BT::ControlNode +class PersistentSequenceNode : public BT::ControlNode { public: - SequenceWithBlackboardMemoryNode(const std::string & name, const BT::NodeConfiguration & conf); + PersistentSequenceNode(const std::string & name, const BT::NodeConfiguration & conf); - ~SequenceWithBlackboardMemoryNode() override = default; + ~PersistentSequenceNode() override = default; //! @brief Declare ports static BT::PortsList providedPorts() @@ -58,4 +58,4 @@ class SequenceWithBlackboardMemoryNode : public BT::ControlNode } // namespace nav2_behavior_tree -#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__SEQUENCE_WITH_BLACKBOARD_MEMORY_HPP_ +#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 8456c2133da..230ce02f70c 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -410,7 +410,7 @@ Service to call to resume - + Index of the child to execute diff --git a/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp b/nav2_behavior_tree/plugins/control/persistent_sequence.cpp similarity index 84% rename from nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp rename to nav2_behavior_tree/plugins/control/persistent_sequence.cpp index d3a0984f54e..1886deb3d5c 100644 --- a/nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp +++ b/nav2_behavior_tree/plugins/control/persistent_sequence.cpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.hpp" +#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 { -SequenceWithBlackboardMemoryNode::SequenceWithBlackboardMemoryNode( +PersistentSequenceNode::PersistentSequenceNode( const std::string & name, const BT::NodeConfiguration & conf) : BT::ControlNode::ControlNode(name, conf) {} -BT::NodeStatus SequenceWithBlackboardMemoryNode::tick() +BT::NodeStatus PersistentSequenceNode::tick() { const int children_count = children_nodes_.size(); @@ -75,10 +75,10 @@ BT_REGISTER_NODES(factory) BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { - return std::make_unique( + return std::make_unique( name, config); }; - factory.registerBuilder( - "SequenceWithBlackboardMemory", builder); + 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 8c8397c89b6..a625bdad792 100644 --- a/nav2_behavior_tree/test/plugins/control/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/control/CMakeLists.txt @@ -9,5 +9,4 @@ plugin_add_test(test_control_nonblocking_sequence test_nonblocking_sequence.cpp plugin_add_test(test_control_pause_resume_controller test_pause_resume_controller.cpp nav2_pause_resume_controller_bt_node) -plugin_add_test(test_sequence_with_blackboard_memory - test_sequence_with_blackboard_memory.cpp nav2_sequence_with_blackboard_memory_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_sequence_with_blackboard_memory.cpp b/nav2_behavior_tree/test/plugins/control/test_persistent_sequence.cpp similarity index 83% rename from nav2_behavior_tree/test/plugins/control/test_sequence_with_blackboard_memory.cpp rename to nav2_behavior_tree/test/plugins/control/test_persistent_sequence.cpp index 16e469e2917..ebf6aa0d6b8 100644 --- a/nav2_behavior_tree/test/plugins/control/test_sequence_with_blackboard_memory.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_persistent_sequence.cpp @@ -18,9 +18,9 @@ #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/sequence_with_blackboard_memory.hpp" +#include "nav2_behavior_tree/plugins/control/persistent_sequence.hpp" -class SequenceWithBlackboardMemoryTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +class PersistentSequenceTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture { public: static void SetUpTestCase() @@ -30,8 +30,8 @@ class SequenceWithBlackboardMemoryTestFixture : public nav2_behavior_tree::Behav config_->blackboard = BT::Blackboard::create(); config_->blackboard->set("seq_child_idx", 0); - factory_->registerNodeType( - "SequenceWithBlackboardMemory"); + factory_->registerNodeType( + "PersistentSequence"); // Register dummy node for testing factory_->registerNodeType("DummyNode"); @@ -57,21 +57,21 @@ class SequenceWithBlackboardMemoryTestFixture : public nav2_behavior_tree::Behav static std::shared_ptr tree_; }; -BT::NodeConfiguration * SequenceWithBlackboardMemoryTestFixture::config_ = nullptr; +BT::NodeConfiguration * PersistentSequenceTestFixture::config_ = nullptr; std::shared_ptr -SequenceWithBlackboardMemoryTestFixture::factory_ = nullptr; -std::shared_ptr SequenceWithBlackboardMemoryTestFixture::tree_ = nullptr; +PersistentSequenceTestFixture::factory_ = nullptr; +std::shared_ptr PersistentSequenceTestFixture::tree_ = nullptr; -TEST_F(SequenceWithBlackboardMemoryTestFixture, test_tick) +TEST_F(PersistentSequenceTestFixture, test_tick) { // create tree std::string xml_txt = R"( - + - + )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); @@ -80,18 +80,18 @@ TEST_F(SequenceWithBlackboardMemoryTestFixture, test_tick) EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); } -TEST_F(SequenceWithBlackboardMemoryTestFixture, test_behavior) +TEST_F(PersistentSequenceTestFixture, test_behavior) { // create tree std::string xml_txt = R"( - + - + )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); From 0db8b63eda1dbded6959da37517ea012c3af86d3 Mon Sep 17 00:00:00 2001 From: redvinaa Date: Thu, 31 Jul 2025 14:15:45 +0200 Subject: [PATCH 16/16] Fix docstring Signed-off-by: redvinaa --- .../plugins/control/pause_resume_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 3746b01d342..a3f5333b19b 100644 --- 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 @@ -68,7 +68,7 @@ const std::map child_indices = { * * * - * + * * * 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,