BehaviorTreeをつかう その２：インプットとアウトプット

実際のところ

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

using namespace BT;

// Example of custom SyncActionNode (synchronous action)
// without ports.
class ApproachObject : public BT::SyncActionNode
{
public:
  ApproachObject(const std::string& name) :
      BT::SyncActionNode(name, {})
  {}

  // You must override the virtual function tick()
  BT::NodeStatus tick() override
  {
    std::cout << "ApproachObject: " << this->name() << std::endl;
    return BT::NodeStatus::SUCCESS;
  }
};

// Simple function that return a NodeStatus
BT::NodeStatus CheckBattery()
{
  std::cout << "[ Battery: 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;
  }

private:
  bool _open; // shared information
};



// SyncActionNode (synchronous action) with an input port.
class SaySomething : public SyncActionNode
{
public:
  // If your Node has ports, you must use this constructor signature 
  SaySomething(const std::string& name, const NodeConfiguration& config)
    : SyncActionNode(name, config)
  { }

  // It is mandatory to define this STATIC method.
  static PortsList providedPorts()
  {
    // This action has a single input port called "message"
    return { InputPort<std::string>("message") };
  }

  // Override the virtual function tick()
  NodeStatus tick() override
  {
    auto msg = getInput<std::string>("message");
    // use the method value() to extract the valid message.
    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") };
  }

  // This Action writes a value into the port "text"
  NodeStatus tick() override
  {        
    // the output may change at each tick(). Here we keep it simple.
    setOutput("text", "The answer is 42" );
    return NodeStatus::SUCCESS;
  }
};

int main()
{
    BehaviorTreeFactory factory;

    // The recommended way to create a Node is through inheritance.
    factory.registerNodeType<ApproachObject>("ApproachObject");

    factory.registerSimpleCondition("CheckBattery", [&](TreeNode&) { return CheckBattery(); });


    //You can also create SimpleActionNodes using methods of a class
    GripperInterface gripper;
    factory.registerSimpleAction("OpenGripper", [&](TreeNode&){ return gripper.open(); } );
    factory.registerSimpleAction("CloseGripper", [&](TreeNode&){ return gripper.close(); } );

    // InOut example
    factory.registerNodeType<SaySomething>("SaySomething");
    factory.registerNodeType<ThinkWhatToSay>("ThinkWhatToSay");

    //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(xml_file);
    auto tree = factory.createTreeFromFile( "/home/tsukarm/dev_ws/src/your_behavior_tree_package/config/my_tree.xml");
    
    tree.tickRootWhileRunning();

    return 0;
}

<root main_tree_to_execute = "MainTree" BTCPP_format="4">
    <BehaviorTree ID="MainTree">
        <Sequence name="root_sequence">
            <ApproachObject name="approach_object"/>
            <CheckBattery   name="check_battery"/>

            <CloseGripper   name="close_gripper"/>
            <OpenGripper    name="open_gripper"/>

            <SaySomething     message="hello" />
            <ThinkWhatToSay   text="{the_answer}"/>
            <SaySomething     message="{the_answer}" />
            <!-- 追加はここに -->
        </Sequence>
    </BehaviorTree>
</root>

実行

$ ros2 run your_behavior_tree_package simple_action_node 
ApproachObject: approach_object
[ Battery: OK ]
GripperInterface::close
GripperInterface::open
Robot says: hello
Robot says: The answer is 42