diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 7b6d475d2f8..6adf6bfae67 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/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) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/nonblocking_sequence.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/nonblocking_sequence.hpp new file mode 100644 index 00000000000..759cc4d32fd --- /dev/null +++ b/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 +#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: + */ +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_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index a6a0dc012d1..45a8f7b4219 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -397,6 +397,7 @@ + Rate diff --git a/nav2_behavior_tree/plugins/control/nonblocking_sequence.cpp b/nav2_behavior_tree/plugins/control/nonblocking_sequence.cpp new file mode 100644 index 00000000000..13e69e41ee4 --- /dev/null +++ b/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 +#include +#include + +#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("NonblockingSequence"); +} diff --git a/nav2_behavior_tree/test/plugins/control/CMakeLists.txt b/nav2_behavior_tree/test/plugins/control/CMakeLists.txt index e4bcb592be7..1a8b0cf781a 100644 --- a/nav2_behavior_tree/test/plugins/control/CMakeLists.txt +++ b/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) diff --git a/nav2_behavior_tree/test/plugins/control/test_nonblocking_sequence.cpp b/nav2_behavior_tree/test/plugins/control/test_nonblocking_sequence.cpp new file mode 100644 index 00000000000..b21dd36ff58 --- /dev/null +++ b/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 +#include + +#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( + "nonblocking_sequence", *config_); + first_child_ = std::make_shared(); + second_child_ = std::make_shared(); + third_child_ = std::make_shared(); + 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 bt_node_; + static std::shared_ptr first_child_; + static std::shared_ptr second_child_; + static std::shared_ptr third_child_; +}; + +std::shared_ptr +NonblockingSequenceTestFixture::bt_node_ = nullptr; +std::shared_ptr +NonblockingSequenceTestFixture::first_child_ = nullptr; +std::shared_ptr +NonblockingSequenceTestFixture::second_child_ = nullptr; +std::shared_ptr +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; +}