Skip to content

Commit fc8a642

Browse files
alexanderjyuenSteveMacenski
alexanderjyuen
and
SteveMacenski
authored
Nonblocking sequence bt control node (#5325)
* added nonblocking sequence Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * adding non blocking sequence test Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * adding non blocking sequence test to CMake Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * added doxygen Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * remove node_ Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * main port Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * added NonblockingSequence to nav2_tree_nodes.xml Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * cpp linting Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * cpplinting Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * clarifed nonblocking sequence functionality in doxygen Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * Update nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/nonblocking_sequence.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
1 parent dc92138 commit fc8a642

File tree

6 files changed

+318
-0
lines changed

6 files changed

+318
-0
lines changed

‎nav2_behavior_tree/CMakeLists.txt

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -212,6 +212,9 @@ list(APPEND plugin_libs nav2_get_current_pose_action_bt_node)
212212
add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp)
213213
list(APPEND plugin_libs nav2_pipeline_sequence_bt_node)
214214

215+
add_library(nav2_nonblocking_sequence_bt_node SHARED plugins/control/nonblocking_sequence.cpp)
216+
list(APPEND plugin_libs nav2_nonblocking_sequence_bt_node)
217+
215218
add_library(nav2_round_robin_node_bt_node SHARED plugins/control/round_robin_node.cpp)
216219
list(APPEND plugin_libs nav2_round_robin_node_bt_node)
217220

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

Lines changed: 91 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,91 @@
1+
// Copyright (c) 2025 Polymath Robotics
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__NONBLOCKING_SEQUENCE_HPP_
16+
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__NONBLOCKING_SEQUENCE_HPP_
17+
18+
#include <string>
19+
#include "behaviortree_cpp/control_node.h"
20+
#include "behaviortree_cpp/bt_factory.h"
21+
22+
namespace nav2_behavior_tree
23+
{
24+
25+
/** @brief Type of sequence node that keeps tickinng through all the children until all children
26+
* return SUCCESS
27+
*
28+
* Type of Control Node | Child Returns Failure | Child Returns Running
29+
* ---------------------------------------------------------------------
30+
* NonblockingSequence | Restart | Continue tickng next child
31+
*
32+
* Continue ticking next child means every node after the running node will be ticked. Even
33+
* if a previous node returns Running or Success, the subsequent nodes will be reticked.
34+
*
35+
* As an example, let's say this node has 3 children: A, B and C. At the start,
36+
* they are all IDLE.
37+
* | A | B | C |
38+
* --------------------------------
39+
* | IDLE | IDLE | IDLE |
40+
* | RUNNING | IDLE | IDLE | - at first A gets ticked. Assume it returns RUNNING
41+
* - NonblockingSequence returns RUNNING and continues to the next
42+
* - node.
43+
* | RUNNING | RUNNING | RUNNING | - Eventually all nodes will be in the running state and
44+
* - NonblockingSequence returns RUNNING
45+
* | SUCCESS | RUNNING | SUCCESS | - Even in a configuration where there are multiple nodes
46+
* - returning SUCCESS, NonblockingSequence continues on ticking all.
47+
* - nodes each time it is ticked and returns RUNNING. Note that even
48+
* - if a node returns `SUCCESS`, on the next tick, it will attempt to
49+
* - restart the node. This is too ensure that successful nodes do
50+
* - not latch a stale state while waiting for another long running
51+
* - node to be complete
52+
* | SUCCESS | SUCCESS | SUCCESS | - If all child nodes return SUCCESS the NonblockingSequence
53+
* - returns SUCCESS
54+
*
55+
* If any children at any time had returned FAILURE. NonblockingSequence would have returned FAILURE
56+
* and halted all children, ending the sequence.
57+
*
58+
* Usage in XML: <NonblockingSequence>
59+
*/
60+
class NonblockingSequence : public BT::ControlNode
61+
{
62+
public:
63+
/**
64+
* @brief A constructor for nav2_behavior_tree::NonblockingSequence
65+
* @param name Name for the XML tag for this node
66+
*/
67+
explicit NonblockingSequence(const std::string & name);
68+
69+
/**
70+
* @brief A constructor for nav2_behavior_tree::NonblockingSequence
71+
* @param name Name for the XML tag for this node
72+
* @param config BT node configuration
73+
*/
74+
NonblockingSequence(const std::string & name, const BT::NodeConfiguration & config);
75+
76+
/**
77+
* @brief Creates list of BT ports
78+
* @return BT::PortsList Containing basic ports along with node-specific ports
79+
*/
80+
static BT::PortsList providedPorts() {return {};}
81+
82+
protected:
83+
/**
84+
* @brief The main override required by a BT action
85+
* @return BT::NodeStatus Status of tick execution
86+
*/
87+
BT::NodeStatus tick() override;
88+
};
89+
} // namespace nav2_behavior_tree
90+
91+
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__NONBLOCKING_SEQUENCE_HPP_

