Introduction to behavior tree

Original link

an introduction to

Behavior Tree

library

It is mainly used for robot and game AI instead of finite element state machine

characteristic:

  • You can perform asynchronous actions
  • You can create a tree at run time
  • Custom trees can be converted into plug-in links and loaded dynamically at run time
  • Including log / optimization architecture, which can be visualized, recorded, replayed and analyzed

What is a behavior tree?

Behavior tree (BT) is a virtual entity whose structure transforms between different automatic terminal tasks, such as robots or games

Advantages of BT over FSM:

  • They are essentially hierarchical
  • Their graphics represent the meaning of language
  • They are more expressive

Why do we need a behavior tree?

be used for Component Based Software Engineering

A good software architecture has the following characteristics:

  • modularization
  • Reusable components
  • Compressibility
  • Good separation of concerns

Basic learning

Introduce BT

tick() trigger

When TreeNode is triggered, NodeStatus is returned

  • SUCCESS
  • FAILURE
  • RUNNING

RUNNING means asynchronous execution

Type of node

Control nodes have 1-N child nodes. After receiving a tick, the tick will be propagated to one or more child nodes

Decorator nodes are similar to control nodes, but have only one child node

Action nodes are leaf nodes without child nodes. Users need to implement their own action nodes to perform actual tasks

Condition nodes are equal to action nodes, but they are always atomic and synchronous. They cannot return to the RUNNING state or change the system state

Sequence example

SequenceNode most used by BT

The child nodes of the control node are ordered and executed from left to right

In short:

  • The child node returns success. tick the next node
  • The child node returns failure, no child node will tick, and the sequence returns failure
  • If all child nodes return success, the sequence returns success

Decorator

Purpose:

  • Convert results received from child nodes
  • Stop execution of child nodes
  • Repeat tick child nodes, depending on the type of decoration node

The node Inverter decorator reverses the returned value

Node Retry repeats child nodes N times

The above node combination will cancel all actions after the door is opened

FallbackNodes

Also known as Selectors, these nodes, as the name suggests, can express the fallback strategy, that is, what to do next if a child node returns FAILURE.

Trigger in sequence:

  • If the child node returns a failure, tick the next node
  • If the return is successful, the tick node is no longer, and the Fallback returns success

course

Create tree

code

#include <iostream>
#include <behaviortree_cpp_v3/bt_factory.h>

class ApproachGripper : public BT::SyncActionNode
{
public:
    explicit ApproachGripper(const std::string &name) : BT::SyncActionNode(name, {})
    {}

    BT::NodeStatus tick() override
    {
        std::cout << "ApproachObject: " << name() << std::endl;
        return BT::NodeStatus::SUCCESS;
    }
};

bool batteryOK = false;

BT::NodeStatus CheckBattery()
{
    if (batteryOK) {
        std::cout << "[ Battery: OK ]" << std::endl;
        return BT::NodeStatus::SUCCESS;
    } else {
        std::cout << "[ Battery: BAD ]" << std::endl;
        return BT::NodeStatus::FAILURE;
    }
}

BT::NodeStatus Charge()
{
    std::cout << "[ Charge Battery ]" << std::endl;
    return BT::NodeStatus::SUCCESS;
}

class GripperInterface
{
public:
    GripperInterface() : _open(true)
    {}

    BT::NodeStatus open()
    {
        _open = true;
        std::cout << "GripperInterface::open" << std::endl;
        return BT::NodeStatus::SUCCESS;
    }

    BT::NodeStatus close()
    {
        _open = false;
        std::cout << "GripperInterface::close" << std::endl;
        return BT::NodeStatus::SUCCESS;
    }

private:
    bool _open;
};

