ROS2でBehaviorTreeを使う

　今回はゲーム開発などでよく使われる制御ツールであるBehaviorTreeとROSを組み合わせる方法について説明します。BehaviorTreeはNavigation2などのROSパッケージでも利用されており，ゲームのみならずロボット制御でも利用されています。
　BehaviorTreeを用いるメリットは，複雑な制御を視覚的にも直感的にも分かりやすく設計できるようになるという点にあります。またROSと同様にプログラムがノードによって分離しているためデバッグや再利用がしやすいのも利点と言えるでしょう。

環境は以下の通りです。
|項目|値|
|---|---|
|Ubuntu|22.04|
|ROS|Humble|
|BehaviorTree|4.3|

# BehaviorTreeを理解する
　BehanviorTreeはSmachなどのステートマシンとは異なり，探索木のような階層構造を持つTreeによってタスクの実行順序を制御します。Tickという信号がTreeを構成するNode同士を繋ぎ，Tickを受け取ったNodeは以下の３状態からいずれかを返します。
 - SUCCESS：タスクが成功
 - FAILURE：タスクが失敗
 - RUNNING：タスクを実行中

　基本的な挙動は深さ優先探索に似ており，あるタスクが何らかの状態を返すと次のタスクへ移行します。また，後ほど詳しく説明しますがNodeは以下のようないくつかの基本的な型により分類されます。
 - ControlNode：複数の子ノードをその状態に応じて制御します。
 - DecoratorNode：一つの子ノードに対して，結果の変更や繰り返しの実行をします。
 - ConditionNode：子ノードを持たず，簡単な処理を実行します。
 - ActionNode：子ノードを持たず，このノード自体がなにかの命令を実行します。

　BehaviorTreeにおいてユーザーが実装しなければならない要素は大まかに言って，Tree自体の構造とActionNodeの２つです。Treeの構造はxml形式で表現されますが，これはGrootというソフトからグラフィカルに生成することもできます。ActionNodeは主に外部のプログラムとの通信の役割を担っており，今回はROSと組み合わせることになります。ノードがTickの信号を受け取るとTick()関数が実行されるので，ここに実行したいプログラムを実装します。
　また，データをやり取りする仕組みとしてPortsというものがあります。Portsは２つのノード間におけるデータのやり取りに利用されます。

# インストール
### BehaviorTree4.3
以下のコマンドを順に実行していきます。
```
sudo apt install libzmq3-dev libboost-dev qtbase5-dev libqt5svg5-dev libzmq3-dev libdw-dev
git clone https://github.com/BehaviorTree/BehaviorTree.CPP.git
cd BehaviorTree.CPP
mkdir build
cd build
cmake ..
make -j8
sudo make install
```
次に生成したライブラリをROS2から読み込めるようにします。
```
sudo nano ~/.bashrc
export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH // 末尾に書き込む
```


### Groot2
https://www.behaviortree.dev/groot
上記ページの下の方から`Linux installer`をダウンロードし，ダウンロードしたファイルの場所で以下のコマンドを実行します。ダウンロードしたバージョンに応じてコマンドを修正してください。
```
sudo chmod +x Groot2-0.9.0-linux-installer.run
./Groot2-0.9.0-linux-installer.run
```
あとはインストーラーの指示に従いインストールを行ってください。
デフォルトの場所にインストールした場合，`~/Groot2/bin/groot2`が実行ファイルなので、これを簡単に起動できるようにします。
以下のコマンドを実行します。
```
nano ~/.bash_aliases
```
以下の内容を書き込みます。
```
alias groot2='~/Groot2/bin/groot2'
```
`ctrl`+`S`で保存，`ctrl`+`X`で閉じます。
```
source ~/.bash_aliases
```
これでターミナルに`groot2`と打ち込めばGroot2が起動するようになります。



# デフォルトノードについて
BehaviorTreeにはいくつかのノードが既に実装されており，それらを組み合わせて全体の動作を作成していきます。使いやすいノードとその説明を一覧にしたので参考にしてください。

また，分からないことがあれば下の各ノードの実装を参照するのが良いでしょう。
https://github.com/BehaviorTree/BehaviorTree.CPP/tree/master/include/behaviortree_cpp

