Add wrap_around parameter to RoundRobin node to control wrap-around behavior

nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp

@@ -82,11 +82,18 @@ class RoundRobinNode : public BT::ControlNode
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
    */
-  static BT::PortsList providedPorts() {return {};}
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<bool>("wrap_around", false,
+          "Enable wrap-around to first child after last child fails")
+    };
+  }
 
 private:
   unsigned int current_child_idx_{0};
   unsigned int num_failed_children_{0};
+  bool wrap_around_{false};
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -388,7 +388,9 @@
       <input_port name="number_of_retries">Number of retries</input_port>
     </Control>
 
-    <Control ID="RoundRobin"/>
+    <Control ID="RoundRobin">
+      <input_port name="wrap_around" default="false">Enable wrap-around to first child after last child fails</input_port>
+    </Control>
 
     <!-- ############################### DECORATOR NODES ############################## -->
     <Decorator ID="RateController">

nav2_behavior_tree/plugins/control/round_robin_node.cpp

@@ -20,7 +20,7 @@
 {
 
 RoundRobinNode::RoundRobinNode(const std::string & name)
-: BT::ControlNode::ControlNode(name, {})
+: BT::ControlNode::ControlNode(name, {}), wrap_around_(false)
 {
 }
 
@@ -29,6 +29,7 @@
   const BT::NodeConfiguration & config)
 : BT::ControlNode(name, config)
 {
+  getInput("wrap_around", wrap_around_);
 }
 
 BT::NodeStatus RoundRobinNode::tick()
@@ -43,9 +44,19 @@
     const BT::NodeStatus child_status = child_node->executeTick();
 
     if (child_status != BT::NodeStatus::RUNNING) {
-      // Increment index and wrap around to the first child
+      // Increment index and wrap around to the first child if enabled
       if (++current_child_idx_ == num_children) {
-        current_child_idx_ = 0;
+        if (wrap_around_) {
+          current_child_idx_ = 0;
+        } else {
+          if (child_status == BT::NodeStatus::SKIPPED) {
+            num_skipped_children++;
+          } else if (child_status == BT::NodeStatus::FAILURE) {
+            num_failed_children_++;
+          }
+          // Exit early if wrap around is disabled and we've reached the end
+          break;
+        }
       }
     }
 

nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp

@@ -25,6 +25,7 @@ class RoundRobinNodeTestFixture : public nav2_behavior_tree::BehaviorTreeTestFix
 public:
   void SetUp() override
   {
+    config_->input_ports["wrap_around"] = "true";
     bt_node_ = std::make_shared<nav2_behavior_tree::RoundRobinNode>(
       "round_robin", *config_);
     first_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
@@ -141,6 +142,38 @@ TEST_F(RoundRobinNodeTestFixture, test_skikpped)
   EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
 }
 
+TEST_F(RoundRobinNodeTestFixture, test_wrap_around_disabled)
+{
+  // Create a node with wrap_around disabled
+  BT::NodeConfiguration config_no_wrap;
+  config_no_wrap.blackboard = BT::Blackboard::create();
+  config_no_wrap.input_ports["wrap_around"] = "false";
+  auto bt_node_no_wrap = std::make_shared<nav2_behavior_tree::RoundRobinNode>(
+    "round_robin_no_wrap", config_no_wrap);
+  auto first_child_no_wrap = std::make_shared<nav2_behavior_tree::DummyNode>();
+  auto second_child_no_wrap = std::make_shared<nav2_behavior_tree::DummyNode>();
+  auto third_child_no_wrap = std::make_shared<nav2_behavior_tree::DummyNode>();
+  bt_node_no_wrap->addChild(first_child_no_wrap.get());
+  bt_node_no_wrap->addChild(second_child_no_wrap.get());
+  bt_node_no_wrap->addChild(third_child_no_wrap.get());
+
+  // Test scenario where all children fail - should return FAILURE without wrapping
+  first_child_no_wrap->changeStatus(BT::NodeStatus::FAILURE);
+  second_child_no_wrap->changeStatus(BT::NodeStatus::FAILURE);
+  third_child_no_wrap->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_no_wrap->executeTick(), BT::NodeStatus::FAILURE);
+
+  // Reset and test scenario where we reach the end skipped
+  bt_node_no_wrap->halt();
+  first_child_no_wrap->changeStatus(BT::NodeStatus::SKIPPED);
+  second_child_no_wrap->changeStatus(BT::NodeStatus::SKIPPED);
+  third_child_no_wrap->changeStatus(BT::NodeStatus::SKIPPED);
+  EXPECT_EQ(bt_node_no_wrap->executeTick(), BT::NodeStatus::SKIPPED);
+  EXPECT_EQ(first_child_no_wrap->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_no_wrap->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_no_wrap->status(), BT::NodeStatus::IDLE);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp

@@ -290,7 +290,7 @@ TEST_F(BehaviorTreeTestFixture, TestAllFailure)
   EXPECT_EQ(result, BT::NodeStatus::FAILURE);
 
   // Goal count should be 2 since only two goals are sent to ComputePathToPose
-  EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 14);
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 4);
   EXPECT_EQ(server_handler->compute_path_to_pose_server->getResult()->error_code, 207);
   EXPECT_EQ(server_handler->compute_path_to_pose_server->getResult()->error_msg, "Timeout");
 
@@ -299,14 +299,14 @@ TEST_F(BehaviorTreeTestFixture, TestAllFailure)
   EXPECT_EQ(server_handler->follow_path_server->getResult()->error_code, 0);
   EXPECT_EQ(server_handler->follow_path_server->getResult()->error_msg, "");
 
-  EXPECT_EQ(server_handler->spin_server->getGoalCount(), 5);
-  EXPECT_EQ(server_handler->wait_server->getGoalCount(), 5);
-  EXPECT_EQ(server_handler->backup_server->getGoalCount(), 5);
+  EXPECT_EQ(server_handler->spin_server->getGoalCount(), 1);
+  EXPECT_EQ(server_handler->wait_server->getGoalCount(), 1);
+  EXPECT_EQ(server_handler->backup_server->getGoalCount(), 1);
 
   // Service count is 1 to try and resolve global planner error
-  EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 13);
+  EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 3);
 
-  EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 6);
+  EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 1);
 }
 
 /**
@@ -504,24 +504,24 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex)
   EXPECT_EQ(result, BT::NodeStatus::FAILURE);
 
   // ComputePathToPose is called 12 times
-  EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 7);
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 3);
   EXPECT_EQ(server_handler->compute_path_to_pose_server->getResult()->error_code, 0);
   EXPECT_EQ(server_handler->compute_path_to_pose_server->getResult()->error_msg, "");
 
   // FollowPath is called 4 times
-  EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 14);
+  EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 6);
   EXPECT_EQ(server_handler->follow_path_server->getResult()->error_code, 106);
   EXPECT_EQ(server_handler->follow_path_server->getResult()->error_msg, "No valid control");
 
   // Local costmap is cleared 5 times
-  EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 9);
+  EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 4);
 
   // Global costmap is cleared 8 times
-  EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 2);
+  EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 1);
 
   // All recovery action servers receive 2 goals
-  EXPECT_EQ(server_handler->spin_server->getGoalCount(), 2);
-  EXPECT_EQ(server_handler->wait_server->getGoalCount(), 2);
+  EXPECT_EQ(server_handler->spin_server->getGoalCount(), 1);
+  EXPECT_EQ(server_handler->wait_server->getGoalCount(), 1);
   EXPECT_EQ(server_handler->backup_server->getGoalCount(), 1);
 }