‎nav2_behavior_tree/nav2_tree_nodes.xml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -403,6 +403,7 @@
403403

404404
<Control ID="RoundRobin"/>
405405

406+
<Control ID="NonblockingSequence"/>
406407
<!-- ############################### DECORATOR NODES ############################## -->
407408
<Decorator ID="RateController">
408409
<input_port name="hz">Rate</input_port>

‎nav2_behavior_tree/plugins/control/nonblocking_sequence.cpp

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
// Copyright (c) 2025 Polymath Robotics
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <stdexcept>
16+
#include <sstream>
17+
#include <string>
18+
19+
#include "nav2_behavior_tree/plugins/control/nonblocking_sequence.hpp"
20+
21+
namespace nav2_behavior_tree
22+
{
23+
24+
NonblockingSequence::NonblockingSequence(const std::string & name)
25+
: BT::ControlNode(name, {})
26+
{
27+
}
28+
29+
NonblockingSequence::NonblockingSequence(
30+
const std::string & name,
31+
const BT::NodeConfiguration & conf)
32+
: BT::ControlNode(name, conf)
33+
{
34+
}
35+
36+
BT::NodeStatus NonblockingSequence::tick()
37+
{
38+
bool all_success = true;
39+
40+
for (std::size_t i = 0; i < children_nodes_.size(); ++i) {
41+
auto status = children_nodes_[i]->executeTick();
42+
switch (status) {
43+
case BT::NodeStatus::FAILURE:
44+
ControlNode::haltChildren();
45+
all_success = false; // probably not needed
46+
return status;
47+
case BT::NodeStatus::SUCCESS:
48+
break;
49+
case BT::NodeStatus::RUNNING:
50+
all_success = false;
51+
break;
52+
default:
53+
std::stringstream error_msg;
54+
error_msg << "Invalid node status. Received status " << status <<
55+
"from child " << children_nodes_[i]->name();
56+
throw std::runtime_error(error_msg.str());
57+
}
58+
}
59+
60+
// Wrap up.
61+
if (all_success) {
62+
ControlNode::haltChildren();
63+
return BT::NodeStatus::SUCCESS;
64+
}
65+
66+
return BT::NodeStatus::RUNNING;
67+
}
68+
69+
} // namespace nav2_behavior_tree
70+
71+
BT_REGISTER_NODES(factory)
72+
{
73+
factory.registerNodeType<nav2_behavior_tree::NonblockingSequence>("NonblockingSequence");
74+
}