int main()
{
    BT::BehaviorTreeFactory factory;
    factory.registerNodeType<ApproachGripper>("ApproachGripper");
    factory.registerSimpleCondition("CheckBattery",
                                    [](BT::TreeNode &node) { return CheckBattery(); });
    factory.registerSimpleCondition("Charge",
                                    [](BT::TreeNode &node) { return Charge(); });
    GripperInterface gripper;
    factory.registerSimpleAction("OpenGripper",
                                 [&gripper](BT::TreeNode &node) { return gripper.open(); });
    factory.registerSimpleAction("CloseGripper",
                                 [&gripper](BT::TreeNode &node) { return gripper.close(); });

    auto tree = factory.createTreeFromFile("../my_tree1.xml");
    tree.tickRoot();
}

<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
    <!-- // -->
    <BehaviorTree ID="BehaviorTree">
        <Sequence>
            <Fallback>
                <Condition ID="CheckBattery"/>
                <Condition ID="Charge"/>
            </Fallback>
            <Action ID="OpenGripper"/>
            <Action ID="ApproachGripper"/>
            <Action ID="CloseGripper"/>
        </Sequence>
    </BehaviorTree>
    <!-- // -->
    <TreeNodesModel>
        <Action ID="ApproachGripper"/>
        <Condition ID="Charge"/>
        <Condition ID="CheckBattery"/>
        <Action ID="CloseGripper"/>
        <Action ID="OpenGripper"/>
    </TreeNodesModel>
    <!-- // -->
</root>

The results after execution are as follows

[ Battery: BAD ]
[ Charge Battery ]
GripperInterface::open
ApproachObject: ApproachGripper
GripperInterface::close

Only nodes with < treenodesmodel > can be read with Groot normally

Input and output interfaces

The behavior tree supports the use of interfaces (ports) to transmit data streams, which is simple to use and type safe

Input interface

The correct input has:

  • Static strings can be parsed by nodes
  • The "pointer" to an entry on the blackboard is identified by a key

blackboard is a simple key / value store shared by all nodes

An entry in blackboard is equivalent to a key / value pair (semantic reference database)

The input interface can read the entry in the blackboard, and the output port can write the entry

Output interface

The output port can be a string or an entry of the blackboard

Strings like {the_answer} represent the name of the on the blackboard_ Entry for answer

code

//
// Created by jiang on 2021/9/18.
//

#include <behaviortree_cpp_v3/action_node.h>
#include <behaviortree_cpp_v3/bt_factory.h>

using namespace BT;

class SaySomething : public SyncActionNode
{
public:
    SaySomething(const std::string &name, const NodeConfiguration &config) :
            SyncActionNode(name, config)
    {}

    static PortsList providedPorts()
    {
        return {InputPort<std::string>("message")};
    }

protected:
    NodeStatus tick() override
    {
        Optional<std::string> msg = getInput<std::string>("message");
        if (!msg) {
            throw RuntimeError("missing required input [message]: ", msg.error());
        }
        std::cout << "Robot says: " << msg.value() << std::endl;
        return NodeStatus::SUCCESS;
    }
};

NodeStatus SaySomethingSimple(TreeNode &self)
{
    Optional<std::string> msg = self.getInput<std::string>("message");
    if (!msg) {
        throw RuntimeError("missing required input [message]: ", msg.error());
    }
    std::cout << "Robot says: " << msg.value() << std::endl;
    return NodeStatus::SUCCESS;
}

class ThinkWhatToSay : public SyncActionNode
{
public:
    ThinkWhatToSay(const std::string &name, const NodeConfiguration &config) :
            SyncActionNode(name, config)
    {}

    static PortsList providedPorts()
    {
        return {OutputPort<std::string>("text")};
    }

protected:
    NodeStatus tick() override
    {
        setOutput("text", "The answer is 42");
        return NodeStatus::SUCCESS;
    }
};

