🌳

ROS2: BehaviorTreeのチュートリアルを試す

2022/11/14に公開約19,600字

海洋ロボコンをやっている人です。
今回は、BehaviorTreeのチュートリアルをROS2およびROS2ワークスペースを使って学んでみたので、その備忘録としてまとめます。

この記事を読むことで

のように、BehaviorTreeによるプログラムを体験することができます。
(monitorモードは未対応、あくまでチュートリアルレベルです)

使用する機器および環境は以下です。

  • Linux OS
    • Ubuntu 22.04 Laptop PC
  • ROS
    • Humble

本記事のプログラム

この記事で扱うプログラムのコードはGithubよりクローンしてご活用ください。

https://github.com/tasada038/ros2_bt_tut

BehaviorTreeパッケージのインストール

BT公式ページのWhat is a Behavior Tree?によると

BehaviorTree(BT)はロボットやPCゲームなどの自律型エージェントのさまざまなタスク切り替えを構造化する方法

とあります。

要は、枝分かの状態遷移図を用いて、タスク管理&ロボットの制御ができるぞという認識で、最初は良いのかもしれません。

このBehaviorTreeですが、枝分かれしているノードtickという信号が送信され、信号の状態に応じて

  • SUCCESS
  • FAILURE
  • RUNNING

ノードが返ってくるので、この状態に応じた制御をしていきます。

さらに、BehaviorTreeはROSなどのミドルウェアに簡単に統合できるようなC++ライブラリとなっており、ツリーをXML形式で記述できる点も特徴です。

詳しくはチュートリアルや、Referenceを参考にして頂けると良いかと。

BehaviorTree.CPPのインストール

まずはBehaviroTreeをROS2から利用できるパッケージをインストールしていきます。

sudo apt-get install libzmq3-dev libboost-dev libncurses5-dev libncursesw5-dev

git clone --branch v3.8 https://github.com/BehaviorTree/BehaviorTree.CPP.git

cd BehaviorTree.CPP
mkdir build; cd build
cmake ..
make
sudo make install

基本的には、上記のとおりにインストールしていけば問題ないのですが、Githubなどから最新版のインストールをする場合、BehaviorTree.CPPのバージョンに気をつける必要があります。

Groot 1.0 is compatible only with BehaviorTree.CPP 3.8.x, and it is not expected to work correctly with BehaviorTree.CPP 4.x.

If you want to compile using catkin build(ROS) or colcon_build (ROS2) then you must be sure that version 3.8.x is used (branch V3.8).

2022/11月現在では、GithubのREADMEに上記のように記載があり

とのこと。

今後、最新バージョンが対応すると思いますが、今は待ちだと思います。

make installまでエラーなく通れば、準備完了です。

ROS2: BehaviorTreeのチュートリアルを試す

ここからはBehaviorTree公式サイトにあるTutorial-BasicをROS2およびROS2ワークスペースで実行して動作確認してきます。

1 Your first Behavior Tree

1つ目のチュートリアルは、tickを制御するControlNodesであるSequenceNodesを使用し、SequenceNodesの子ノード(ActionNodes)を順に実行するTreeを作成します。

ROS2ワークスペースで作成するため、

  • tree情報を記述したxmlファイルをconfigフォルダ下
  • 各Actionノードを記述したプログラムをinclude下
  • BehaviorTreeに登録し実行するプログラムをsrc下

に配置します。

Githubのサンプルでは、ROS2 C++準拠になるようにbt_tut_01_component.cppも用意していますが、プログラムでは使用していません。

プログラムの配置や中身を確認したら、以下のコマンドで実行してみましょう。

ros2 run ros2_bt_tut bt_tut_01

以下のような実行結果が得られればBehaviorTreeの最初の一歩は完了です。

Tutorial 01
Battery OK
Gripper open
ApproachObject: approach_object
Gripper close

2 Blackboard and ports

チュートリアル1では、xml形式でのBehaviorTreeの構成方法、各ノードのプログラム記述がどんなものかが大まかに確認できるものでした。

ただ現実問題として、プログラムの中は単純に各動作・処理をつなげたものとなるわけではなく、対象のノードにパラメータといった情報を渡したい場面が多数存在します。

