　今回はゲーム開発などでよく使われる制御ツールである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を継承して作成するのが良いと思います。

サンプル

前提

#include "behaviortree_cpp/behavior_tree.h"
#include "behaviortree_cpp/bt_factory.h"
using namespace BT;

ポート無しSyncActionNode
他のノードと値のやり取りがなく，処理がすぐに終わる場合。

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
他のノードと値のやり取りがあり，処理がすぐに終わる場合。

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
他のノードと値のやり取りがなく，処理に時間がかかる場合。

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
他のノードと値のやり取りがあり，処理に時間がかかる場合。

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;
    }
};

チュートリアル

簡単なサンプルプログラムを作成していきます。全体のコードはこちらを参照してください。

サンプルプログラムには

ROSのサービス通信で文字列を要求する
受け取った文字列の字数をカウントする
カウントを出力する

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

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行目あたりに次の二行を追加します。

<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

CMakeLists.txtを以下のように書き換えます。

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を作成し，以下の内容を書き込みます。

#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を作成して，以下のコードを書き込みます。

#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を作成します。

ウィンドウ左側のUntitledとなっている部分を右クリックし，Renameから名前をMainBTに変更してください。

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

GetTextはROSのサービスサーバーからテキストを受け取り，そのテキストをOutputPortにセットするStatefulActionNodeです。

CounterはInputPortから受け取ったテキストの文字数をカウントして，OutputPortにセットするSyncActionNodeです。

DisplayはInputPortから受け取った数字をターミナル上に表示するSyncActionNodeです。

作成したActionノードはModels下のActionタブ内にあります。

次にデフォルトのノードとカスタムアクションノードを使ってTree構造を作ります。
今回は下の画像のように並べます。

　この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/に保存します。

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

ActionNodeの中身の実装

先ほどGroot2で作成した3つのカスタムアクションノード，Counter，Display，GetTextの中身を実装していきます。
includeディレクトリ下にaction_node.hppを作成して以下の内容を書き込みます。

#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というファイルを作成し，以下の内容を書き込みます。

#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_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

動作確認の動画

終わりに

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