Skip to content

Commit 9c644e1

Browse files
facontidavidefacontidavide
committed
Update TestNode and the corresponding tutorial
1 parent 561eead commit 9c644e1

File tree

4 files changed

+155
-109
lines changed

4 files changed

+155
-109
lines changed

‎examples/t11_replace_rules.cpp‎

Lines changed: 105 additions & 71 deletions
Original file line numberDiff line numberDiff line change
@@ -9,14 +9,17 @@ static const char* xml_text = R"(
99
<BehaviorTree ID="MainTree">
1010
<Sequence>
1111
<SaySomething name="talk" message="hello world"/>
12-
<Fallback>
13-
<AlwaysFailure name="failing_action"/>
14-
<SubTree ID="MySub" name="mysub"/>
15-
</Fallback>
16-
<SaySomething message="before last_action"/>
17-
<Script code="msg:='after last_action'"/>
18-
<AlwaysSuccess name="last_action"/>
19-
<SaySomething message="{msg}"/>
12+
13+
<SubTree ID="MySub" name="mysub"/>
14+
15+
<Script name="set_message" code="msg:= 'the original message' "/>
16+
<SaySomething message="{msg}"/>
17+
18+
<Sequence name="counting">
19+
<SaySomething message="1"/>
20+
<SaySomething message="2"/>
21+
<SaySomething message="3"/>
22+
</Sequence>
2023
</Sequence>
2124
</BehaviorTree>
2225
@@ -30,95 +33,126 @@ static const char* xml_text = R"(
3033
</root>
3134
)";
3235

33-
static const char* json_text = R"(
34-
{
35-
"TestNodeConfigs": {
36-
"MyTest": {
37-
"async_delay": 2000,
38-
"return_status": "SUCCESS",
39-
"post_script": "msg ='message SUBSTITUED'"
40-
}
41-
},
42-
43-
"SubstitutionRules": {
44-
"mysub/action_*": "TestAction",
45-
"talk": "TestSaySomething",
46-
"last_action": "MyTest"
47-
}
48-
}
49-
)";
50-
5136
// clang-format on
5237