これらの情報の入力と出力を行う機能としてBehaviorTreeにはBlackboardとportsという機能があるので、こちらを試していきます。

BehaviorTreeの構造としては、ControlNodesであるSequenceNodesから3つの子ノードを配置し、そのうち2つの子ノード間でデータの入出力を行っていきます。

チュートリアル2では、ROS2のトピックをpublishしてROS2とも連携させてみました。

まずはBehaviorTreeの構想をxmlで記述します。

bt_tut_02_tree.xml
<root main_tree_to_execute = "MainTree" >
  <BehaviorTree ID="MainTree">
    <Sequence name="root_sequence">
           <SaySomething     message="hello" />
           <ThinkWhatToSay   text="{the_answer}"/>
           <SaySomething     message="{the_answer}" />
    </Sequence>
  </BehaviorTree>
</root>

つづいて、各ノードで必要なメンバ関数をhppファイルにて宣言します。

*もちろん開発規模が大きくなるに連れ、デストラクタや必要なメンバ関数もhppにて追記していきます。

bt_tut_02.hpp
#ifndef ROS2_BT_TUT__BT_TUT_02_HPP_
#define ROS2_BT_TUT__BT_TUT_02_HPP_

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

#include "behaviortree_cpp_v3/action_node.h"
#include "behaviortree_cpp_v3/bt_factory.h"

using namespace BT;

// SyncActionNode (synchronous action) with an input port.
class SaySomething : public SyncActionNode
{
public:
  // Constructor 
  SaySomething(const std::string& name, const NodeConfiguration& config);
  // STATIC method.
  static PortsList providedPorts();
  // Override the virtual function tick()
  virtual NodeStatus tick() override;

private:
  rclcpp::Node::SharedPtr node_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
};


class ThinkWhatToSay : public SyncActionNode
{
public:
  ThinkWhatToSay(const std::string& name, const NodeConfiguration& config);
  static PortsList providedPorts();
  // This Action writes a value into the port "text"
  virtual NodeStatus tick() override;
};

#endif //ROS2_BT_TUT__BT_TUT_02_HPP_

ros2で使用するnode_pub_などもメンバ変数としてprivateで宣言をしています。

hppで宣言した内容を元に、cpp側でコンストラクタやメンバ関数の中身を記述していきます。

チュートリアル02では

  • コンストラクタ: SaySomething
  • メンバ関数: providePorts, tick

となっています。

bt_tut_02_component.cpp
#include "rclcpp/rclcpp.hpp"
#include "ros2_bt_tut/bt_tut_02.hpp"

using namespace BT;

  // Constructor 
SaySomething::SaySomething(const std::string& name, const NodeConfiguration& config)
    : SyncActionNode(name, config)
{
  node_ = rclcpp::Node::make_shared("input");
  pub_ = node_->create_publisher<std_msgs::msg::String>("/input_string",10);
}

// It is mandatory to define this STATIC method.
PortsList SaySomething::providedPorts()
{
  // This action has a single input port called "message"
  return { InputPort<std::string>("message") };
}

// Override the virtual function tick()
NodeStatus SaySomething::tick()
{
  Optional<std::string> msg = getInput<std::string>("message");
  // Check if optional is valid. If not, throw its error
  if (!msg)
  {
    throw RuntimeError("missing required input [message]: ", 
                            msg.error() );
  }
  auto msg_data = msg.value();
  // use the method value() to extract the valid message.
  std::cout << "Robot says: " << msg_data << std::endl;

  std_msgs::msg::String input_msg;
  input_msg.data = msg_data;
  RCLCPP_INFO(node_->get_logger(), "input data: %s", input_msg.data.c_str());
  pub_->publish(input_msg);

  return NodeStatus::SUCCESS;
}


ThinkWhatToSay::ThinkWhatToSay(const std::string& name, const NodeConfiguration& config)
  : SyncActionNode(name, config)
{ }

PortsList ThinkWhatToSay::providedPorts()
{
  return { OutputPort<std::string>("text") };
}

