Skip to content

Commit

Permalink
Update TestNode and the corresponding tutorial
Browse files Browse the repository at this point in the history
  • Loading branch information
@facontidavide
facontidavide committed Apr 24, 2024
1 parent 561eead commit 9c644e1
Show file tree
Hide file tree
Showing 4 changed files with 155 additions and 109 deletions.
176 changes: 105 additions & 71 deletions examples/t11_replace_rules.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,14 +9,17 @@ static const char* xml_text = R"(
<BehaviorTree ID="MainTree">
<Sequence>
<SaySomething name="talk" message="hello world"/>
<Fallback>
<AlwaysFailure name="failing_action"/>
<SubTree ID="MySub" name="mysub"/>
</Fallback>
<SaySomething message="before last_action"/>
<Script code="msg:='after last_action'"/>
<AlwaysSuccess name="last_action"/>
<SaySomething message="{msg}"/>
<SubTree ID="MySub" name="mysub"/>
<Script name="set_message" code="msg:= 'the original message' "/>
<SaySomething message="{msg}"/>
<Sequence name="counting">
<SaySomething message="1"/>
<SaySomething message="2"/>
<SaySomething message="3"/>
</Sequence>
</Sequence>
</BehaviorTree>
Expand All @@ -30,95 +33,126 @@ static const char* xml_text = R"(
</root>
)";

static const char* json_text = R"(
{
"TestNodeConfigs": {
"MyTest": {
"async_delay": 2000,
"return_status": "SUCCESS",
"post_script": "msg ='message SUBSTITUED'"
}
},
"SubstitutionRules": {
"mysub/action_*": "TestAction",
"talk": "TestSaySomething",
"last_action": "MyTest"
}
}
)";

// clang-format on

/**
* @brief In this example we will see how we can substitute some nodes
* in the Tree above with
* @param argc
* @param argv
* @return
*/