38+
/**
39+
* @brief In this example we will see how we can substitute some nodes
40+
* in the Tree above with
41+
* @param argc
42+
* @param argv
43+
* @return
44+
*/
45+
5346
int main(int argc, char** argv)
5447
{
5548
using namespace DummyNodes;
5649
BT::BehaviorTreeFactory factory;
57-
5850
factory.registerNodeType<SaySomething>("SaySomething");
51+
factory.registerBehaviorTreeFromText(xml_text);
5952

60-
// We use lambdas and registerSimpleAction, to create
61-
// a "dummy" node, that we want to create instead of a given one.
53+
// let's check what the "original" tree should return
54+
{
55+
auto tree = factory.createTree("MainTree");
56+
57+
std::cout << "----- Nodes fullPath() -------\n";
58+
// as a reminder, let's print the full names of all the nodes
59+
tree.applyVisitor(
60+
[](BT::TreeNode* node) { std::cout << node->fullPath() << std::endl; });
61+
62+
std::cout << "\n------ Output (original) ------\n";
63+
tree.tickWhileRunning();
64+
}
65+
66+
// We have three mechanisms to create Nodes to be used as "mocks".
67+
// We will see later how to use them.
68+
69+
//---------------------------------------------------------------
70+
// Mock type 1: register a specific "dummy" Node into the factory
71+
// You can use any registration method, but to keep this short,
72+
// we use registerSimpleAction()
6273

63-
// Simple node that just prints its name and return SUCCESS
6474
factory.registerSimpleAction("DummyAction", [](BT::TreeNode& self) {
65-
std::cout << "DummyAction substituting: " << self.name() << std::endl;
75+
std::cout << "DummyAction substituting node with fullPath(): " << self.fullPath()
76+
<< std::endl;
6677
return BT::NodeStatus::SUCCESS;
6778
});
6879

69-
// Action that is meant to substitute SaySomething.
70-
// It will try to use the input port "message"
71-
factory.registerSimpleAction("TestSaySomething", [](BT::TreeNode& self) {
80+
factory.registerSimpleAction("DummySaySomething", [](BT::TreeNode& self) {
7281
auto msg = self.getInput<std::string>("message");
73-
if(!msg)
74-
{
75-
throw BT::RuntimeError("missing required input [message]: ", msg.error());
76-
}
77-
std::cout << "TestSaySomething: " << msg.value() << std::endl;
82+
std::cout << "DummySaySomething: " << msg.value() << std::endl;
7883
return BT::NodeStatus::SUCCESS;
7984
});
8085

81-
//----------------------------
82-
// pass "no_sub" as first argument to avoid adding rules
83-
bool skip_substitution = (argc == 2) && std::string(argv[1]) == "no_sub";
86+
//---------------------------------------------------------------
87+
// Mock type 2: Use our configurable BT::TestNode
88+
89+
// This is the configuration passed to the TestNode
90+
BT::TestNodeConfig test_config;
91+
// we want this to return always SUCCESS
92+
test_config.return_status = BT::NodeStatus::SUCCESS;
93+
// Convert the node in asynchronous and wait 2000 ms
94+
test_config.async_delay = std::chrono::milliseconds(2000);
95+
// Execute this postcondition, once completed
96+
test_config.post_script = "msg := 'message SUBSTITUTED' ";
8497

85-
if(!skip_substitution)
98+
// this will be synchronous (async_delay is 0)
99+
BT::TestNodeConfig counting_config;
100+
test_config.return_status = BT::NodeStatus::SUCCESS;
101+
102+
//---------------------------------------------------------------
103+
// Next, we want to substitute one or more of out Nodes with this mocks
104+
// The simplest way is to use a JSON file, otherwise we can do it manually.
105+
bool const USE_JSON = true;
106+
107+
if(!USE_JSON) // manually add substitution rules
86108
{
87-
// we can use a JSON file to configure the substitution rules
88-
// or do it manually
89-
bool const USE_JSON = true;
109+
// Substitute nodes which match the wildcard pattern "mysub/action_*"
110+
// with DummyAction
111+
factory.addSubstitutionRule("mysub/action_*", "DummyAction");
90112

91-
if(USE_JSON)
92-
{
93-
factory.loadSubstitutionRuleFromJSON(json_text);
94-
}
95-
else
96-
{
97-
// Substitute nodes which match this wildcard pattern with TestAction
98-
factory.addSubstitutionRule("mysub/action_*", "TestAction");
99-
100-
// Substitute the node with name [talk] with TestSaySomething
101-
factory.addSubstitutionRule("talk", "TestSaySomething");
102-
103-
// This configuration will be passed to a TestNode
104-
BT::TestNodeConfig test_config;
105-
// Convert the node in asynchronous and wait 2000 ms
106-
test_config.async_delay = std::chrono::milliseconds(2000);
107-
// Execute this postcondition, once completed
108-
test_config.post_script = "msg ='message SUBSTITUED'";
109-
110-
// Substitute the node with name [last_action] with a TestNode,
111-
// configured using test_config
112-
factory.addSubstitutionRule("last_action", test_config);
113-
}
114-
}
113+
// Substitute the node with name "talk" with DummySaySomething
114+
factory.addSubstitutionRule("talk", "DummySaySomething");
115115

116-
factory.registerBehaviorTreeFromText(xml_text);
116+
// Substitute the node with name "set_message" with
117+
// the a BT::TestNode with the give configuration
118+
factory.addSubstitutionRule("set_message", test_config);
117119

120+
// we can also substitute entire branches, for instance the Sequence "counting"
121+
factory.addSubstitutionRule("counting", counting_config);
122+
}
123+
else // use a JSON file to apply substitution rules programmatically
124+
{
125+
// this JSON is equivalent to the code we wrote above
126+
const char* json_text = R"(
127+
{
128+
"TestNodeConfigs": {
129+
"NewMessage": {
130+
"async_delay": 2000,
131+
"return_status": "SUCCESS",
132+
"post_script": "msg ='message SUBSTITUTED'"
133+
},
134+
"NoCounting": {
135+
"return_status": "SUCCESS"
136+
}
137+
},
138+
139+
"SubstitutionRules": {
140+
"mysub/action_*": "DummyAction",
141+
"talk": "DummySaySomething",
142+
"set_message": "NewMessage",
143+
"counting": "NoCounting"
144+
}
145+
})";
146+
147+
factory.loadSubstitutionRuleFromJSON(json_text);
148+
}
149+
//---------------------------------------------------------------
150+
// IMPORTANT: all substiutions must be done BEFORE creating the tree
118151
// During the construction phase of the tree, the substitution
119152
// rules will be used to instantiate the test nodes, instead of the
120153
// original ones.
121154
auto tree = factory.createTree("MainTree");
155+
std::cout << "\n------ Output (substituted) ------\n";
122156
tree.tickWhileRunning();
123157

124158
return 0;

‎include/behaviortree_cpp/actions/test_node.h‎

Lines changed: 13 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -22,20 +22,24 @@ namespace BT
2222

2323
struct TestNodeConfig
2424
{
25-
/// status to return when the action is completed
25+
/// status to return when the action is completed.
2626
NodeStatus return_status = NodeStatus::SUCCESS;
2727

28+
/// script to execute when complete_func() returns SUCCESS
29+
std::string success_script;
30+
31+
/// script to execute when complete_func() returns FAILURE
32+
std::string failure_script;
33+
2834
/// script to execute when actions is completed
2935
std::string post_script;
3036

3137
/// if async_delay > 0, this action become asynchronous and wait this amount of time
3238
std::chrono::milliseconds async_delay = std::chrono::milliseconds(0);
3339

34-
/// C++ callback to execute at the beginning
35-
std::function<void()> pre_func;
36-
37-
/// C++ callback to execute at the end
38-
std::function<void()> post_func;
40+
/// Function invoked when the action is completed. By default just return [return_status]
41+
/// Override it to intorduce more comple cases
42+
std::function<NodeStatus(void)> complete_func = [this]() { return return_status; };
3943
};
4044

4145
/**
@@ -84,7 +88,9 @@ class TestNode : public BT::StatefulActionNode
8488
NodeStatus onCompleted();
8589

8690
TestNodeConfig _test_config;
87-
ScriptFunction _executor;
91+
ScriptFunction _success_executor;
92+
ScriptFunction _failure_executor;
93+
ScriptFunction _post_executor;
8894
TimerQueue<> _timer;
8995
std::atomic_bool _completed = false;
9096
};

‎src/actions/test_node.cpp‎

Lines changed: 24 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -8,24 +8,24 @@ void BT::TestNode::setConfig(const TestNodeConfig& config)
88
}
99
_test_config = config;
1010

11-
if(!_test_config.post_script.empty())
12-
{
13-
auto executor = ParseScript(_test_config.post_script);
14-
if(!executor)
11+
auto prepareScript = [](const std::string& script, auto& executor) {
12+
if(!script.empty())
1513
{
16-
throw RuntimeError(executor.error());
14+
auto result = ParseScript(script);
15+
if(!result)
16+
{
17+
throw RuntimeError(result.error());
18+
}
19+
executor = result.value();
1720
}
18-
_executor = executor.value();
19-
}
21+
};
22+
prepareScript(config.success_script, _success_executor);
23+
prepareScript(config.failure_script, _failure_executor);
24+
prepareScript(config.post_script, _post_executor);
2025
}
2126

2227
BT::NodeStatus BT::TestNode::onStart()
2328
{
24-
if(_test_config.pre_func)
25-
{
26-
_test_config.pre_func();
27-
}
28-
2929
if(_test_config.async_delay <= std::chrono::milliseconds(0))
3030
{
3131
return onCompleted();
@@ -63,14 +63,20 @@ void BT::TestNode::onHalted()
6363

6464
BT::NodeStatus BT::TestNode::onCompleted()
6565
{
66-
if(_executor)
66+
Ast::Environment env = { config().blackboard, config().enums };
67+
68+
auto status = _test_config.complete_func();
69+
if(status == NodeStatus::SUCCESS && _success_executor)
70+
{
71+
_success_executor(env);
72+
}
73+
else if(status == NodeStatus::FAILURE && _failure_executor)
6774
{
68-
Ast::Environment env = { config().blackboard, config().enums };
69-
_executor(env);
75+
_failure_executor(env);
7076
}
71-
if(_test_config.post_func)
77+
if(_post_executor)
7278
{
73-
_test_config.post_func();
79+
_post_executor(env);
7480
}
75-
return _test_config.return_status;
81+
return status;
7682
}

‎src/bt_factory.cpp‎

Lines changed: 13 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -326,7 +326,7 @@ std::unique_ptr<TreeNode> BehaviorTreeFactory::instantiateTreeNode(
326326
}
327327
else
328328
{
329-
throw RuntimeError("Substituted Node ID not found");
329+
throw RuntimeError("Substituted Node ID [", *substituted_ID, "] not found");
330330
}
331331
substituted = true;
332332
break;
@@ -490,8 +490,8 @@ void BehaviorTreeFactory::loadSubstitutionRuleFromJSON(const std::string& json_t
490490
{
491491
auto& config = configs[name];
492492

493-
auto status = test_config.at("return_status").get<std::string>();
494-
config.return_status = convertFromString<NodeStatus>(status);
493+
auto return_status = test_config.at("return_status").get<std::string>();
494+
config.return_status = convertFromString<NodeStatus>(return_status);
495495
if(test_config.contains("async_delay"))
496496
{
497497
config.async_delay =
@@ -501,6 +501,14 @@ void BehaviorTreeFactory::loadSubstitutionRuleFromJSON(const std::string& json_t
501501
{
502502
config.post_script = test_config["post_script"].get<std::string>();
503503
}
504+
if(test_config.contains("success_script"))
505+
{
506+
config.success_script = test_config["success_script"].get<std::string>();
507+
}
508+
if(test_config.contains("failure_script"))
509+
{
510+
config.failure_script = test_config["failure_script"].get<std::string>();
511+
}
504512
}
505513

506514
auto substitutions = json.at("SubstitutionRules");
@@ -616,20 +624,12 @@ Blackboard::Ptr Tree::rootBlackboard()
616624

617625
void Tree::applyVisitor(const std::function<void(const TreeNode*)>& visitor)
618626
{
619-
for(auto const& subtree : subtrees)
620-
{
621-
BT::applyRecursiveVisitor(static_cast<const TreeNode*>(subtree->nodes.front().get()),
622-
visitor);
623-
}
627+
BT::applyRecursiveVisitor(static_cast<const TreeNode*>(rootNode()), visitor);
624628
}
625629

626630
void Tree::applyVisitor(const std::function<void(TreeNode*)>& visitor)
627631
{
628-
for(auto const& subtree : subtrees)
629-
{
630-
BT::applyRecursiveVisitor(static_cast<TreeNode*>(subtree->nodes.front().get()),
631-
visitor);
632-
}
632+
BT::applyRecursiveVisitor(static_cast<TreeNode*>(rootNode()), visitor);
633633
}
634634

635635
uint16_t Tree::getUID()

0 commit comments

Comments
 (0)