// This Action writes a value into the port "text"
NodeStatus ThinkWhatToSay::tick()
{
  // the output may change at each tick(). Here we keep it simple.
  setOutput("text", "The answer is 42" );
  return NodeStatus::SUCCESS;
}

コンストラクタやメンバ関数の内容を記述できたら、mainでBehaviorTreeへ各ノードを登録し、実行するプログラムを追加します。

bt_tut_02.cpp
#include "rclcpp/rclcpp.hpp"
#include "ros2_bt_tut/bt_tut_02.hpp"

#define DEFAULT_BT_XML "/home/ubuntu/dev_ws/src/ros2_bt_tut/config/bt_tut_02_tree.xml"

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  BehaviorTreeFactory factory;
  factory.registerNodeType<SaySomething>("SaySomething");
  factory.registerNodeType<ThinkWhatToSay>("ThinkWhatToSay");

  auto tree = factory.createTreeFromFile(DEFAULT_BT_XML);

  NodeStatus status = NodeStatus::RUNNING;
  while(rclcpp::ok() && status == NodeStatus::RUNNING){
    status = tree.tickRoot();
  }

  return 0;
}

xmlファイルのパスはチュートリアル1, 2では絶対パスで指定していますが、以降のチュートリアルでは別の記述方法で指定していきます。

cd ~/dev_ws
colcon build
. install/setup.bash
ros2 run ros2_bt_tut bt_tut_02

で実行し、動作確認すると以下のように表示されるはずです。

Tutorial 02
[WARN] [1668345931.947518573] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
Robot says: hello
[INFO] [1668345931.949463504] [input]: input data: hello
Robot says: The answer is 42
[INFO] [1668345931.949576916] [input]: input data: The answer is 42

[WARN]が生じる理由は、

ためです。

SaySomethingのActionノードが複数回実行されると、2回目以降はすでに宣言されているよ!という警告になるので、同じActionノードを複数使用しない or publisherの宣言方法を少し変える必要がありますね。

簡単な解決策としてxmlファイル内のSaySomethingを2つ→1つに変更してあげれば、複数回のノード実行がされなくなるため、[WARN]は発生しなくなります。

bt_tut_02_tree.xml
    <Sequence name="root_sequence">
-           <SaySomething     message="hello" />
           <ThinkWhatToSay   text="{the_answer}"/>
           <SaySomething     message="{the_answer}" />
    </Sequence>

また、上記の実行結果より、ThinkWhatToSayで入力した値をSaySomethingで出力できていることも確認できます。

3 Ports with generic types

チュートリアル3では、Portsの入力を様々なタイプで試してみようということです。

チュートリアル2では、string型のデータを扱っていましたが、実際にロボット等で使用する場合はint,long,double,boolなど様々かと思います。

ここではPosition2Dというdouble型の値をもつ構造を用います。

また、BehaviorTreeの宣言もcpp内で以下のように記述することもできます。

bt_tut_03.cpp
#include "rclcpp/rclcpp.hpp"
#include "ros2_bt_tut/bt_tut_03.hpp"

static const char* xml_text = R"(

 <root main_tree_to_execute = "MainTree" >
     <BehaviorTree ID="MainTree">
        <Sequence name="root">
            <CalculateGoal goal="{GoalPosition}" />
            <PrintTarget   target="{GoalPosition}" />
            <SetBlackboard output_key="OtherGoal" value="-1;3;0" />
            <PrintTarget   target="{OtherGoal}" />
        </Sequence>
     </BehaviorTree>
 </root>
 )";

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  BT::BehaviorTreeFactory factory;
  factory.registerNodeType<CalculateGoal>("CalculateGoal");
  factory.registerNodeType<PrintTarget>("PrintTarget");

  auto tree = factory.createTreeFromText(xml_text);
  BT::NodeStatus status = BT::NodeStatus::RUNNING;
  while(rclcpp::ok() && status == BT::NodeStatus::RUNNING){
    status = tree.tickRoot();
  }
  
  return 0;
}

上記でTreeの作成部分は以下のように記述されており

  • テキストからTree作成:factory.createTreeFromText(xml_text);
  • ファイルからTree作成:factory.createTreeFromFile(DEFAULT_BT_XML);

