BehaviorTreeをつかう その３：FallbackとSequence

実際のところ

Fallback => 一個でも成功すればSuccess。色々試してどれかがOKならいいというケース。
Sequence => 一個でも失敗すればFailure。順序事に実行する事が大切なケース。

script

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


using namespace BT;

// Simple function that return a NodeStatus
BT::NodeStatus CheckBattery()
{
  std::cout << "[ Battery: OK ]" << std::endl;
  return BT::NodeStatus::SUCCESS;
}

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

// We want to wrap into an ActionNode the methods open() and close()
class GripperInterface
{
public:
  GripperInterface(): _open(true) {}
    
  NodeStatus open() 
  {
    _open = true;
    std::cout << "GripperInterface::open" << std::endl;
    return NodeStatus::SUCCESS;
  }

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

  void registerNodes(BT::BehaviorTreeFactory& factory)
  {
    factory.registerSimpleAction("OpenGripper", [&](TreeNode&){ return open(); } );
    factory.registerSimpleAction("CloseGripper", [&](TreeNode&){ return close(); } );
  }

private:
  bool _open; // shared information
};


int main()
{
    BehaviorTreeFactory factory;

    // CheckBattery 条件の登録
    factory.registerSimpleCondition("CheckBattery", [&](TreeNode&) { return CheckBattery(); });
    factory.registerSimpleCondition("SampleCondition", [&](TreeNode&) { return SampleCondition(); });

    // GripperInterface ノードの登録
    GripperInterface gripper;
    gripper.registerNodes(factory);

    //indicate the path to the file containing the tree
    std::string xml_file = "/home/tsukarm/dev_ws/src/your_behavior_tree_package/config/my_tree.xml";
    auto tree = factory.createTreeFromFile( "/path/to/tree.xml");
    
    printTreeRecursively(tree.rootNode());
    tree.tickRootWhileRunning();

    return 0;
}

tree

<root main_tree_to_execute = "MainTree" BTCPP_format="4">
    <BehaviorTree ID="MainTree">
        <Sequence name="root_sequence">
            <Fallback name="root_fallback">
                <SubTree ID="Gripper" />
                <Sequence name="battery_sequence">
                    <CheckBattery   name="check_battery"/>
                    <Sequence name="sample_sequence"> 
                        <SampleCondition name="sample_condition" />
                    </Sequence>
                    <SampleCondition name="sample_condition" />
                </Sequence>
            </Fallback>
        </Sequence>
    </BehaviorTree>

    <BehaviorTree ID="Gripper">
        <Sequence name="gripper_sequence">
            <CloseGripper   name="close_gripper"/>
            <OpenGripper    name="open_gripper"/>
            <CloseGripper   name="close_gripper"/>
        </Sequence>
    </BehaviorTree>
</root>

実行結果

$ ros2 run your_behavior_tree_package simple_action_node
----------------
root_sequence
   root_fallback
      Gripper
         gripper_sequence
            close_gripper
            open_gripper
            close_gripper
      battery_sequence
         check_battery
         sample_sequence
            sample_condition
         sample_condition
----------------
GripperInterface::close
GripperInterface::open
GripperInterface::close