int main()
{
    BehaviorTreeFactory factory;
    factory.registerNodeType<SaySomething>("SaySomething");
    factory.registerNodeType<ThinkWhatToSay>("ThinkWhatToSay");

    PortsList saySomethingPorts = {InputPort<std::string>("message")};
    factory.registerSimpleAction("SaySomething2", SaySomethingSimple, saySomethingPorts);
    auto tree = factory.createTreeFromFile("../../test_port/port1.xml");
    tree.tickRoot();
    return 0;
}
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
    <!-- // -->
    <BehaviorTree ID="BehaviorTree">
        <Sequence>
            <Action ID="SaySomething" message="start thinking..."/>
            <Action ID="ThinkWhatToSay" text="{the_answer}"/>
            <Action ID="SaySomething" message="{the_answer}"/>
            <Action ID="SaySomething2" message="say something 2 works"/>
            <Action ID="SaySomething2" message="{the_answer}"/>
        </Sequence>
    </BehaviorTree>
    <!-- // -->
    <TreeNodesModel>
        <Action ID="SaySomething">
            <input_port name="message"/>
        </Action>
        <Action ID="SaySomething2">
            <input_port name="message"/>
        </Action>
        <Action ID="ThinkWhatToSay">
            <output_port name="text"/>
        </Action>
    </TreeNodesModel>
    <!-- // -->
</root>

General interface

By overloading the template function, you can convert the string into a custom class structure

template <typename T> 
inline T convertFromString(StringView str);

// for example
struct Position2D 
{ 
  double x;
  double y; 
};

namespace BT
{
    template <> inline Position2D convertFromString(StringView str)
    {
        // The next line should be removed...
        printf("Converting string: \"%s\"\n", str.data() );

        // We expect real numbers separated by semicolons
        auto parts = splitString(str, ';');
        if (parts.size() != 2)
        {
            throw RuntimeError("invalid input)");
        }
        else{
            Position2D output;
            output.x     = convertFromString<double>(parts[0]);
            output.y     = convertFromString<double>(parts[1]);
            return output;
        }
    }
} // end namespace BT

Registered interface usage

static PortsList providedPorts()
{
    // Optionally, a port can have a human readable description
    const char*  description = "Simply print the goal on console...";
    return { InputPort<Position2D>("target", description) };
}

Use in xml files

<SequenceStar name="root">
    <CalculateGoal   goal="{GoalPosition}" />
    <PrintTarget     target="{GoalPosition}" />
    <SetBlackboard   output_key="OtherGoal" value="-1;3" />
    <PrintTarget     target="{OtherGoal}" />
</SequenceStar>

Sequential and asynchronous action nodes

The difference between SequenceNode and ReactiveSequence

Asynchronous actions have their own threads. This allows the user to use blocking functions, but to return the executed process to the tree.

Examples of robot movement:

//
// Created by jiang on 2021/9/18.
//

#include <behaviortree_cpp_v3/bt_factory.h>
#include <behaviortree_cpp_v3/action_node.h>
#include <thread>

using namespace BT;

inline void SleepMS(int ms)
{
    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}

struct Pose2D
{
    double x;
    double y;
    double theta;
};
namespace BT
{

template<>
inline Pose2D convertFromString(StringView str)
{
    auto parts = splitString(str, ';');
    if (parts.size() != 3) {
        throw RuntimeError("invalid input)");
    } else {
        Pose2D output{
                convertFromString<double>(parts[0]),
                convertFromString<double>(parts[1]),
                convertFromString<double>(parts[2]),
        };
        return output;
    }
}
}

class MoveBaseAction : public AsyncActionNode
{
public:
    MoveBaseAction(const std::string &name, const NodeConfiguration &config) :
            AsyncActionNode(name, config)
    {}

    static PortsList providedPorts()
    {
        return {InputPort<Pose2D>("goal")};
    }

protected:
    NodeStatus tick() override;
};

NodeStatus MoveBaseAction::tick()
{
    std::cout << "move base thread id: " << std::this_thread::get_id();
    Pose2D goal{};
    if (!getInput<Pose2D>("goal", goal)) {
        throw RuntimeError("missing required input [goal]");
    }
    printf("[MoveBase STATED]. goal: x=%.f y = %.1f theta=%.2f\n", goal.x, goal.y, goal.theta);
    int count = 0;
    while (!isHaltRequested() && count++ < 25) {
        SleepMS(10);
    }
    std::cout << "[MoveBase FINISHED]" << std::endl;
    return isHaltRequested() ? NodeStatus::FAILURE : NodeStatus::SUCCESS;
}