|Decorator|必ず子ノードを一つ持つ|
|---|---|
|Dealy|Input Portで指定された時間後に子ノードにTickを送る。子ノードの状態を返す。|
|ForceFailure|子ノードがRUNNINGを返す場合はRUNNINGを返す。それ以外は常にFAILUREを返す|
|ForceSuccess|子ノードがRUNNINGを返す場合はRUNNINGを返す。それ以外は常にSUCCESSを返す|
|Inverter|子ノードに一度だけTickを送り，失敗したらSUCCESSを，成功したらFAILUREを返す|
|KeepRunningUntilFailure|失敗するまでTickし続ける|
|Precondition|BlackBordの値に応じて条件分岐を行う|
|Repeat|子ノードにN回Tickを送り，全てSUCCESSならSUCCESSを返す。FAILUREならループを中断しFAILUREを返す。|
|RetryUntilSuccessful|子ノードにN回Tickを送り，全てFAILUREならFAILUREを返す。SUCCESSならループを中断しSUCCESSを返す。|
|RunOnce|子ノードを一度だけ実行し(非同期プロセスならSUCCESSかFAILUREを返すまでTickし)，子ノードを将来的にスキップする|
|Timeout|特定時間後に子ノードが実行中であれば停止させる|

|Control|複数の子ノードを制御する|
|---|---|
|Fallback|途中で子ノードがSUCCESSを返したら，停止してSUCCESSを返す。全ての子ノードがFAILUREを返したらFAILUREを返す。二回目はSUCCESSを返したところから|
|IfThenElse|このノードは正確に2つか3つの子ノードを持たなければならない。最初の子ノードの状態によって条件分岐し，もしSUCCESSを返せば2番目の子ノードが実行される。FAILUREを返した場合は、3番目の子ノードが実行されます。子ノードが2つしかない場合、ステートメントがFAILUREを返すたびに、このノードはFAILUREを返します。|
|Parallel|ParallelNodeはすべての子ノードを同時に実行しますが、別々のスレッドでは実行しません。ReactiveSequenceと似ているように見えるかもしれませんが、このControl Nodeは複数の子ノードを同時に実行できる唯一のノードです。失敗した子ノードの数がfailure_cocuntを超えるとノードは失敗を返す。success_countを超えると成功を返す。-1は子ノードの数を意味する。|
|ParallelAll|ParallelAllNodeはすべての子ノードを同時に実行しますが、別々のスレッドでは実行しません。ParallelNodeとは異なり、このノードは常にすべての子ノードの実行を完了します。失敗した子ノードの数がmax_failuresを超えると失敗を返す。|
|Sequence|途中で子ノードがFAILUREを返したら，停止してFAILUREを返す。全ての子ノードがSUCCESSを返したらSUCCESSを返す。二回目は最初から|
|SequenceWithMemory|途中で子ノードがFAILUREを返したら，停止してFAILUREを返す。全ての子ノードがSUCCESSを返したらSUCCESSを返す。二回目はFAILUREを返したところから|
|WhileDoElse|IfThenElseNodeのREACTIVEノード|

|Action|子ノードを持たず、このノード自体がなにかの命令を実行する|
|---|---|
|AlwaysFailure|常にFAILUREを返す|
|AlwaiySuccess|常にSUCCESSを返す|
|SetBlackBoard|BlackBoardに値を設定する|

# Actionノードについて
　オリジナルのActionノードは，ライブラリで提供されているノードのテンプレートを継承して作成します。テンプレートの詳細についてはライブラリの実装部分を参照するのが良いでしょう。
https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/include/behaviortree_cpp/action_node.h

個人的には，すぐに終わる処理は`SyncActionNode`，時間のかかる処理は`StatefulActionNode`を継承して作成するのが良いと思います。

### サンプル
前提
```cpp
#include "behaviortree_cpp/behavior_tree.h"
#include "behaviortree_cpp/bt_factory.h"
using namespace BT;
```
 - ポート無しSyncActionNode
他のノードと値のやり取りがなく，処理がすぐに終わる場合。
```cpp
class SampleNode : public SyncActionNode {
public:
    SampleNode(const std::string& name) : SyncActionNode(name, {}){ }

    // ノードが呼び出されると一度だけ実行される
    NodeStatus tick() override {
        std::cout << "call SampleNode" << std::endl;
        
        // 簡単な処理を記述
        
        return NodeStatus::SUCCESS;
    }
};
```
 - ポート有りSyncActionNode