と違いがあります。

ros2 run ros2_bt_tut bt_tut_03

と実行してみると

Tutorial 03
Target positions: [ 1.1, 2.3, 0.0]
Target positions: [ -1.0, 3.0, 0.0]

とdouble型で入出力されているのが確認できます。

5 Using Subtrees

チュートリアル4は飛ばし、チュートリアル5へ進みます。
チュートリアル5ではCrossDoor behaviorの動作をサブツリーを使用して確認していきます。

サブツリーを使用することで、再利用可能なツリーを作成でき、大規模なツリーへの追加ができることが利点です。

サブツリー機能を確認するために、ControlNodesやDecoratorNodesを使用したBehaviorTreeで動作確認をします。

チュートリアル5のプログラムは、ROS2ワークスペース内で以下のように構成されており

ros2_bt_tut/
  ├ include/
      ├ ros2_bt_tut/
          ├ crossdoor_nodes.hpp
  ├ src/
      ├ crossdoor_nodes.cpp
      ├ bt_tut_05.cpp

サブツリーは以下のようにbt_tut_05.cpp側で記述するとツリーを分けることができます。

bt_tut_05.cp
#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "ros2_bt_tut/crossdoor_nodes.hpp"

static const char* xml_text = R"(
<root main_tree_to_execute = "MainTree">
    <BehaviorTree ID="MainTree">
        <Sequence>
            <Fallback>
                <Inverter>
                    <IsDoorClosed/>
                </Inverter>
                <SubTree ID="DoorClosed"/>
            </Fallback>
            <PassThroughDoor/>
        </Sequence>
    </BehaviorTree>

    <BehaviorTree ID="DoorClosed">
        <Fallback>
            <OpenDoor/>
            <RetryUntilSuccessful num_attempts="5">
                <PickLock/>
            </RetryUntilSuccessful>
            <SmashDoor/>
        </Fallback>
    </BehaviorTree>
</root>
 )";

int main(int argc, char **argv)
{
  // 略
  return 0;
}

プログラムを実行するとBT::printTreeRecursively(tree.rootNode());の部分が最初に表示され、CrossDoorの処理に進んでいきます。

ros2 run ros2_bt_tut bt_tut_05

----------------
Sequence
   Fallback
      Inverter
         IsDoorClosed
      DoorClosed
         Fallback
            OpenDoor
            RetryUntilSuccessful
               PickLock
            SmashDoor
   PassThroughDoor
----------------
isDoorClosed: FAILURE
openDoor: FAILURE
pick a Lock: FAILURE
pick a Lock: SUCCESS
passThroughDoor: SUCCESS

crossdoor_nodes.hppに記述のあるprivateメンバ変数_door_open,_door_locked,_pick_attemptsの値を変えることで、実行結果が変わることも確認してみると、crossdoorのBehaviorTreeがよく理解できるかと思います。

6 Port Remapping

チュートリアル6では、Blackboardによる入出力を異なる名称にリマッピングし、ツリーとサブツリーの名前が重ならないようにするものです。

ros2 run ros2_bt_tut bt_tut_06

で実行すると

Tutorial 6
[ MoveBase: STARTED ]. goal: x=1 y=2.0 yaw=3.00
[ MoveBase: FINISHED ]
Robot says: mission accomplished
[INFO] [1668346370.479600523] [input]: input data: mission accomplished
--------------
move_result (std::string) -> full
move_goal (Pose2D) -> full
--------------
--------------

となります。

7 Use multiple XML files

XML形式のツリーを複数のファイルに分割する方法です。

bt_tut_07_tree.xml
<root main_tree_to_execute = "MainTree" >
  <include path="./bt_tut_07_subtreeA.xml" />
  <include path="./bt_tut_07_subtreeB.xml" />
  <BehaviorTree ID="MainTree">
    <Sequence name="root_sequence">
            <SaySomething     message="starting MainTree" />
            <SubTree ID="SubA" />
            <SubTree ID="SubB" />
    </Sequence>
  </BehaviorTree>
</root>