int main(int argc, char** argv)
{
using namespace DummyNodes;
BT::BehaviorTreeFactory factory;

factory.registerNodeType<SaySomething>("SaySomething");
factory.registerBehaviorTreeFromText(xml_text);

// We use lambdas and registerSimpleAction, to create
// a "dummy" node, that we want to create instead of a given one.
// let's check what the "original" tree should return
{
auto tree = factory.createTree("MainTree");

std::cout << "----- Nodes fullPath() -------\n";
// as a reminder, let's print the full names of all the nodes
tree.applyVisitor(
[](BT::TreeNode* node) { std::cout << node->fullPath() << std::endl; });

std::cout << "\n------ Output (original) ------\n";
tree.tickWhileRunning();
}

// We have three mechanisms to create Nodes to be used as "mocks".
// We will see later how to use them.

//---------------------------------------------------------------
// Mock type 1: register a specific "dummy" Node into the factory
// You can use any registration method, but to keep this short,
// we use registerSimpleAction()

// Simple node that just prints its name and return SUCCESS
factory.registerSimpleAction("DummyAction", [](BT::TreeNode& self) {
std::cout << "DummyAction substituting: " << self.name() << std::endl;
std::cout << "DummyAction substituting node with fullPath(): " << self.fullPath()
<< std::endl;
return BT::NodeStatus::SUCCESS;
});

// Action that is meant to substitute SaySomething.
// It will try to use the input port "message"
factory.registerSimpleAction("TestSaySomething", [](BT::TreeNode& self) {
factory.registerSimpleAction("DummySaySomething", [](BT::TreeNode& self) {
auto msg = self.getInput<std::string>("message");
if(!msg)
{
throw BT::RuntimeError("missing required input [message]: ", msg.error());
}
std::cout << "TestSaySomething: " << msg.value() << std::endl;
std::cout << "DummySaySomething: " << msg.value() << std::endl;
return BT::NodeStatus::SUCCESS;
});

//----------------------------
// pass "no_sub" as first argument to avoid adding rules
bool skip_substitution = (argc == 2) && std::string(argv[1]) == "no_sub";
//---------------------------------------------------------------
// Mock type 2: Use our configurable BT::TestNode

// This is the configuration passed to the TestNode
BT::TestNodeConfig test_config;
// we want this to return always SUCCESS
test_config.return_status = BT::NodeStatus::SUCCESS;
// Convert the node in asynchronous and wait 2000 ms
test_config.async_delay = std::chrono::milliseconds(2000);
// Execute this postcondition, once completed
test_config.post_script = "msg := 'message SUBSTITUTED' ";

if(!skip_substitution)
// this will be synchronous (async_delay is 0)
BT::TestNodeConfig counting_config;
test_config.return_status = BT::NodeStatus::SUCCESS;

//---------------------------------------------------------------
// Next, we want to substitute one or more of out Nodes with this mocks
// The simplest way is to use a JSON file, otherwise we can do it manually.
bool const USE_JSON = true;

if(!USE_JSON) // manually add substitution rules
{
// we can use a JSON file to configure the substitution rules
// or do it manually
bool const USE_JSON = true;
// Substitute nodes which match the wildcard pattern "mysub/action_*"
// with DummyAction
factory.addSubstitutionRule("mysub/action_*", "DummyAction");

if(USE_JSON)
{
factory.loadSubstitutionRuleFromJSON(json_text);
}
else
{
// Substitute nodes which match this wildcard pattern with TestAction
factory.addSubstitutionRule("mysub/action_*", "TestAction");

// Substitute the node with name [talk] with TestSaySomething
factory.addSubstitutionRule("talk", "TestSaySomething");

// This configuration will be passed to a TestNode
BT::TestNodeConfig test_config;
// Convert the node in asynchronous and wait 2000 ms
test_config.async_delay = std::chrono::milliseconds(2000);
// Execute this postcondition, once completed
test_config.post_script = "msg ='message SUBSTITUED'";

// Substitute the node with name [last_action] with a TestNode,
// configured using test_config
factory.addSubstitutionRule("last_action", test_config);
}
}
// Substitute the node with name "talk" with DummySaySomething
factory.addSubstitutionRule("talk", "DummySaySomething");

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

// we can also substitute entire branches, for instance the Sequence "counting"
factory.addSubstitutionRule("counting", counting_config);
}
else // use a JSON file to apply substitution rules programmatically
{
// this JSON is equivalent to the code we wrote above
const char* json_text = R"(
{
"TestNodeConfigs": {
"NewMessage": {
"async_delay": 2000,
"return_status": "SUCCESS",
"post_script": "msg ='message SUBSTITUTED'"
},
"NoCounting": {
"return_status": "SUCCESS"
}
},
"SubstitutionRules": {
"mysub/action_*": "DummyAction",
"talk": "DummySaySomething",
"set_message": "NewMessage",
"counting": "NoCounting"
}
})";

factory.loadSubstitutionRuleFromJSON(json_text);
}
//---------------------------------------------------------------
// IMPORTANT: all substiutions must be done BEFORE creating the tree
// During the construction phase of the tree, the substitution
// rules will be used to instantiate the test nodes, instead of the
// original ones.
auto tree = factory.createTree("MainTree");
std::cout << "\n------ Output (substituted) ------\n";
tree.tickWhileRunning();

return 0;
Expand Down
20 changes: 13 additions & 7 deletions include/behaviortree_cpp/actions/test_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,20 +22,24 @@ namespace BT

struct TestNodeConfig
{
/// status to return when the action is completed
/// status to return when the action is completed.
NodeStatus return_status = NodeStatus::SUCCESS;

/// script to execute when complete_func() returns SUCCESS
std::string success_script;

/// script to execute when complete_func() returns FAILURE
std::string failure_script;

/// script to execute when actions is completed
std::string post_script;

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

/// C++ callback to execute at the beginning
std::function<void()> pre_func;

/// C++ callback to execute at the end
std::function<void()> post_func;
/// Function invoked when the action is completed. By default just return [return_status]
/// Override it to intorduce more comple cases
std::function<NodeStatus(void)> complete_func = [this]() { return return_status; };
};

/**
Expand Down Expand Up @@ -84,7 +88,9 @@ class TestNode : public BT::StatefulActionNode
NodeStatus onCompleted();

TestNodeConfig _test_config;
ScriptFunction _executor;
ScriptFunction _success_executor;
ScriptFunction _failure_executor;
ScriptFunction _post_executor;
TimerQueue<> _timer;
std::atomic_bool _completed = false;
};
Expand Down
42 changes: 24 additions & 18 deletions src/actions/test_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,24 +8,24 @@ void BT::TestNode::setConfig(const TestNodeConfig& config)
}
_test_config = config;

if(!_test_config.post_script.empty())
{
auto executor = ParseScript(_test_config.post_script);
if(!executor)
auto prepareScript = [](const std::string& script, auto& executor) {
if(!script.empty())
{
throw RuntimeError(executor.error());
auto result = ParseScript(script);
if(!result)
{
throw RuntimeError(result.error());
}
executor = result.value();
}
_executor = executor.value();
}
};
prepareScript(config.success_script, _success_executor);
prepareScript(config.failure_script, _failure_executor);
prepareScript(config.post_script, _post_executor);
}

BT::NodeStatus BT::TestNode::onStart()
{
if(_test_config.pre_func)
{
_test_config.pre_func();
}

if(_test_config.async_delay <= std::chrono::milliseconds(0))
{
return onCompleted();
Expand Down Expand Up @@ -63,14 +63,20 @@ void BT::TestNode::onHalted()

BT::NodeStatus BT::TestNode::onCompleted()
{
if(_executor)
Ast::Environment env = { config().blackboard, config().enums };

auto status = _test_config.complete_func();
if(status == NodeStatus::SUCCESS && _success_executor)
{
_success_executor(env);
}
else if(status == NodeStatus::FAILURE && _failure_executor)
{
Ast::Environment env = { config().blackboard, config().enums };
_executor(env);
_failure_executor(env);
}
if(_test_config.post_func)
if(_post_executor)
{
_test_config.post_func();
_post_executor(env);
}
return _test_config.return_status;
return status;
}
26 changes: 13 additions & 13 deletions src/bt_factory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -326,7 +326,7 @@ std::unique_ptr<TreeNode> BehaviorTreeFactory::instantiateTreeNode(
}
else
{
throw RuntimeError("Substituted Node ID not found");
throw RuntimeError("Substituted Node ID [", *substituted_ID, "] not found");
}
substituted = true;
break;
Expand Down Expand Up @@ -490,8 +490,8 @@ void BehaviorTreeFactory::loadSubstitutionRuleFromJSON(const std::string& json_t
{
auto& config = configs[name];

auto status = test_config.at("return_status").get<std::string>();
config.return_status = convertFromString<NodeStatus>(status);
auto return_status = test_config.at("return_status").get<std::string>();
config.return_status = convertFromString<NodeStatus>(return_status);
if(test_config.contains("async_delay"))
{
config.async_delay =
Expand All @@ -501,6 +501,14 @@ void BehaviorTreeFactory::loadSubstitutionRuleFromJSON(const std::string& json_t
{
config.post_script = test_config["post_script"].get<std::string>();
}
if(test_config.contains("success_script"))
{
config.success_script = test_config["success_script"].get<std::string>();
}
if(test_config.contains("failure_script"))
{
config.failure_script = test_config["failure_script"].get<std::string>();
}
}

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

void Tree::applyVisitor(const std::function<void(const TreeNode*)>& visitor)
{
for(auto const& subtree : subtrees)
{
BT::applyRecursiveVisitor(static_cast<const TreeNode*>(subtree->nodes.front().get()),
visitor);
}
BT::applyRecursiveVisitor(static_cast<const TreeNode*>(rootNode()), visitor);
}

void Tree::applyVisitor(const std::function<void(TreeNode*)>& visitor)
{
for(auto const& subtree : subtrees)
{
BT::applyRecursiveVisitor(static_cast<TreeNode*>(subtree->nodes.front().get()),
visitor);
}
BT::applyRecursiveVisitor(static_cast<TreeNode*>(rootNode()), visitor);
}

uint16_t Tree::getUID()
Expand Down

0 comments on commit 9c644e1

Please sign in to comment.