他のノードと値のやり取りがあり，処理がすぐに終わる場合。
```cpp
class SampleNode : public SyncActionNode {
public:
    SampleNode(const std::string& name, const NodeConfig& config) : SyncActionNode(name, config){ }

    // Portの情報を記述する
    static PortsList providedPorts() {
        return { OutputPort<int>("sample_output"),
                 InputPort<std::string>("sample_input") };
    }
    
    // ノードが呼び出されると一度だけ実行される
    NodeStatus tick() override {
        std::cout << "call SampleNode" << std::endl;
        
        // InputPortの値を受け取る
        Expected<std::string> msg = getInput<std::string>("sample_input");
        if (!msg) { // Inputの値が適切でないときの処理
            throw BT::RuntimeError("missing required input [sample_input]: ", msg.error() );
        }
        std::string sample_input = msg.value();
        
        //簡単な処理を記述
        
        // OutputPortに値をセット
        setOutput("sample_output", 123);
        
        return NodeStatus::SUCCESS;
    }
};
```
 - ポート無しStatefulActionNode
他のノードと値のやり取りがなく，処理に時間がかかる場合。
```cpp
class SampleNode : public StatefulActionNode {
public:
    SampleNode(const std::string& name) : StatefulActionNode(name, {}){ }

    // ノードが呼び出されると一度だけ実行される
    NodeStatus onStart() override {
        std::cout << "call SampleNode" << std::endl;
        
        // 簡単な処理を記述
        
        // RUNNING状態を返すと次のTickでonRunning()が呼び出される。
        return NodeStatus::RUNNING;
    }

    // Running状態のときに実行される
    NodeStatus onRunning() override {
    
        // 時間のかかる処理などを記述
        
        return NodeStatus::SUCCESS;
    }

    void onHalted() override {
        std::cout << "interrupt SampleNode" << std::endl;
    }
};
```
 - ポート有りStatefulActionNode
他のノードと値のやり取りがあり，処理に時間がかかる場合。
```cpp
class SampleNode : public StatefulActionNode {
public:
    SampleNode(const std::string& name, const NodeConfig& config) : StatefulActionNode(name, config){ }

    // Portの情報を記述する
    static PortsList providedPorts() {
        return { OutputPort<std::string>("sample_output"),
                 InputPort<int>("sample_input") };
    }

    // ノードが呼び出されると一度だけ実行される
    NodeStatus onStart() override {
        std::cout << "call SampleNode" << std::endl;
        
        // InputPortの値を受け取る
        Expected<int> msg = getInput<int>("sample_input");
        if (!msg) { // Inputの値が適切でないときの処理
            throw BT::RuntimeError("missing required input [sample_input]: ", msg.error() );
        }
        int sample_input = msg.value();
        
        //簡単な処理を記述
        
        return NodeStatus::RUNNING;
    }
    
    // Running状態のときに実行される
    NodeStatus onRunning() override {
    
        // 時間のかかる処理などを記述
        
        // OutputPortに値をセット
        setOutput("sample_output", "hello world!");
        
        return NodeStatus::SUCCESS;
    }

    void onHalted() override {
        std::cout << "interrupt SampleNode" << std::endl;
    }
};
```