class SaySomething : public SyncActionNode
{
public:
    SaySomething(const std::string &name, const NodeConfiguration &config) :
            SyncActionNode(name, config)
    {}

    static PortsList providedPorts()
    {
        return {InputPort<std::string>("message")};
    }

protected:
    NodeStatus tick() override
    {
        Optional<std::string> msg = getInput<std::string>("message");
        if (!msg) {
            throw RuntimeError("missing required input [message]: ", msg.error());
        }
        std::cout << "Robot says: " << msg.value() << std::endl;
        return NodeStatus::SUCCESS;
    }
};

NodeStatus CheckBattery(TreeNode &node)
{
    std::cout << "[ Battery: OK ]" << std::endl;
    return NodeStatus::SUCCESS;
}

int main()
{
    BehaviorTreeFactory factory;
    factory.registerSimpleCondition("BatteryOK", CheckBattery);
    factory.registerNodeType<MoveBaseAction>("MoveBase");
    factory.registerNodeType<SaySomething>("SaySomething");

    auto tree = factory.createTreeFromFile("../../test_async/async.xml");

    NodeStatus status;

    std::cout << "main thread id: " << std::this_thread::get_id();

    std::cout << "\n--- 1st executeTick() --- " << std::endl;
    status = tree.tickRoot();
    std::cout << "result : " << status << std::endl;

    SleepMS(150);
    std::cout << "\n--- 2nd executeTick() ---" << std::endl;
    status = tree.tickRoot();
    std::cout << "result : " << status << std::endl;

    SleepMS(150);
    std::cout << "\n--- 3rd executeTick() ---" << std::endl;
    status = tree.tickRoot();
    std::cout << "result : " << status << std::endl;

    std::cout << std::endl;

    return 0;
}
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
    <BehaviorTree ID="BehaviorTree">
        <Sequence>
            <Action ID="BatteryOK"/>
            <Action ID="SaySomething" message="mission started..."/>
            <Action ID="MoveBase" goal="1.0;2.0;3.0"/>
            <Action ID="SaySomething" message="mission completed!"/>
        </Sequence>
    </BehaviorTree>

    <TreeNodesModel>
        <Action ID="SaySomething">
            <input_port name="message"/>
        </Action>
        <Action ID="MoveBase">
            <input_port name="goal"/>
        </Action>
    </TreeNodesModel>
</root>

MoveBaseAction::tick() is executed in the thread, while MoveBaseAction::executeTick() is executed in the main thread

It is responsible to implement the halt() function to stop the task

The first and second returns RUNNING, and the third returns SUCCESS

RUNNING stops the execution of subsequent tasks for the sequence

Subtree and log

Subtree means that a hierarchical behavior tree can be created, and many small reusable behavior trees can be used to form a large behavior tree

example

<root main_tree_to_execute = "MainTree">

    <BehaviorTree ID="DoorClosed">
        <Sequence name="door_closed_sequence">
            <Inverter>
                <IsDoorOpen/>
            </Inverter>
            <RetryUntilSuccesful num_attempts="4">
                <OpenDoor/>
            </RetryUntilSuccesful>
            <PassThroughDoor/>
        </Sequence>
    </BehaviorTree>

    <BehaviorTree ID="MainTree">
        <Fallback name="root_Fallback">
            <Sequence name="door_open_sequence">
                <IsDoorOpen/>
                <PassThroughDoor/>
            </Sequence>
            <SubTree ID="DoorClosed"/>
            <PassThroughWindow/>
        </Fallback>
    </BehaviorTree>

</root>

Use < subtree id = "doorclosed" / > to call the subtree

Logging is a mechanism used to display and record changes in publishing status

