diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp index 86f2b38c5bb..eecc00170db 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp +++ b/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("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 diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 4439393afa4..bc8d358ee28 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -388,7 +388,9 @@ Number of retries - + + Enable wrap-around to first child after last child fails + diff --git a/nav2_behavior_tree/plugins/control/round_robin_node.cpp b/nav2_behavior_tree/plugins/control/round_robin_node.cpp index 9293a5b1181..5eb6ce3ddb4 100644 --- a/nav2_behavior_tree/plugins/control/round_robin_node.cpp +++ b/nav2_behavior_tree/plugins/control/round_robin_node.cpp @@ -20,7 +20,7 @@ namespace nav2_behavior_tree { RoundRobinNode::RoundRobinNode(const std::string & name) -: BT::ControlNode::ControlNode(name, {}) +: BT::ControlNode::ControlNode(name, {}), wrap_around_(false) { } @@ -29,6 +29,7 @@ RoundRobinNode::RoundRobinNode( const BT::NodeConfiguration & config) : BT::ControlNode(name, config) { + getInput("wrap_around", wrap_around_); } BT::NodeStatus RoundRobinNode::tick() @@ -43,9 +44,19 @@ BT::NodeStatus RoundRobinNode::tick() 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; + } } } diff --git a/nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp b/nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp index 7daa91c0697..026d2c86c49 100644 --- a/nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp +++ b/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( "round_robin", *config_); first_child_ = std::make_shared(); @@ -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( + "round_robin_no_wrap", config_no_wrap); + auto first_child_no_wrap = std::make_shared(); + auto second_child_no_wrap = std::make_shared(); + auto third_child_no_wrap = std::make_shared(); + 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); diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 2f62894abb7..19b1462cecb 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/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); }