二足歩行ロボット研修二足歩行ロボット研修(kora編)

二足歩行ロボット研修(kora編)[25] Behavior Tree.CPP を動かしてみる

こんにちは。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)) を使って登録します。
  • ノードの登録ができたら、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 のおおよその使い方が分かったので、次回からさらに作り込んでいきたいと思います。

タイトルとURLをコピーしました