‎nav2_behavior_tree/test/plugins/control/CMakeLists.txt

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,3 +3,5 @@ plugin_add_test(test_control_recovery_node test_recovery_node.cpp nav2_recovery_
33
plugin_add_test(test_control_pipeline_sequence test_pipeline_sequence.cpp nav2_pipeline_sequence_bt_node)
44

55
plugin_add_test(test_control_round_robin_node test_round_robin_node.cpp nav2_round_robin_node_bt_node)
6+
7+
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

Lines changed: 147 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,147 @@
1+
// Copyright (c) 2025 Polymath Robotics
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <gtest/gtest.h>
16+
#include <memory>
17+
18+
#include "utils/test_behavior_tree_fixture.hpp"
19+
#include "utils/test_dummy_tree_node.hpp"
20+
#include "nav2_behavior_tree/plugins/control/nonblocking_sequence.hpp"
21+
22+
class NonblockingSequenceTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
23+
{
24+
public:
25+
void SetUp() override
26+
{
27+
bt_node_ = std::make_shared<nav2_behavior_tree::NonblockingSequence>(
28+
"nonblocking_sequence", *config_);
29+
first_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
30+
second_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
31+
third_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
32+
bt_node_->addChild(first_child_.get());
33+
bt_node_->addChild(second_child_.get());
34+
bt_node_->addChild(third_child_.get());
35+
}
36+
37+
void TearDown() override
38+
{
39+
first_child_.reset();
40+
second_child_.reset();
41+
third_child_.reset();
42+
bt_node_.reset();
43+
}
44+
45+
protected:
46+
static std::shared_ptr<nav2_behavior_tree::NonblockingSequence> bt_node_;
47+
static std::shared_ptr<nav2_behavior_tree::DummyNode> first_child_;
48+
static std::shared_ptr<nav2_behavior_tree::DummyNode> second_child_;
49+
static std::shared_ptr<nav2_behavior_tree::DummyNode> third_child_;
50+
};
51+
52+
std::shared_ptr<nav2_behavior_tree::NonblockingSequence>
53+
NonblockingSequenceTestFixture::bt_node_ = nullptr;
54+
std::shared_ptr<nav2_behavior_tree::DummyNode>
55+
NonblockingSequenceTestFixture::first_child_ = nullptr;
56+
std::shared_ptr<nav2_behavior_tree::DummyNode>
57+
NonblockingSequenceTestFixture::second_child_ = nullptr;
58+
std::shared_ptr<nav2_behavior_tree::DummyNode>
59+
NonblockingSequenceTestFixture::third_child_ = nullptr;
60+
61+
TEST_F(NonblockingSequenceTestFixture, test_failure_on_idle_child)
62+
{
63+
first_child_->changeStatus(BT::NodeStatus::IDLE);
64+
EXPECT_THROW(bt_node_->executeTick(), std::runtime_error);
65+
}
66+
67+
TEST_F(NonblockingSequenceTestFixture, test_failure)
68+
{
69+
first_child_->changeStatus(BT::NodeStatus::FAILURE);
70+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
71+
EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
72+
EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
73+
EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
74+
75+
first_child_->changeStatus(BT::NodeStatus::SUCCESS);
76+
second_child_->changeStatus(BT::NodeStatus::FAILURE);
77+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
78+
EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
79+
EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
80+
EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
81+
82+
first_child_->changeStatus(BT::NodeStatus::SUCCESS);
83+
second_child_->changeStatus(BT::NodeStatus::SUCCESS);
84+
third_child_->changeStatus(BT::NodeStatus::FAILURE);
85+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
86+
EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
87+
EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
88+
EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
89+
90+
first_child_->changeStatus(BT::NodeStatus::SUCCESS);
91+
second_child_->changeStatus(BT::NodeStatus::SUCCESS);
92+
third_child_->changeStatus(BT::NodeStatus::RUNNING);
93+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
94+
first_child_->changeStatus(BT::NodeStatus::FAILURE);
95+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
96+
EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
97+
EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
98+
EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
99+
}
100+
101+
TEST_F(NonblockingSequenceTestFixture, test_behavior)
102+
{
103+
first_child_->changeStatus(BT::NodeStatus::RUNNING);
104+
second_child_->changeStatus(BT::NodeStatus::SUCCESS);
105+
third_child_->changeStatus(BT::NodeStatus::RUNNING);
106+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
107+
108+
first_child_->changeStatus(BT::NodeStatus::SUCCESS);
109+
second_child_->changeStatus(BT::NodeStatus::SUCCESS);
110+
third_child_->changeStatus(BT::NodeStatus::RUNNING);
111+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
112+
113+
first_child_->changeStatus(BT::NodeStatus::RUNNING);
114+
second_child_->changeStatus(BT::NodeStatus::SUCCESS);
115+
third_child_->changeStatus(BT::NodeStatus::SUCCESS);
116+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
117+
118+
first_child_->changeStatus(BT::NodeStatus::SUCCESS);
119+
second_child_->changeStatus(BT::NodeStatus::SUCCESS);
120+
third_child_->changeStatus(BT::NodeStatus::SUCCESS);
121+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
122+
123+
// Even if first two children are running, we will still tick the third
124+
// node, which if set to failure, fails everything
125+
first_child_->changeStatus(BT::NodeStatus::RUNNING);
126+
second_child_->changeStatus(BT::NodeStatus::RUNNING);
127+
third_child_->changeStatus(BT::NodeStatus::FAILURE);
128+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
129+
EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
130+
EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
131+
EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
132+
}
133+
134+
int main(int argc, char ** argv)
135+
{
136+
::testing::InitGoogleTest(&argc, argv);
137+
138+
// initialize ROS
139+
rclcpp::init(argc, argv);
140+
141+
int all_successful = RUN_ALL_TESTS();
142+
143+
// shutdown ROS
144+
rclcpp::shutdown();
145+
146+
return all_successful;
147+
}

0 commit comments

Comments
 (0)