上記のように、includeを行い、インクルード先のIDと紐づければ複数のXMLファイルを使用できます。

また、C++側でXMLファイルを読み込むとき、絶対パスではなく、以下のように<filesystem> <iostream>を使用して記述もできます。

bt_tut_07.cpp
#include "rclcpp/rclcpp.hpp"
#include "ros2_bt_tut/bt_tut_02.hpp"

#include <filesystem>
#include <iostream>

// #define MAIN_BT_XML "/home/ubuntu/dev_ws/src/ros2_bt_tut/config/bt_tut_07_tree.xml"

using namespace BT;

int main(int argc, char **argv)
{
  std::filesystem::path ros_ws_path = std::filesystem::current_path();
  std::string xml_file_name = "/src/ros2_bt_tut/config/bt_tut_07_tree.xml";
  std::string xml_path = ros_ws_path.string() + xml_file_name;

  rclcpp::init(argc, argv); 
  BehaviorTreeFactory factory;
  factory.registerNodeType<SaySomething>("SaySomething");
  
  factory.registerBehaviorTreeFromFile(xml_path);

  auto tree = factory.createTreeFromFile(xml_path);
  NodeStatus status = NodeStatus::RUNNING;
  while(rclcpp::ok() && status == NodeStatus::RUNNING){
    status = tree.tickRoot();
  }

  //Check that the BTs have been registered correctly
  std::cout << "Registered BehaviorTrees:" << std::endl;
  for (const std::string& bt_name : factory.registeredBehaviorTrees())
  {
    std::cout << " - " << bt_name << std::endl;
  }
  return 0;
}

実行すると以下のように表示されます。(こちらも[WARN]が発生しますが、省いています。)

ros2 run ros2_bt_tut bt_tut_07

Robot says: starting MainTree
[INFO] [1668346387.117569955] [input]: input data: starting MainTree
Robot says: Executing SubA
[INFO] [1668346387.117667646] [input]: input data: Executing SubA
Robot says: Executing SubB
[INFO] [1668346387.117699933] [input]: input data: Executing SubB
Registered BehaviorTrees:
 - MainTree
 - SubA
 - SubB

8 Pass additional arguments

最後に、ノードに追加の変数を渡すチュートリアルを見ていきます。

いままでのチュートリアルでは、ツリーの葉部分にあたるActionNodes

MyCustomNode(const std::string& name, const NodeConfiguration& config);

という形で定義していましたが、場合によっては追加の変数やポインタなどを使用したい時がでてきます。

なので、このチュートリアルでは、ActionNodesに複数の変数を渡してみよう!という内容になっています。

記述方法はシンプルであり、コンストラクタ内に引数を追加していく形で実装します。

bt_tut_08.hpp
// Action_A has a different constructor than the default one.
class Action_A : public SyncActionNode
{
public:
  // additional arguments passed to the constructor
  Action_A(const std::string& name, const NodeConfiguration& config,
           int arg1, double arg2, std::string arg3) :
    SyncActionNode(name, config), _arg1(arg1), _arg2(arg2), _arg3(arg3)
  {
    node_ = rclcpp::Node::make_shared("action_a");
    int_pub_ = node_->create_publisher<std_msgs::msg::Int32>("/action/int",10);
    float_pub_ = node_->create_publisher<std_msgs::msg::Float32>("/action/float",10);
    string_pub_ = node_->create_publisher<std_msgs::msg::String>("/action/string",10);
  }

  NodeStatus tick() override
  {
    std::cout << "Action_A: " << _arg1 << " / " << _arg2 << " / " << _arg3 << std::endl;
    std_msgs::msg::Int32 int_msg;
    std_msgs::msg::Float32 float_msg;
    std_msgs::msg::String string_msg;
    int_msg.data = _arg1;
    float_msg.data = _arg2;
    string_msg.data = _arg3;
    RCLCPP_INFO(node_->get_logger(),
                "arg1, arg2, arg3: [%d, %f, %s]",
                int_msg.data, float_msg.data, string_msg.data.c_str());
    int_pub_->publish(int_msg);
    float_pub_->publish(float_msg);
    string_pub_->publish(string_msg);
    
    return NodeStatus::SUCCESS;
  }
  static PortsList providedPorts()
  {
    return {};
  }

private:
  int _arg1;
  double _arg2;
  std::string _arg3;

