BehaviorTreeをつかう その４：rclと連動する

実際のところ

スクリプト

#include <behaviortree_cpp_v3/behavior_tree.h>
#include <behaviortree_cpp_v3/bt_factory.h>
#include <rclcpp/rclcpp.hpp>
#include <xarm_msgs/srv/set_int16.hpp>
#include <xarm_msgs/srv/set_int16_by_id.hpp>
#include <xarm_msgs/srv/move_cartesian.hpp>
#include <iostream>
#include <string>

using namespace BT;
using SetInt16 = xarm_msgs::srv::SetInt16;
using SetInt16ById = xarm_msgs::srv::SetInt16ById;

class InitializeNode : public BT::SyncActionNode
{
public:
    InitializeNode(const std::string& name, const NodeConfiguration& config)
        : BT::SyncActionNode(name, config),
          node_(std::make_shared<rclcpp::Node>("initialize_node")),
          motion_enable_client_(node_->create_client<SetInt16ById>("/xarm/motion_enable")),
          set_mode_client_(node_->create_client<SetInt16>("/xarm/set_mode")),
          set_state_client_(node_->create_client<SetInt16>("/xarm/set_state")) {}

    NodeStatus tick() override
    {
        auto motion_enable_request = std::make_shared<SetInt16ById::Request>();
        motion_enable_request->id = 8;
        motion_enable_request->data = 1;
        auto future_motion_enable = motion_enable_client_->async_send_request(motion_enable_request);

        auto set_mode_request = std::make_shared<SetInt16::Request>();
        set_mode_request->data = 0;
        auto future_set_mode = set_mode_client_->async_send_request(set_mode_request);

        auto set_state_request = std::make_shared<SetInt16::Request>();
        set_state_request->data = 0;
        auto future_set_state = set_state_client_->async_send_request(set_state_request);

        // サービスの応答を待つ
        if (rclcpp::spin_until_future_complete(node_, future_motion_enable) != rclcpp::FutureReturnCode::SUCCESS ||
            rclcpp::spin_until_future_complete(node_, future_set_mode) != rclcpp::FutureReturnCode::SUCCESS ||
            rclcpp::spin_until_future_complete(node_, future_set_state) != rclcpp::FutureReturnCode::SUCCESS)
        {
            RCLCPP_ERROR(node_->get_logger(), "Failed to call service");
            return NodeStatus::FAILURE;
        }

        return NodeStatus::SUCCESS;
    }

private:
    std::shared_ptr<rclcpp::Node> node_;
    rclcpp::Client<SetInt16ById>::SharedPtr motion_enable_client_;
    rclcpp::Client<SetInt16>::SharedPtr set_mode_client_;
    rclcpp::Client<SetInt16>::SharedPtr set_state_client_;
};

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

    NodeStatus tick() override
    {
        // ry
        return NodeStatus::SUCCESS;
    }
};

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    auto ros_node = rclcpp::Node::make_shared("behavior_tree_node");

    BehaviorTreeFactory factory;

    // InitializeNode と RobotMoveNode の登録
    factory.registerNodeType<InitializeNode>("InitializeNode");
    factory.registerNodeType<RobotMoveNode>("RobotMoveNode");

    // その他のノードの登録

    // BehaviorTree の作成
    auto tree = factory.createTreeFromFile("/path/to/your/tree/file.xml");

    // BehaviorTree の実行
    while (rclcpp::ok()) {
        tree.tickRoot();
        rclcpp::spin_some(ros_node);
    }

    rclcpp::shutdown();
    return 0;
}

ツリーファイル

<root main_tree_to_execute = "MainTree" BTCPP_format="4">
    <BehaviorTree ID="MainTree">
        <Sequence name="root_sequence">
            <SubTree ID="xArm" />
            <!-- 他のノード -->
        </Sequence>
    </BehaviorTree>

    <BehaviorTree ID="xArm">
        <Sequence name="xarm_sequence">
            <InitializeNode   name="initialize_node"/>
            <RobotMoveNode    name="robot_move_node"/>
        </Sequence>
    </BehaviorTree>
</root>