int main()
{
    using namespace BT;
    BehaviorTreeFactory factory;

    // register all the actions into the factory
    // We don't show how these actions are implemented, since most of the 
    // times they just print a message on screen and return SUCCESS.
    // See the code on Github for more details.
    factory.registerSimpleCondition("IsDoorOpen", std::bind(IsDoorOpen));
    factory.registerSimpleAction("PassThroughDoor", std::bind(PassThroughDoor));
    factory.registerSimpleAction("PassThroughWindow", std::bind(PassThroughWindow));
    factory.registerSimpleAction("OpenDoor", std::bind(OpenDoor));
    factory.registerSimpleAction("CloseDoor", std::bind(CloseDoor));
    factory.registerSimpleCondition("IsDoorLocked", std::bind(IsDoorLocked));
    factory.registerSimpleAction("UnlockDoor", std::bind(UnlockDoor));

    // Load from text or file...
    auto tree = factory.createTreeFromText(xml_text);

    // This logger prints state changes on console
    StdCoutLogger logger_cout(tree);

    // This logger saves state changes on file
    FileLogger logger_file(tree, "bt_trace.fbl");

    // This logger stores the execution time of each node
    MinitraceLogger logger_minitrace(tree, "bt_trace.json");

#ifdef ZMQ_FOUND
    // This logger publish status changes using ZeroMQ. Used by Groot
    PublisherZMQ publisher_zmq(tree);
#endif

    printTreeRecursively(tree.rootNode());

    //while (1)
    {
        NodeStatus status = NodeStatus::RUNNING;
        // Keep on ticking until you get either a SUCCESS or FAILURE state
        while( status == NodeStatus::RUNNING)
        {
            status = tree.tickRoot();
            CrossDoor::SleepMS(1);   // optional sleep to avoid "busy loops"
        }
        CrossDoor::SleepMS(2000);
    }
    return 0;
}

Groot uses the status information published by ZMQ

Interface remapping

In the CrossDoor example, we see that the subtree looks like a single leaf node from the perspective of its parent node (MainTree in the example). In addition, in order to avoid name conflicts in very large trees, any tree and subtree use different blackboard instances.

For this reason, we need to explicitly connect the port of a tree to the port of its subtree.

example

<root main_tree_to_execute = "MainTree">

    <BehaviorTree ID="MainTree">

        <Sequence name="main_sequence">
            <SetBlackboard output_key="move_goal" value="1;2;3" />
            <SubTree ID="MoveRobot" target="move_goal" output="move_result" />
            <SaySomething message="{move_result}"/>
        </Sequence>

    </BehaviorTree>

    <BehaviorTree ID="MoveRobot">
        <Fallback name="move_robot_main">
            <SequenceStar>
                <MoveBase       goal="{target}"/>
                <SetBlackboard output_key="output" value="mission accomplished" />
            </SequenceStar>
            <ForceFailure>
                <SetBlackboard output_key="output" value="mission failed" />
            </ForceFailure>
        </Fallback>
    </BehaviorTree>

</root>
  • MoveRobot is a subtree contained in MainTree
  • Want to connect the port of MoveRobot with the port of MainTree
  • Use XML tags, where the words internal/external refer to the child tree and its parent tree, respectively

<SubTree ID="MoveRobot" target="move_goal" output="move_result" />

move_goal passes in the target of the subtree and outputs the result output of the subtree to move_result

Packaging legacy code

use λ function

class LegacyClass {
public:
    bool method() {
        return true;
    }
}

LegacyClass legacy;
auto wrapFunc = [&legacy] (NodeTree &node) -> NodeStatus {
    ...
    return ...
}

PortsList ports = { ... };
factory.registerSimpleAction("...", wrapFunc, ports);

...

Additional parameters

Method 1: use NodeBuilder

Suppose the node's constructor has more parameters

