Add Pause and SequenceWithBlackboardMemory BT nodes

nav2_behavior_tree/CMakeLists.txt

@@ -218,6 +218,12 @@ list(APPEND plugin_libs nav2_nonblocking_sequence_bt_node)
 add_library(nav2_round_robin_node_bt_node SHARED plugins/control/round_robin_node.cpp)
 list(APPEND plugin_libs nav2_round_robin_node_bt_node)
 
+add_library(nav2_pause_resume_controller_bt_node SHARED plugins/control/pause_resume_controller.cpp)
+list(APPEND plugin_libs nav2_pause_resume_controller_bt_node)
+
+add_library(nav2_persistent_sequence_bt_node SHARED plugins/control/persistent_sequence.cpp)
+list(APPEND plugin_libs nav2_persistent_sequence_bt_node)
+
 add_library(nav2_single_trigger_bt_node SHARED plugins/decorator/single_trigger_node.cpp)
 list(APPEND plugin_libs nav2_single_trigger_bt_node)
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp

@@ -0,0 +1,152 @@
+// Copyright (c) 2025 Enjoy Robotics
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_
+
+// Other includes
+#include <string>
+#include <memory>
+#include <map>
+
+// ROS includes
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
+#include "behaviortree_cpp/control_node.h"
+#include "nav2_ros_common/service_server.hpp"
+
+// Interface definitions
+#include "std_srvs/srv/trigger.hpp"
+
+
+namespace nav2_behavior_tree
+{
+
+using Trigger = std_srvs::srv::Trigger;
+
+enum state_t {RESUMED, PAUSED, PAUSE_REQUESTED, ON_PAUSE, RESUME_REQUESTED, ON_RESUME};
+const std::map<state_t, std::string> state_names = {
+  {RESUMED, "RESUMED"},
+  {PAUSED, "PAUSED"},
+  {PAUSE_REQUESTED, "PAUSE_REQUESTED"},
+  {ON_PAUSE, "ON_PAUSE"},
+  {RESUME_REQUESTED, "RESUME_REQUESTED"},
+  {ON_RESUME, "ON_RESUME"}
+};
+const std::map<state_t, uint16_t> child_indices = {
+  {RESUMED, 0},
+  {PAUSED, 1},
+  {ON_PAUSE, 2},
+  {ON_RESUME, 3}
+};
+
+/* @brief Controlled through service calls to pause and resume the execution of the tree
+ * It has one mandatory child for the RESUMED, and three optional for the PAUSED state,
+ * the ON_PAUSE event and the ON_RESUME event.
+ * It has two input ports:
+ * - pause_service_name: name of the service to pause
+ * - resume_service_name: name of the service to resume
+ *
+ * Usage:
+ * <PauseResumeController pause_service_name="/pause" resume_service_name="/resume">
+ *     <!-- RESUMED branch -->
+ *
+ *     <!-- PAUSED branch (optional) -->
+ *
+ *     <!-- ON_PAUSE branch (optional) -->
+ *
+ *     <!-- ON_RESUME branch (optional) -->
+ * </PauseResumeController>
+ *
+ * The controller starts in RESUMED state, and ticks it until it returns success.
+ * When the pause service is called, ON_PAUSE is ticked until completion,
+ * then the controller switches to PAUSED state.
+ * When the resume service is called, ON_RESUME is ticked until completion,
+ * then the controller switches back to RESUMED state.
+ *
+ * The controller only returns success when the RESUMED child returns success.
+ * The controller returns failure if any child returns failure.
+ * In any other case, it returns running.
+ */
+
+
+class PauseResumeController : public BT::ControlNode
+{
+public:
+  //! @brief Constructor
+  PauseResumeController(
+    const std::string & xml_tag_name,
+    const BT::NodeConfiguration & conf);
+
+  //! @brief Reset state and go to Idle
+  void halt() override;
+
+  //! @brief Handle transitions if requested and tick child related to the actual state
+  BT::NodeStatus tick() override;
+
+  //! @brief Declare ports
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<std::string>(
+        "pause_service_name",
+        "Name of the service to pause"),
+      BT::InputPort<std::string>(
+        "resume_service_name",
+        "Name of the service to resume"),
+    };
+  }
+
+  [[nodiscard]] inline state_t getState() const
+  {
+    return state_;
+  }
+
+private:
+  //! @brief Set state to PAUSE_REQUESTED
+  void pauseServiceCallback(
+    const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+    const std::shared_ptr<Trigger::Request> request,
+    std::shared_ptr<Trigger::Response> response);
+
+  //! @brief Set state to RESUME_REQUESTED
+  void resumeServiceCallback(
+    const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+    const std::shared_ptr<Trigger::Request> request,
+    std::shared_ptr<Trigger::Response> response);
+
+  /** @brief Switch to the next state based on the current state
+   *
+   * PAUSE_REQUESTED -> ON_PAUSE
+   * ON_PAUSE -> PAUSED
+   *
+   * RESUME_REQUESTED -> ON_RESUME
+   * ON_RESUME -> RESUMED
+   *
+   * Do nothing if in end state
+   */
+  void switchToNextState();
+
+  rclcpp::Logger logger_{rclcpp::get_logger("PauseResumeController")};
+  rclcpp::CallbackGroup::SharedPtr cb_group_;
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+  nav2::ServiceServer<Trigger>::SharedPtr pause_srv_;
+  nav2::ServiceServer<Trigger>::SharedPtr resume_srv_;
+  state_t state_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/persistent_sequence.hpp

@@ -0,0 +1,61 @@
+// Copyright (c) 2025 Enjoy Robotics
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PERSISTENT_SEQUENCE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PERSISTENT_SEQUENCE_HPP_
+
+#include <string>
+#include "behaviortree_cpp/control_node.h"
+#include "behaviortree_cpp/bt_factory.h"
+
+namespace nav2_behavior_tree
+{
+/**
+ * @brief The PersistentSequenceNode is similar to the SequenceNode, but it
+ * stores the index of the last running child in the blackboard (key: "current_child_idx"),
+ * and it does not reset the index when it got halted.
+ * It used to tick children in an ordered sequence. If any child returns RUNNING, previous
+ * children will NOT be ticked again.
+ *
+ * - If all the children return SUCCESS, this node returns SUCCESS.
+ *
+ * - If a child returns RUNNING, this node returns RUNNING.
+ *   Loop is NOT restarted, the same running child will be ticked again.
+ *
+ * - If a child returns FAILURE, stop the loop and return FAILURE.
+ *   Restart the loop only if (reset_on_failure == true)
+ *
+ */
+class PersistentSequenceNode : public BT::ControlNode
+{
+public:
+  PersistentSequenceNode(const std::string & name, const BT::NodeConfiguration & conf);
+
+  ~PersistentSequenceNode() override = default;
+
+  //! @brief Declare ports
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::BidirectionalPort<int>("current_child_idx", "The index of the current child"),
+    };
+  }
+
+private:
+  BT::NodeStatus tick() override;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PERSISTENT_SEQUENCE_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -404,6 +404,16 @@
     <Control ID="RoundRobin"/>
 
     <Control ID="NonblockingSequence"/>
+
+    <Control ID="PauseResumeController">
+      <input_port name="pause_service_name">Service to call to pause</input_port>
+      <input_port name="resume_service_name">Service to call to resume</input_port>
+    </Control>
+
+    <Control ID="PersistentSequence">
+      <bidirectional_port name="current_child_idx">Index of the child to execute</bidirectional_port>
+    </Control>
+
     <!-- ############################### DECORATOR NODES ############################## -->
     <Decorator ID="RateController">
       <input_port name="hz">Rate</input_port>