  rclcpp::Node::SharedPtr node_;
  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr int_pub_;
  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr float_pub_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr string_pub_;
};

実行してみると

Tutorial 8
ros2 run ros2_bt_tut bt_tut_08

Action_A: 42 / 3.14 / hello world
[INFO] [1668346442.312539579] [action_a]: arg1, arg2, arg3: [42, 3.140000, hello world]
Action_B: 69 / 9.99 / interesting_value

std_msgsで定義したInt32,Float32,StringのROS2メッセージが確認できました。

Asynchronous Actions

Tutorial-Advancedでは非同期アクション, 同時実行と並列処理, 非同期と同期処理について説明があり、以下を実行するとこれらを確認できます。

Tutorial 9
ros2 run ros2_bt_tut bt_tut_09

action_A: Started. Send Request to server.
action_A: Waiting Reply...
action_A: Done. 'Waiting Reply' loop repeated 72521 times
action_A: cleaning up after SUCCESS

action_B: Started. Send Request to server.
action_B: Waiting Reply...
action_B: Halted.
action_B: cleaning up after an halt()

action_A: Halted.
action_A: cleaning up after an halt()

action_B: Halted.
action_B: cleaning up after an halt()

GrootでBehaviorTreeを記述する

BehaiorTreeには、Grootというツリーをつくるためのエディターが存在するので、上記で手打ちしていたXML形式のツリーをこちらでも作成してみたいと思います。

Grootのインストール

sudo apt install qtbase5-dev libqt5svg5-dev libzmq3-dev libdw-dev

git clone https://github.com/BehaviorTree/Groot.git
cd Groot
git submodule update --init --recursive
mkdir build; cd build
cmake ..
make
./Groot

BehaviorTree.CPPのインストール時でも記述しましたが

ため、注意が必要です。

インストール後、起動すると下図のようになるので、Editorを選択しましょう。

Editor画面になると各Nodeを左側のTreeNode Paletteからドラッグで追加できるので、チュートリアル5のCrossDoorをGrootで再現してみましょう。

追加するノード

  • Control
    • Fallback
    • Sequence
  • Decorator
    • Inverter
    • RetryUntilSuccessful

またノードの葉部分にあたるAction Nodeは、TreeNode Paletteの+ボタン「Add Custom Node to Palette」からActionNodeを選択し追加します。

名称はIsDoorClosedなど任意の名前に指定します。

また、Behavior Treeが正しくないとき、ステータスLEDは赤点滅となっています。
正しくないTreeの状態でSave Treeを実行すると

Oops!
Malformed behavior tree. File can not be saved

というウィンドウが表示され怒られます。

https://github.com/BehaviorTree/Groot/issues/30

もちろん、イシューにもバグではなく、Treeが正しくないからだと記述があります。

ただしく、BehaviorTreeを作れたら、Save TreeよりXMLファイルを保存して、保存したファイル名にプログラムを書き換えて実行してみましょう。

ros2 run ros2_bt_tut bt_tut_05_v2

うまく、実行できればOKだと思います。

Reference

BehaviorTree Tutorial - Basics

二足歩行ロボット研修(kora編)[24] Behavior Treeを使ったロボットの行動計画について

[Behavior Tree] ワタシハ ビヘイビアツリー チョットデキル

Github: BehaviorTree.CPP

ROS2中的行为树 BehaviorTree

Youtube: Behavior Trees in Robotics (Part 2 - C++ Implementation)


さいごに

BehaviorTreeを使えばproject_srsさんの下記記事のように

https://qiita.com/srs/items/bd90ea26ee45c3331c10

monitorモードで状態を監視しながら、ロボットの制御までを行えます。

GithubにもBehaviorTree rosというワードで調べると、ロボットアームやAGVを制御する例がいくつかでてくるので、ここまでは最低限学んでおきたいと思いました。

以上

Discussion

ログインするとコメントできます