# チュートリアル
簡単なサンプルプログラムを作成していきます。全体のコードは[こちら](https://github.com/Azuma413/bt_sample)を参照してください。

サンプルプログラムには
1. ROSのサービス通信で文字列を要求する
2. 受け取った文字列の字数をカウントする
3. カウントを出力する

という三つの機能を持たせることにします。

### ROS2プロジェクトの作成
最初にROSのプロジェクトを作成します。
ターミナルで以下のコマンドを順に実行します。
```
cd ~/ros2_ws/src
ros2 pkg create bt_sample --build-type ament_cmake --dependencies rclcpp std_msgs rosidl_default_generators behaviortree_cpp
cd bt_sample
mkdir config srv
```
`config`にBehaviorTreeの構造を記述したxmlファイルを，`srv`にサービス通信用のカスタムメッセージを格納します。

### カスタムメッセージの作成
`srv`ディレクトリ下に`CallText.srv`を作成し，以下の内容を書き込みます。
```
---
string text
```
`package.xml`の16行目あたりに次の二行を追加します。
```xml
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
```
`CMakeLists.txt`を以下のように書き換えます。
```cmake
cmake_minimum_required(VERSION 3.8)
project(bt_sample)

find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/CallText.srv"
  DEPENDENCIES
)

install(DIRECTORY
  config
  DESTINATION share/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_auto_package()
```
一度ビルドし，カスタムメッセージを生成します。
```
cd ~/ros2_ws
colcon build
```


### サービスサーバーの実装
サービスが呼び出されたら，コンソールからテキスト入力を受け付けるようにします。
`src`ディレクトリ下に`server.cpp`を作成し，以下の内容を書き込みます。
```cpp
#include "rclcpp/rclcpp.hpp"
#include "bt_sample/srv/call_text.hpp"

using namespace std::chrono_literals;

class TextServer : public rclcpp::Node{
    public:
    TextServer() : Node("text_server"){
        auto service_callback = [this](const std::shared_ptr<bt_sample::srv::CallText::Request> request, std::shared_ptr<bt_sample::srv::CallText::Response> response) -> void{
            std::cout << "input text : ";
            std::cin >> input_text;
            response->text = input_text;
        };
        srv = create_service<bt_sample::srv::CallText>("call_text", service_callback);
    }
    private:
    std::string input_text;
    rclcpp::Service<bt_sample::srv::CallText>::SharedPtr srv;
};

int main(int argc, char **argv){
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TextServer>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
```
`CMakeLists.txt`を変更します。
```
cmake_minimum_required(VERSION 3.8)
project(bt_sample)

find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

// 追加
ament_auto_add_executable(
  server src/server.cpp
)

rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/CallText.srv"
  DEPENDENCIES
)

rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} rosidl_typesupport_cpp) // 追加
target_link_libraries(server "${cpp_typesupport_target}") // 追加

install(DIRECTORY
  config
  DESTINATION share/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_auto_package()
```

### BehaviorTree側のROSの実装
BehaviorTreeで利用するROSノードを実装します。
`include`ディレクトリ下に`ros_node.hpp`を作成して，以下のコードを書き込みます。
```cpp
#pragma once
#include "rclcpp/rclcpp.hpp"
#include "bt_sample/srv/call_text.hpp"

using namespace std::chrono_literals;

class BTNode : public rclcpp::Node{
    public:
    BTNode() : Node("bt_node"){
        std::cout << "bt_node is called" << std::endl;
        // サービスクライアントを作成
        call_text_cli = create_client<bt_sample::srv::CallText>("call_text");
        // サーバーが起動するまで待機
        while(!call_text_cli->wait_for_service(1s)){
            if(!rclcpp::ok()){
                break;
            }
            std::cout << "call_text service not available" << std::endl;
        }
        std::cout << "call_text service available" << std::endl;
    }

    // サーバーにリクエストを送り，返ってきた値を戻り値とする関数
    std::string get_text(){
        auto request = std::make_shared<bt_sample::srv::CallText::Request>();
        auto future_result = call_text_cli->async_send_request(request);
        if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS){
            return future_result.get()->text;
        }
        std::cout << "can't get future_result" << std::endl;
        // 値を取得できなかった場合は空文字列を返す
        return "";
    }

    private:
    rclcpp::Client<bt_sample::srv::CallText>::SharedPtr call_text_cli;
};

// ノードのインスタンスを定義
std::shared_ptr<BTNode> ros_node;
```

### BehaviorTreeのxmlの作成
Groot2を利用して，GUIからBehaviorTreeのxmlファイルを作成していきます。
Groot2を起動したら`Create Empty File`を選択して新しくBehaviorTreeを作成します。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/240f9705-4502-9c5f-fa02-84bd65830e62.png)

ウィンドウ左側の`Untitled`となっている部分を右クリックし，`Rename`から名前を`MainBT`に変更してください。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/97d3a1fe-d7ff-1a68-6b17-ba3b4cd92c98.png)

次にTreeに組み込むカスタムアクションノードを作成します。
ウィンドウ左側の`Models`の横の`+`マークをクリックして，表示されたウィンドウにノードの情報を入力します。

