こんにちは。koraです。
前回の記事では Behavior Tree の基本的な概念について書きました。
今回は、ROS から利用できる Behavior Tree のライブラリである BehaviorTree.CPP を使ってみます。
ノードの確認
まず、このライブラリに実装されているノードを確認します。このライブラリでは、大きく分けて4種類のノードがあります。
- ControlNodes
ツリーの枝分かれ部分にあたるノードで、tick の流れを制御します。子ノードを1つ以上持つことができます。 - DecoratorNodes
tick の流れを制御する点は ControlNodes と似ていますが、子ノードを1つだけしか持ちません。 - ActionNodes
ツリーの葉部分にあたるノードで、アクションを実行します。アクションの結果に応じて SUCCESS か FAILURE か RUNNING を返します。アクションの中身はライブラリのユーザーが実装します。 - ConditionNodes
条件に応じて SUCCESS か FAILURE を返します。基本的にシステムの状態を変更することはありません。
ActionNodes と ConditionNodes を合わせて 葉ノード (LeafNodes) と呼びます。
ControlNodes
ControlNodes には以下のようなノードがあります。基本的には前回の記事で説明した Sequence、Fallback、Parallel ですが、BehaviorTree.CPP ではさらに細かく分かれています。
- SequenceNodes
子ノードを順に実行するノードです。SequenceNodes には Sequence と ReactiveSequence と SequenceStar の3種類あり、子ノードから RUNNING が返ってきた後の処理が異なります。それぞれの詳細については Sequence のチュートリアルと Sequence のドキュメントに書かれています。 - FallbackNodes
子ノードのうちひとつを実行するノードです。FallbackNodes にも Fallback と ReactiveFallback があり、子ノードから RUNNING が返ってきた後の処理が異なります。詳細は Fallback のドキュメントにあります。 - ParallelNodes
複数の子ノードを並列に実行するノードです。実装はされているようですが、本記事執筆時点 (2021年7月) ではドキュメントはまだないようです。参考: How to use Parallel node)
DecoratorNodes
DecoratorNodes には以下のようなノードが実装されています。
- InverterNode
子ノードの SUCCESS と FAILURE を反転して親ノードに返します。子ノードが RUNNING を返した場合はそのまま RUNNING を親ノードに返します。 - ForceSuccessNode
子ノードが FAILURE を返した場合 SUCCESS を親ノードに返します。子ノードが RUNNING を返した場合はそのまま RUNNING を親ノードに返します。 - ForceFailureNode
子ノードが SUCCESS を返した場合 FAILURE を親ノードに返します。子ノードが RUNNING を返した場合はそのまま RUNNING を親ノードに返します。 - RepeatNode
子ノードの tick を N 回繰り返します。子ノードが FAILURE を返した場合はループを中断して親ノードに FAILURE を返します。子ノードが RUNNING を返した場合は親ノードに RUNNING を返します。 - RetryNode
子ノードの tick を N 回繰り返します。子ノードがSUCCESS を返した場合はループを中断して親ノードに SUCCESS を返します。子ノードが RUNNING を返した場合は親ノードに RUNNING を返します。 - DecoratorBlackboardPreconditionNode
指定された条件が満たされた時だけ子ノードを tick します。 - DelayNode
指定された時間だけ子ノードの実行を先延ばしします。待機中は親ノードに RUNNING を返します。 - TimeoutNode
子ノードが指定された時間より長く RUNNING を返し続けた場合、子ノードを中断させて親ノードに FAILURE を返します。
ActionNodes
ActionNodes は SyncActionNode と AsyncActionNode に分けられます。
- SyncActionNode
同じスレッドでアクションを実行します。同期処理なのでこのアクションは SUCCESS か FAILURE のみ返し、RUNNING は返しません。 - AsyncActionNode
別のスレッドでアクションを実行します。非同期処理になっており、アクションの進み具合に応じて SUCCESS か FAILURE か RUNNING を返します。
Behaivor Tree の作成
How to create a BehaviorTree を参考に、簡単な Behavior Tree を作ってみます。
準備
BehaviorTree.CPP ライブラリは ROS パッケージとして利用できるようになっています。以下のコマンドでインストールします。
$ sudo apt-get install ros-$ROS_DISTRO-behaviortree-cpp-v3
さらに、以下のようなコマンドで ROS の作業ディレクトリに自作パッケージを作成します。
$ cd ~/catkin_ws/src $ catkin_create_pkg behavior_tree_tutorial roscpp #
ノードの作成
BehaviorTree.CPP のノードは C++ で作成します。
- ROS の慣習に合わせて behavior_tree_tutorial/include/behavior_tree_tutorial ディレクトリにヘッダーファイルを作成します。ファイル名は単純に dummy_nodes.h とします。
-
ノードを作成する際、基本的には SyncActionNode クラスか AsyncActionNode クラスを継承します。次の例では SyncActionNode クラスを継承して TouchBall クラスを作成しています。(アクションの中身は tick() に記述します。この例は実際にロボットを動かすわけではなく、std::cout で文章を出力するだけの単純なアクションです。)
class TouchBall : public BT::SyncActionNode { public: TouchBall(const std::string& name) : BT::SyncActionNode(name, {}) { } BT::NodeStatus tick() override { std::cout << "TouchBall" << std::endl; return BT::NodeStatus::SUCCESS; } };
-
ノードは関数オブジェクト(ファンクタ)を使って作ることもできます。次の例では、CheckBallClose という普通の関数を定義しています。これを後でノードにします。
BT::NodeStatus CheckBallClose() { std::cout << "CheckBallClose" << std::endl; return BT::NodeStatus::SUCCESS; }
- ここで省略しますが、クラスを作ってそのメンバ関数をノードにすることもできます。
ツリーの作成
-
ツリー構造は XML で作成します。パッケージ内に config ディレクトリを作って、my_tree.xml ファイルを作成します。以下の例は、Sequence が親ノードで CheckBallClose と TouchBall が子ノードとなってます。
<root main_tree_to_execute = "MainTree" > <BehaviorTree ID="MainTree"> <Sequence name="root_sequence"> <CheckBallClose name="check_ball_close"/> <TouchBall name="touch_ball"/> </Sequence> </BehaviorTree> </root>
- ツリーを実行するプログラム本体を作ります。ファイル名は dummy_bt.cpp として behavior_tree_tutorial/src ディレクトリに作成します。
-
最初に BehaviorTreeFactory オブジェクトに自作したノードを登録する必要があります。
-
TouchBall は SyncActionNode クラスを継承して作ったので、factory.registerNodeType
(“TouchBall”) で登録します。
- CheckBallClose は普通の関数ですので、std::bind で関数オブジェクトを作って、factory.registerSimpleCondition(“CheckBallClose”, std::bind(CheckBallClose)) を使って登録します。
-
TouchBall は SyncActionNode クラスを継承して作ったので、factory.registerNodeType
- ノードの登録ができたら、factory.createTreeFromFile(path + “/config/my_tree.xml”) で先程の XML ファイルを読み込んでツリーを構築します。
-
最後に tree.tickRoot() で tick を1回実行します。実際にロボットをコントロールするようなツリーでは何度も tick を実行することになりますが、この例では全ての葉ノードが SUCCESS しか返さない単純なツリーなので1回の実行で十分です。
#include <ros/package.h> #include "behaviortree_cpp_v3/bt_factory.h" #include "behavior_tree_tutorial/dummy_nodes.h" int main() { BT::BehaviorTreeFactory factory; factory.registerNodeType<TouchBall>("TouchBall"); factory.registerSimpleCondition("CheckBallClose", std::bind(CheckBallClose)); std::string path = ros::package::getPath("behavior_tree_tutorial"); auto tree = factory.createTreeFromFile(path + "/config/my_tree.xml"); tree.tickRoot(); return 0; }
実行結果
パッケージをビルドして実行すると、期待通り CheckBallClose ノードと TouchBall ノードが順に実行される出力が得られました。
次回
BehaviorTree.CPP のおおよその使い方が分かったので、次回からさらに作り込んでいきたいと思います。