Nonblocking sequence bt control node

nav2_behavior_tree/CMakeLists.txt

@@ -212,6 +212,9 @@ list(APPEND plugin_libs nav2_get_current_pose_action_bt_node)
 add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp)
 list(APPEND plugin_libs nav2_pipeline_sequence_bt_node)
 
+add_library(nav2_nonblocking_sequence_bt_node SHARED plugins/control/nonblocking_sequence.cpp)
+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)
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/nonblocking_sequence.hpp

@@ -0,0 +1,91 @@
+// Copyright (c) 2025 Polymath 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__NONBLOCKING_SEQUENCE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__NONBLOCKING_SEQUENCE_HPP_
+
+#include <string>
+#include "behaviortree_cpp/control_node.h"
+#include "behaviortree_cpp/bt_factory.h"
+
+namespace nav2_behavior_tree
+{
+
+/** @brief Type of sequence node that keeps tickinng through all the children until all children
+ * return SUCCESS
+ *
+ * Type of Control Node  | Child Returns Failure | Child Returns Running
+ * ---------------------------------------------------------------------
+ * NonblockingSequence   |      Restart          | Continue tickng next child
+ *
+ * Continue ticking next child means every node after the running node will be ticked. Even
+ * if a previous node returns Running or Success, the subsequent nodes will be reticked.
+ *
+ * As an example, let's say this node has 3 children: A, B and C. At the start,
+ * they are all IDLE.
+ * |    A    |    B    |    C    |
+ * --------------------------------
+ * |  IDLE   |  IDLE   |   IDLE  |
+ * | RUNNING |  IDLE   |   IDLE  |  - at first A gets ticked. Assume it returns RUNNING
+ *                                  - NonblockingSequence returns RUNNING and continues to the next
+ *                                  - node.
+ * | RUNNING | RUNNING | RUNNING |  - Eventually all nodes will be in the running state and
+ *                                  - NonblockingSequence returns RUNNING
+ * | SUCCESS | RUNNING | SUCCESS |  - Even in a configuration where there are multiple nodes
+ *                                  - returning SUCCESS, NonblockingSequence continues on ticking all.
+ *                                  - nodes each time it is ticked and returns RUNNING. Note that even
+ *                                  - if a node returns `SUCCESS`, on the next tick, it will attempt to
+ *                                  - restart the node. This is too ensure that successful nodes do
+ *                                  - not latch a stale state while waiting for another long running
+ *                                  - node to be complete
+ * | SUCCESS | SUCCESS | SUCCESS |  - If all child nodes return SUCCESS the NonblockingSequence
+ *                                  - returns SUCCESS
+ *
+ * If any children at any time had returned FAILURE. NonblockingSequence would have returned FAILURE
+ * and halted all children, ending the sequence.
+ *
+ * Usage in XML: <NonblockingSequence>
+ */
+class NonblockingSequence : public BT::ControlNode
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::NonblockingSequence
+   * @param name Name for the XML tag for this node
+   */
+  explicit NonblockingSequence(const std::string & name);
+
+  /**
+   * @brief A constructor for nav2_behavior_tree::NonblockingSequence
+   * @param name Name for the XML tag for this node
+   * @param config BT node configuration
+   */
+  NonblockingSequence(const std::string & name, const BT::NodeConfiguration & config);
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing basic ports along with node-specific ports
+   */
+  static BT::PortsList providedPorts() {return {};}
+
+protected:
+  /**
+   * @brief The main override required by a BT action
+   * @return BT::NodeStatus Status of tick execution
+   */
+  BT::NodeStatus tick() override;
+};
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__NONBLOCKING_SEQUENCE_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -397,6 +397,7 @@
 
     <Control ID="RoundRobin"/>
 
+    <Control ID="NonblockingSequence"/>
     <!-- ############################### DECORATOR NODES ############################## -->
     <Decorator ID="RateController">
       <input_port name="hz">Rate</input_port>

nav2_behavior_tree/plugins/control/nonblocking_sequence.cpp