`GetText`はROSのサービスサーバーからテキストを受け取り，そのテキストをOutputPortにセットするStatefulActionNodeです。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/8218a1e8-ab4d-656b-8545-d260b4f9fa85.png)

`Counter`はInputPortから受け取ったテキストの文字数をカウントして，OutputPortにセットするSyncActionNodeです。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/7149884d-2eec-fd23-65db-2ff8b3cb385c.png)

`Display`はInputPortから受け取った数字をターミナル上に表示するSyncActionNodeです。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/00aebc1e-3e09-47ea-ddb9-3d72b9d8d5e4.png)

作成したActionノードは`Models`下の`Action`タブ内にあります。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/74f5d5da-556f-6b93-cd74-32afd5511d79.png)

次にデフォルトのノードとカスタムアクションノードを使ってTree構造を作ります。
今回は下の画像のように並べます。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/467f1ecc-3014-c0c2-a996-47c3766eb3ff.png)

　このTreeの流れを大まかに説明すると，処理はまず`Root`から始まり，ControlNodeの一つである`Sequence`を通り，中央の`Display`が実行されます。このとき`in_num`ポートに`123456`という数字がセットされているので，この数字がそのまま画面に出力されます。
　`Display`がSUCCESSを返すと，次にDecoratorNodeの一つである`KeepRunningUntilFailure`を経由してから`Sequence`を通り`GetText`が実行されます。ここでROSから受け取ったテキストは`out_text`ポートから`text_data`というキーに登録されます。
 　次に`Counter`が実行されると`in_text`ポートから`text_data`キーにアクセスして，先ほど登録されたテキストを受け取ります。また`out_num`ポートから`num_data`というキーに計算した数値を登録します。
　次に`Display`が実行されると，`in_num`ポートから`num_data`キーにアクセスして，先ほど登録された数値を受け取り，画面に出力します。
　ここまでの間でノードがFAILUREを返していない場合は`KeepRunningUntilFailure`によって再び`GetText`が呼び出され，同様の処理が繰り返されます。

ここまでできたらTreeのxmlを出力します。
ウィンドウ左側の`untitled_1`を右クリックして，`Save as...`から`main_bt.xml`と名前を付けて`~/ros2_ws/src/bt_sample/config/`に保存します。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/8b4dbf2a-02b9-b91a-f8f1-a5c4b0d1c379.png)

出力できたらGroot2はもう閉じても構いません。

# ActionNodeの中身の実装
先ほどGroot2で作成した3つのカスタムアクションノード，`Counter`，`Display`，`GetText`の中身を実装していきます。
`include`ディレクトリ下に`action_node.hpp`を作成して以下の内容を書き込みます。
```cpp
#pragma once
#include <rclcpp/allocator/allocator_common.hpp>
#include "behaviortree_cpp/behavior_tree.h"
#include "behaviortree_cpp/bt_factory.h"
#include "ros_node.hpp"

using namespace BT;

namespace MyActionNodes{
    class Counter : public SyncActionNode {
    public:
        Counter(const std::string& name, const NodeConfig& config) : SyncActionNode(name, config){ }

        // Portの情報を記述する
        static PortsList providedPorts() {
            return { OutputPort<int>("out_num"),
                     InputPort<std::string>("in_text") };
        }
        
        // ノードが呼び出されると一度だけ実行される
        NodeStatus tick() override {
            std::cout << "call Counter" << std::endl;
            
            // InputPortの値を受け取る
            Expected<std::string> msg = getInput<std::string>("in_text");
            if (!msg) { // Inputの値が適切でないときの処理
                throw BT::RuntimeError("missing required input [in_text]: ", msg.error() );
            }
            std::string in_text = msg.value();

            // 文字数をカウントする
            int out_num = in_text.length();
            
            // OutputPortに値をセット
            setOutput("out_num", out_num);
            
            return NodeStatus::SUCCESS;
        }
    };

    class Display : public SyncActionNode {
    public:
        Display(const std::string& name, const NodeConfig& config) : SyncActionNode(name, config){ }

        // Portの情報を記述する
        static PortsList providedPorts() {
            return { InputPort<int>("in_num") };
        }
        
        // ノードが呼び出されると一度だけ実行される
        NodeStatus tick() override {
            std::cout << "call Display" << std::endl;
            
            // InputPortの値を受け取る
            Expected<int> msg = getInput<int>("in_num");
            if (!msg) { // Inputの値が適切でないときの処理
                throw BT::RuntimeError("missing required input [in_num]: ", msg.error() );
            }
            int in_num = msg.value();

            // 文字数を表示する
            std::cout << "入力された文字列の文字数は " << in_num << " です" << std::endl;
            
            return NodeStatus::SUCCESS;
        }
    };

    class GetText : public StatefulActionNode {
    public:
        GetText(const std::string& name, const NodeConfig& config) : StatefulActionNode(name, config){ }

        // Portの情報を記述する
        static PortsList providedPorts() {
            return { OutputPort<std::string>("out_text") };
        }

        // ノードが呼び出されると一度だけ実行される
        NodeStatus onStart() override {
            std::cout << "call GetText" << std::endl;
            return NodeStatus::RUNNING;
        }
        
        // Running状態のときに実行される
        NodeStatus onRunning() override {

            // ROSからテキストを受け取る
            //ros_nodeはros_node.hppで定義したROSノードのインスタンス
            std::string out_text = ros_node->get_text();
            
            // OutputPortに値をセット
            setOutput("out_text", out_text);
            
            return NodeStatus::SUCCESS;
        }

        void onHalted() override {
            std::cout << "interrupt GetText" << std::endl;
        }
    };
}
```