Action_A(const std::string& name, const NodeConfiguration& config,
             int arg1, double arg2, std::string arg3 ):
        SyncActionNode(name, config),
        _arg1(arg1),
        _arg2(arg2),
        _arg3(arg3) {}

A NodeBuilder function is created

NodeBuilder builder_A =
   [](const std::string& name, const NodeConfiguration& config)
{
    return std::make_unique<Action_A>( name, config, 42, 3.14, "hello world" );
};

Then pass in the second parameter of the registration function

factory.registerBuilder<Action_A>( "Action_A", builder_A);

Method 2: use initialization method

void init( int arg1, double arg2, const std::string& arg3 )
{
    _arg1 = (arg1);
    _arg2 = (arg2);
    _arg3 = (arg3);
}

Calling functions by type using C++ RTTI

for( auto& node: tree.nodes )
{
    if( auto action_B = dynamic_cast<Action_B*>( node.get() ))
    {
        action_B->init( 42, 3.14, "hello world");
    }
}

Synergetic process

The behavior tree provides two convenient abstractions for creating asynchronous actions. For the following actions:

  • It takes a long time to finish
  • RUNNING may be returned
  • Can be stopped

The first type is AsyncActionNode, which executes the tick() function in an isolated thread

In this tutorial, the CoroActionNode is introduced to complete actions with similar results using a coroutine

Coroutines do not generate new threads, are more efficient, and do not need to worry about thread safety

In the collaboration process, when the user wants to cancel the executed action, the yield method needs to be explicitly called

CoroActionNode encapsulates the yield function into setstatus runningyield()

example

typedef std::chrono::milliseconds Milliseconds;

class MyAsyncAction: public CoroActionNode
{
  public:
    MyAsyncAction(const std::string& name):
        CoroActionNode(name, {})
    {}

  private:
    // This is the ideal skeleton/template of an async action:
    //  - A request to a remote service provider.
    //  - A loop where we check if the reply has been received.
    //  - You may call setStatusRunningAndYield() to "pause".
    //  - Code to execute after the reply.
    //  - A simple way to handle halt().
    NodeStatus tick() override
    {
        std::cout << name() <<": Started. Send Request to server." << std::endl;

        TimePoint initial_time = Now();
        TimePoint time_before_reply = initial_time + Milliseconds(100);

        int count = 0;
        bool reply_received = false;

        while( !reply_received )
        {
            if( count++ == 0)
            {
                // call this only once
                std::cout << name() <<": Waiting Reply..." << std::endl;
            }
            // pretend that we received a reply
            if( Now() >= time_before_reply )
            {
                reply_received = true;
            }

            if( !reply_received )
            {
                // set status to RUNNING and "pause/sleep"
                // If halt() is called, we will NOT resume execution
                setStatusRunningAndYield();
            }
        }

        // This part of the code is never reached if halt() is invoked,
        // only if reply_received == true;
        std::cout << name() <<": Done. 'Waiting Reply' loop repeated "
                  << count << " times" << std::endl;
        cleanup(false);
        return NodeStatus::SUCCESS;
    }

    // you might want to cleanup differently if it was halted or successful
    void cleanup(bool halted)
    {
        if( halted )
        {
            std::cout << name() <<": cleaning up after an halt()\n" << std::endl;
        }
        else{
            std::cout << name() <<": cleaning up after SUCCESS\n" << std::endl;
        }
    }

    void halt() override
    {
        std::cout << name() <<": Halted." << std::endl;
        cleanup(true);
        // Do not forget to call this at the end.
        CoroActionNode::halt();
    }

    Timepoint Now()
    { 
        return std::chrono::high_resolution_clock::now(); 
    };
};
 <root >
     <BehaviorTree>
        <Timeout msec="150">
            <SequenceStar name="sequence">
                <MyAsyncAction name="action_A"/>
                <MyAsyncAction name="action_B"/>
            </SequenceStar>
        </Timeout>
     </BehaviorTree>
 </root>

Keywords: C++

Added by jasonbullard on Sun, 02 Jan 2022 18:37:48 +0200