Add pause and seq with bb memory BT nodes

Author: redvinaa

nav2_behavior_tree/CMakeLists.txt

@@ -212,6 +212,12 @@ list(APPEND plugin_libs nav2_pipeline_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)
 

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 <string>
+#include <memory>
+#include <mutex>
+
+// 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<std::string>(
+        "pause_service_name",
+        "Name of the service to pause"),
+      BT::InputPort<std::string>(
+        "resume_service_name",
+        "Name of the service to resume"),
+    };
+  }
+
+private:
+  //! @brief Service callback to pause
+  void pause_service_callback(
+    const std::shared_ptr<Trigger::Request> request,
+    std::shared_ptr<Trigger::Response> response);
+
+  //! @brief Service callback to resume
+  void resume_service_callback(
+    const std::shared_ptr<Trigger::Request> request,
+    std::shared_ptr<Trigger::Response> response);
+
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::CallbackGroup::SharedPtr cb_group_;
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+  std::unique_ptr<std::thread> spinner_thread_;
+  rclcpp::Service<Trigger>::SharedPtr pause_srv_;
+  rclcpp::Service<Trigger>::SharedPtr resume_srv_;
+  state_t state_;
+  std::mutex state_mutex_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_HPP_

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 <string>
+#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<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__SEQUENCE_WITH_BLACKBOARD_MEMORY_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -390,6 +390,15 @@
 
     <Control ID="RoundRobin"/>
 
+    <Control ID="Pause">
+      <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="SequenceWithBlackboardMemory">
+      <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>