### メインプログラムの実装
先ほど作成した`ros_node.hpp`，`action_node.hpp`，`main_bt.xml`を読み込んでROSノードとBehaviorTreeを実行するプログラムを作成します。
`src`ディレクトリ下に`bt_node.cpp`というファイルを作成し，以下の内容を書き込みます。
```cpp
#include "../include/action_node.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp"

using namespace MyActionNodes;
using namespace BT;

int main(int argc, char* argv[]){
  rclcpp::init(argc, argv);
  
  // ros_node.hppで定義したインスタンスにポインタを入力
  ros_node = std::make_shared<BTNode>();
  
  BehaviorTreeFactory factory;
  // action_node.hppで定義したActionNodeクラスを登録
  factory.registerNodeType<Counter>("Counter");
  factory.registerNodeType<Display>("Display");
  factory.registerNodeType<GetText>("GetText");

  // main_bt.xmlのpathを取得して登録
  std::string package_path = ament_index_cpp::get_package_share_directory("bt_sample");
  factory.registerBehaviorTreeFromFile(package_path + "/config/main_bt.xml");

  // main_bt.xml内のMainBTツリーを作成
  BT::Tree tree = factory.createTree("MainBT");

  // Tree構造を表示
  printTreeRecursively(tree.rootNode());
  
  NodeStatus status = NodeStatus::RUNNING;
  while(status == NodeStatus::RUNNING && rclcpp::ok()){
    // ROSの処理とBehaviorTreeの処理を交互に実行
    rclcpp::spin_some(ros_node);
    status = tree.tickOnce();
  }

  rclcpp::shutdown();
  return 0;
}
```
`CMakeLists.txt`を以下のように変更します。
```cmake
cmake_minimum_required(VERSION 3.8)
project(bt_sample)

find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

ament_auto_add_executable(
  server src/server.cpp
)

// 追加
ament_auto_add_executable(
  bt_node src/bt_node.cpp include/action_node.hpp include/ros_node.hpp
)

rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/CallText.srv"
  DEPENDENCIES
)

rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} rosidl_typesupport_cpp)
target_link_libraries(server "${cpp_typesupport_target}")
target_link_libraries(bt_node "${cpp_typesupport_target}") // 追加

install(DIRECTORY
  config
  DESTINATION share/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_auto_package()
```
colcon buildします。


# テスト
ターミナル１
```
ros2 run bt_sample server
```
ターミナル２
```
ros2 run bt_sample bt_node
```

動作確認の動画

https://youtu.be/CzbTxIAG26I

# 終わりに
　実用の上ではデバッグの観点から複雑な処理はROSなどの外部のプログラムに実装し，BehaviorTreeはあくまでもそれらの実行順の制御のために使うのが良いと思われる。