@@ -0,0 +1,74 @@
+// Copyright (c) 2025 Polymath Robotics
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <stdexcept>
+#include <sstream>
+#include <string>
+
+#include "nav2_behavior_tree/plugins/control/nonblocking_sequence.hpp"
+
+namespace nav2_behavior_tree
+{
+
+NonblockingSequence::NonblockingSequence(const std::string & name)
+: BT::ControlNode(name, {})
+{
+}
+
+NonblockingSequence::NonblockingSequence(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::ControlNode(name, conf)
+{
+}
+
+BT::NodeStatus NonblockingSequence::tick()
+{
+  bool all_success = true;
+
+  for (std::size_t i = 0; i < children_nodes_.size(); ++i) {
+    auto status = children_nodes_[i]->executeTick();
+    switch (status) {
+      case BT::NodeStatus::FAILURE:
+        ControlNode::haltChildren();
+        all_success = false;  // probably not needed
+        return status;
+      case BT::NodeStatus::SUCCESS:
+        break;
+      case BT::NodeStatus::RUNNING:
+        all_success = false;
+        break;
+      default:
+        std::stringstream error_msg;
+        error_msg << "Invalid node status. Received status " << status <<
+          "from child " << children_nodes_[i]->name();
+        throw std::runtime_error(error_msg.str());
+    }
+  }
+
+  // Wrap up.
+  if (all_success) {
+    ControlNode::haltChildren();
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  return BT::NodeStatus::RUNNING;
+}
+
+}  // namespace nav2_behavior_tree
+
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::NonblockingSequence>("NonblockingSequence");
+}

nav2_behavior_tree/test/plugins/control/CMakeLists.txt

@@ -3,3 +3,5 @@ plugin_add_test(test_control_recovery_node test_recovery_node.cpp nav2_recovery_
 plugin_add_test(test_control_pipeline_sequence test_pipeline_sequence.cpp nav2_pipeline_sequence_bt_node)
 
 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)

nav2_behavior_tree/test/plugins/control/test_nonblocking_sequence.cpp

@@ -0,0 +1,147 @@
+// Copyright (c) 2025 Polymath Robotics
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+
+#include "utils/test_behavior_tree_fixture.hpp"
+#include "utils/test_dummy_tree_node.hpp"
+#include "nav2_behavior_tree/plugins/control/nonblocking_sequence.hpp"
+
+class NonblockingSequenceTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp() override
+  {
+    bt_node_ = std::make_shared<nav2_behavior_tree::NonblockingSequence>(
+      "nonblocking_sequence", *config_);
+    first_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    second_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    third_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    bt_node_->addChild(first_child_.get());
+    bt_node_->addChild(second_child_.get());
+    bt_node_->addChild(third_child_.get());
+  }
+
+  void TearDown() override
+  {
+    first_child_.reset();
+    second_child_.reset();
+    third_child_.reset();
+    bt_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::NonblockingSequence> bt_node_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> first_child_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> second_child_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> third_child_;
+};
+
+std::shared_ptr<nav2_behavior_tree::NonblockingSequence>
+NonblockingSequenceTestFixture::bt_node_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode>
+NonblockingSequenceTestFixture::first_child_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode>
+NonblockingSequenceTestFixture::second_child_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode>
+NonblockingSequenceTestFixture::third_child_ = nullptr;
+
+TEST_F(NonblockingSequenceTestFixture, test_failure_on_idle_child)
+{
+  first_child_->changeStatus(BT::NodeStatus::IDLE);
+  EXPECT_THROW(bt_node_->executeTick(), std::runtime_error);
+}
+
+TEST_F(NonblockingSequenceTestFixture, test_failure)
+{
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
+
+  first_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  second_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
+
+  first_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  third_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
+
+  first_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  third_child_->changeStatus(BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
+}
+
+TEST_F(NonblockingSequenceTestFixture, test_behavior)
+{
+  first_child_->changeStatus(BT::NodeStatus::RUNNING);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  third_child_->changeStatus(BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+
+  first_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  third_child_->changeStatus(BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+
+  first_child_->changeStatus(BT::NodeStatus::RUNNING);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  third_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+
+  first_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  third_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+
+  // Even if first two children are running, we will still tick the third
+  // node, which if set to failure, fails everything
+  first_child_->changeStatus(BT::NodeStatus::RUNNING);
+  second_child_->changeStatus(BT::NodeStatus::RUNNING);
+  third_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
+}
+
+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;
+}