ROS講座128 BehaviorTreeを使う

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 18.04 |
| ROS | Melodic |
| Gazebo | 9.0.0 |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
ロボットの行動を制御するためにbehavior treeが使われることがあります。これはゲームAI等で発達した技術で、行動や判断を表すツリー状の図によって実際の動きを制御できるものです。

# behavior tree
behavior treeの概要についての概要は[参考資料](https://www.slideshare.net/torisoup/unity-behavior-treeai)が仕組みや応用について詳しいので見てみてください。簡単に説明するとタスクを実行する「Actionノード」や「Conditionノード」があり、またそれらを装飾するための「Decorationノード」や並べるための「Controlノード」があり、これら接続したツリーグラフ構造でロボットの行動を記述します。各ノードはSUCCESS、FAILURE、RUNNINGの状態を返すことが出来てこれらを使ってノード間の制御が行われます。

このフレームワークを用いることで、各ノードのそれぞれを実装する作業とそれらを接続する作業に綺麗に分岐することが出来ます。これによってロボットの動きをちょっと変える等のことが簡単にできるようになります。

## behaviortree_cpp_v3
behavior treeについて調べるとUnityの記事が多数見つかりますが、もちろんC++から使えるライブラリも「behaviortree_cpp_v3」というものがあります。このライブラリではbehavior treeが実装できるのはもちろんGrootというソフトがありこれで、behavior treeを記述することや実行中の動きを見ることやログを再生することもできます。

## インストール
本体は以下でインストールできます。

```shell
sudo apt install ros-melodic-behaviortree-cpp-v3
```

エディタのGrootはaptでは入らないのでgithubからダウンロードします

```shell
git clone https://github.com/BehaviorTree/Groot.git
```

# 実装する動作

今回は台車ロボットを動かすことを考えます。ユーザーは任意のposeを指示できるとします。

* ユーザーがposeを指示するとposeキューに情報が格納される。
* ロボットはposeキューに情報があればそこに向かう
* 到着したら3秒待つ、その後にまだposeキューに情報があればそこに向かう
* 待った後にposeキューに情報がなかったら位置(0,0)に戻る

後々の作業ですが、これをbehavior treeにで記述すると以下のようになります。移動処理まで作るのは大変なので、以前作ったmove_baseのサンプルを流用します。

# 各ノードの実装

今回は以下の4つのノードが必要です

* MobeObject
    * 開始時にmoveノードにキューの先頭の位置を指令する
    * 目標地点に着いたらSUCCESS
    * 想定外の事態になったらFAULURE
* ReturnObject
    * 開始時にmoveノードに(0, 0)の位置を指令する
    * 目標地点に着いたらSUCCESS
    * 想定外の事態になったらFAULURE
* WaitObject
    * 開始時にから3秒たったらSUCCESS
    * その間動かない
* StandbyObject
    * 動かない

このような処理をするときに外部センサーなどの情報をbehavior tree（の各ノード）が知っていないといけません。また各ノードにおいて動作の実行が出来ないといけないです。1つのポリシーとして各ノードがrostopicのsubscribeをしたり、move_baseへの指令をするということも可能ですが、今回はbehavior treeのblackboard機能を使って情報のやり取りをします。

## black boardについて
black boardとは簡単に言うとbehavior treeの各ノードやbehavior treeを動かすプログラムでの共有メモリです。キーバリューストア形式になっていて、基本型やstring文字列の値を入れることが出来ます。基本型以外の値も入れられますが、stringとの変換コードを書かないといけないです。今回は

* behavior treeの各tickを実行する前に、判断に必用な情報をblack boardに記入する
* behavior treeを実行
* black boardの値を見てmove_baseへ指令等を行う

という流れで実装していきます。

今回使用するblackboardでbehavior treeへ入力するものとして

* pose_queue_size: ユーザー指令をためているposeキューの現在のサイズ
* move_base_finished: move_baseがSUCCESSしたらtrue
* move_base_idle: mobe_baseが動作していない（SUCCESS、ABORT、PREEMPTED、REJECTED、LOST）とtrue
* home_pose_far: 原点（0,0）から0.2m以上離れているとtrue

behavior treeから出力するものとして

* action: NONEなら何もしない、MOVE_COMMANDならポーズキューのの先頭をmove_baseに指示、RETURN_COMMANDなら原点を指示

## ソースコード

### StandbyObject

```cpp:bt_lecture/src/navigation_node.hの一部
class StandbyObject : public BT::SyncActionNode
{
public:
  StandbyObject(const std::string& name, const NodeConfiguration& config) : BT::SyncActionNode(name, config)
  {
    std::cout << this->name() << ": constructor" << std::endl;
  }

  static PortsList providedPorts()
  {
    return { OutputPort<std::string>("action") };
  }

  BT::NodeStatus tick() override
  {
    setOutput<std::string>("action", "NONE");
    return BT::NodeStatus::SUCCESS;
  }
};
```

* 時間のかからず即座にSUCCESS/FAILUREを返せるノードはSyncActionNodeを継承して実装します。
* このノードがアクセスするblack boardのキーの名前を`providedPorts`で返します。in側（このノード中では読み込みしかしない）はこのようになります。
* `tick`でノードの処理を書きます。今回はblack boardのactionにNONEを書き込んで、即SUCCESSを返しています。

### WaitObject

```cpp:bt_lecture/src/navigation_node.hの一部
class WaitObject : public BT::CoroActionNode
{
public:
  WaitObject(const std::string& name, const NodeConfiguration& config) : BT::CoroActionNode(name, config)
  {
    std::cout << this->name() << ": constructor" << std::endl;
  }

  static PortsList providedPorts()
  {
    return { InputPort<int>("wait_duration"), OutputPort<std::string>("action") };
  }

  BT::NodeStatus tick() override
  {
    std::cout << name() << ": tick start" << std::endl;
    float wait_duration = 1.0;
    auto input_wait_duration = getInput<int>("wait_duration");
    if (wait_duration)
    {
      wait_duration = input_wait_duration.value();
      std::cout << name() << ": wait " << wait_duration << " s" << std::endl;
    }

    ros::Time initial_time = ros::Time::now();
    ros::Time end_time = initial_time + ros::Duration(wait_duration);

    while (true)
    {
      ros::Time now = ros::Time::now();
      if (end_time <= now)
      {
        std::cout << name() << ": break loop " << std::endl;
        break;
      }
      setOutput<std::string>("action", "NONE");
      setStatusRunningAndYield();
    }
    std::cout << name() << ": tick end" << std::endl;
    setOutput<std::string>("action", "NONE");
    return BT::NodeStatus::SUCCESS;
  }
  void halt() override
  {
    std::cout << this->name() << ": halt" << std::endl;
    CoroActionNode::halt();
  }
};
```

* このようにSUCCESS/FAILUREを返すまで時間がかかるノードの実装方法はいくつかありますが、今回はコルーチンを使うCoroActionNodeを継承して実装します。
* このノードがアクセスするblack boardのキーの名前を`providedPorts`で返します。待機時間をパラメーターにしておくと後々に変更が簡単にできます。
* `tick`でノードの処理を書きます。まだ時間になっていない場合はwaitで待つのではなく、`setStatusRunningAndYield()`を呼びます。こうすることでノードはRUNNINGを返して、ここでプログラムがいったん中断されて、次回tick時にここから再開されます。

### MoveObject

```cpp:bt_lecture/src/navigation_node.hの一部
class MoveObject : public BT::CoroActionNode
{
public:
  MoveObject(const std::string& name, const NodeConfiguration& config) : BT::CoroActionNode(name, config)
  {
    std::cout << this->name() << ": constructor" << std::endl;
  }

  static PortsList providedPorts()
  {
    return { InputPort<bool>("move_base_finished"), InputPort<bool>("move_base_idle"),
             InputPort<bool>("pose_queue_size"), OutputPort<std::string>("action") };
  }

  BT::NodeStatus tick() override
  {
    auto pose_queue_size = getInput<bool>("pose_queue_size");
    if (!pose_queue_size || pose_queue_size.value() == 0)
    {
      return BT::NodeStatus::FAILURE;
    }

    setOutput<std::string>("action", "MOVE_COMMAND");
    std::cout << name() << ": MOVE_COMMAND Yield" << std::endl;
    setStatusRunningAndYield();

    while (true)
    {
      auto move_base_idle = getInput<bool>("move_base_idle");

      auto move_base_finished = getInput<bool>("move_base_finished");
      if (move_base_finished && move_base_finished.value())
      {
        std::cout << name() << ": move_base is finidshed: SUCCESS" << std::endl;
        setOutput<std::string>("action", "NONE");
        return BT::NodeStatus::SUCCESS;
      }

      if (move_base_idle && move_base_idle.value())
      {
        std::cout << name() << ": move_base is idle: FAILURE" << std::endl;
        setOutput<std::string>("action", "NONE");
        return BT::NodeStatus::FAILURE;
      }

      setOutput<std::string>("action", "NONE");
      setStatusRunningAndYield();
    }
  }

  void halt() override
  {
    std::cout << this->name() << ": halt" << std::endl;
    CoroActionNode::halt();
  }
};
```

* queueのサイズをみて0（=ユーザーからの指令が無いorやりつくした）ならFAILUREを返します。>0ならMOVE_COMMANDをactionに書き込み、その後にmove_base_finishedとなったらSUCCESSを返します。

### ReturnObject

```cpp:bt_lecture/src/navigation_node.hの一部
class ReturnObject : public BT::CoroActionNode
{
public:
  ReturnObject(const std::string& name, const NodeConfiguration& config) : BT::CoroActionNode(name, config)
  {
    std::cout << this->name() << ": constructor" << std::endl;
  }

  static PortsList providedPorts()
  {
    return { InputPort<bool>("move_base_finished"), InputPort<bool>("move_base_idle"), InputPort<bool>("home_pose_far"),
             OutputPort<std::string>("action") };
  }

  BT::NodeStatus tick() override
  {
    auto home_pose_far = getInput<bool>("home_pose_far");
    if (!home_pose_far || !home_pose_far.value())
    {
      return BT::NodeStatus::FAILURE;
    }

    setOutput<std::string>("action", "RETURN_COMMAND");
    std::cout << name() << ": RETURN_COMMAND Yield" << std::endl;
    setStatusRunningAndYield();

    while (true)
    {
      auto move_base_idle = getInput<bool>("move_base_idle");

      auto move_base_finished = getInput<bool>("move_base_finished");
      if (move_base_finished && move_base_finished.value())
      {
        std::cout << name() << ": move_base is finidshed: SUCCESS" << std::endl;
        setOutput<std::string>("action", "NONE");
        return BT::NodeStatus::SUCCESS;
      }

      if (move_base_idle && move_base_idle.value())
      {
        std::cout << name() << ": move_base is idle: FAILURE" << std::endl;
        setOutput<std::string>("action", "NONE");
        return BT::NodeStatus::FAILURE;
      }

      setOutput<std::string>("action", "NONE");
      setStatusRunningAndYield();
    }
  }

  void halt() override
  {
    std::cout << this->name() << ": halt" << std::endl;
    CoroActionNode::halt();
  }
};
```

* 内容としてはほぼMoveObjectと同等のものになります。

# プログラム本体の実装

behavior treeを実行する外側のプログラムです。

初期化部分、ループ内でblack boardへ入力部、behavior tree実行部、black boardの出力の実行部分に分かれます。

```cpp:bt_lecture/src/move_bt.cpp
#include <unistd.h>
#include <deque>
#include <ros/ros.h>
#include <behaviortree_cpp_v3/bt_factory.h>
#include <behaviortree_cpp_v3/loggers/bt_zmq_publisher.h>
#include <behaviortree_cpp_v3/loggers/bt_file_logger.h>
#include <geometry_msgs/PoseStamped.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tf2_ros/transform_listener.h>

#include "navigation_node.h"

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> Client;

int main(int argc, char** argv)
{
  ros::init(argc, argv, "nav_bt");
  ros::NodeHandle nh;
  ros::NodeHandle pnh("~");

  std::deque<geometry_msgs::PoseStamped> pose_queue;
  ros::Subscriber goal_sub =
      nh.subscribe<geometry_msgs::PoseStamped>("user_goal", 1, [&](const geometry_msgs::PoseStampedConstPtr pose) {
        std::cout << "pose_queue push_back " << std::endl;
        pose_queue.push_back(*pose);
      });

  Client client("/dtw_robot1/move_base", true);
  move_base_msgs::MoveBaseGoal current_goal;
  bool sent_goal = false;

  geometry_msgs::PoseStamped home_pose;
  home_pose.header.frame_id = "dtw_robot1/map";
  home_pose.pose.position.x = 0.0;
  home_pose.pose.position.y = 0.0;
  home_pose.pose.orientation.w = 1.0;

  tf2_ros::Buffer tfBuffer_;
  tf2_ros::TransformListener tfListener_(tfBuffer_);

  BT::BehaviorTreeFactory factory;
  factory.registerNodeType<MoveObject>("MoveObject");
  factory.registerNodeType<ReturnObject>("ReturnObject");
  factory.registerNodeType<WaitObject>("WaitObject");
  factory.registerNodeType<StandbyObject>("StandbyObject");

  Blackboard::Ptr my_blackboard = Blackboard::create();
  my_blackboard->set("pose_queue_size", (unsigned int)0);
  my_blackboard->set("move_base_finished", false);
  my_blackboard->set("move_base_idle", false);
  my_blackboard->set("home_pose_far", false);
  my_blackboard->set("action", "NONE");
  my_blackboard->debugMessage();

  std::string bt_filepath = "default_tree.xml";
  pnh.getParam("bt_filepath", bt_filepath);
  ROS_INFO("tree file: %s\n", bt_filepath.c_str());
  auto tree = factory.createTreeFromFile(bt_filepath, my_blackboard);
  PublisherZMQ publisher_zmq(tree);
  FileLogger logger_file(tree, "bt_trace.fbl");

  while (ros::ok())
  {
    // update input
    bool move_base_finished = false;
    bool move_base_idle = false;
    if (client.isServerConnected())
    {
      switch (client.getState().state_)
      {
        case actionlib::SimpleClientGoalState::REJECTED:
        case actionlib::SimpleClientGoalState::PREEMPTED:
        case actionlib::SimpleClientGoalState::ABORTED:
        case actionlib::SimpleClientGoalState::LOST:
          move_base_idle = true;
          break;
        case actionlib::SimpleClientGoalState::SUCCEEDED:
          move_base_finished = true;
          move_base_idle = true;
          break;
        case actionlib::SimpleClientGoalState::PENDING:
        case actionlib::SimpleClientGoalState::ACTIVE:
        case actionlib::SimpleClientGoalState::RECALLED:
        default:
          break;
      }
    }

    bool home_distance_far = false;
    try
    {
      geometry_msgs::TransformStamped transformStamped;
      transformStamped = tfBuffer_.lookupTransform("dtw_robot1/map", "dtw_robot1/base_link", ros::Time(0));
      auto& trans = transformStamped.transform.translation;
      float home_dx = (trans.x - home_pose.pose.position.x);
      float home_dy = (trans.y - home_pose.pose.position.y);
      home_distance_far = 0.2 < sqrt(home_dx * home_dx + home_dy * home_dy);
    }
    catch (tf2::TransformException& ex)
    {
      ROS_WARN("%s", ex.what());
    }

    my_blackboard->set("pose_queue_size", (unsigned int)pose_queue.size());
    my_blackboard->set("move_base_finished", move_base_finished);
    my_blackboard->set("move_base_idle", move_base_idle);
    my_blackboard->set("home_pose_far", home_distance_far);

    // bt
    auto result = tree.tickRoot();
    std::string action;
    my_blackboard->get(std::string("action"), action);

    // output
    if (client.isServerConnected())
    {
      if (action == "RETURN_COMMAND")
      {
        current_goal.target_pose = home_pose;
        client.sendGoal(current_goal);
        ROS_INFO("RETURN");
      }
      else if (action == "MOVE_COMMAND")
      {
        if (!pose_queue.empty())
        {
          auto top = pose_queue.front();
          pose_queue.pop_front();
          current_goal.target_pose = top;
          client.sendGoal(current_goal);
          ROS_INFO("MOVE");
        }
      }
    }
    ros::spinOnce();
    sleep(1);
  }
  return 0;
}
```

black board、behavior tree周りのみを解説します。

* black board
    * black boardの実態は`Blackboard::Ptr my_blackboard = Blackboard::create()`の様にインスタンス化します。
    * `my_blackboard->set("move_base_finished", false)`の様にしてblack boardに値を入れて初期化します。未初期化だと後のbehavior treeの生成時に、エラーが出ます。
    * またblack boardの値を取得する時は`my_blackboard->get(std::string("action"), action)`の様に書きます。
* behavior tree
    * `  factory.registerNodeType<MoveObject>("MoveObject")`とすることで上記のオリジナルで作ったノードをbehavior treeで使えるようになります。
    * `  auto tree = factory.createTreeFromFile(bt_filepath, my_blackboard)`の様にして生成します。第一引数で後述のbehavior treeの記述ファイルのファイルパスを書きます。
    * `auto result = tree.tickRoot()`とすると一回分のtickを実行します。
    * `PublisherZMQ publisher_zmq(tree)`を実行することでログ用のサーバーを起動します。後述のgrootのmonitorモードで状態を見ることが出来ます。
    * `FileLogger logger_file(tree, "bt_trace.fbl")`でログをファイルに残せます。


# behavior treeの記述
最終的なxmlファイルを手書きで書いてもよいのですが、せっかくなのでGrootというエディターを使用します。
上記のinstallにあるgrootをダウンロードして`catkin_make`してください。以下のコマンドで起動します。

```shell:grootの起動
rosrun groot Groot
```

最初のランチャーで`editor`を押すと編集モードになります。

## オリジナルノードの登録
左側（パレット）にはデフォルトのノードの一覧がありますが、まだオリジナルで作ったノードはありません。オリジナルのノードは別途登録する必要があります。
左のタブの上の右から4番目のボタン（Add Custom Node to Pallete）を選びます。Nameにはノードの名前を入れます。
ノードがblack boardの値を参照する場合は「add port」を押してin/outの種別とkey名を入れます。

![bt_custom_node.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/091e53f5-689d-414d-7ecd-99baf736e6ed.png)

## ノードをつなげる
左のパレットよりドラッグ＆ドロップをするとノードを追加できます。これを最初から表示されているRootのードとつなげます。Rootノードが見えないときは「Auto Zoom」を押してみてください。

![bt_edit.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/f3cadbcc-796f-45db-3815-70503ed5e16a.png)


## portとblack boardをつなげる。パラメーターを入れる。
WaitObject等portを指定したものはノードグラフの編集画面ではportに値を入れるメニューが出ます。各ノードの`providedPorts`とblack boardで同じ名前の物があっても自動でつながるわけではなく、指定をする必要があります。
wait_durationのように固定値を入れるものはその値を、actionのようにblack boardの値と対応させるようなものは`{action}`の様に{black boardのキー}と入れます。

最後にsave treeで保存します。

# ビルド
```shell
cd ~/catkin_ws
catkin_make
```

# 実行
[move_base_bt.launch](https://github.com/project-srs/ros_lecture/blob/master/bt_lecture/launch/move_base_bt.launch)で一度にsimとmove_baseとbehavior treeを起動できます。

```shell
roslaunch bt_lecture move_base_bt.launch 
```

またGrootのmonitorモードを使うと現在のbehavior treeの状態を見ることが出来ます。monitorモードで起動してconnectを押せばつながります。

![bt_ros2.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/ee015898-12d7-8c99-39ba-867b79b2a2b6.gif)


# メモ：behaviortree_cpp_v3のノードの種類

* アクションノード
    * sync系
        * [Base]SyncActionNode
        * AlwaysFailureNode
        * AlwaysSuccessNode
        * SetBlackboard blackboardにset
    * async系
        * [Base]AsyncActionNode
    * coro系
        * [Base]CoroActionNode
    * state系
        * [Base]StatefulActionNode
* コンディションノード
    * ConditionNode
* デコレーションノード
    * [Base]DecoratorNode
    * BlackboardPreconditionNode	blackboardで指定と一致すれば子ノード実行
    * DelayNode
    * ForceFailureNode
    * ForceSuccessNode
    * InverterNode
    * KeepRunningUntilFailureNode
    * RepeatNode　N回successしたらsuccess
    * RetryNode N回Failしたらfail
    * TimeoutNode
* コントロールノード
    * [Base]ControlNode
    * 非パラレル系？
        * SequenceNode
        * FallbackNode
        * IfThenElseNode
        * SequenceStarNode？
        * SwitchNode blackboardの値によって実行する子を選択
        * WhileDoElseNode？
        * ManualSelectorNode　ユーザーがterminal入力で指定
    * パラレル系？
        * ParallelNode
        * ReactiveFallback
        * ReactiveSequence

# 参考
[behavior treeでゲームAI](https://www.slideshare.net/torisoup/unity-behavior-treeai)
[ゲームAI](https://www.google.com/url?sa=t&rct=j&q=&esrc=s&source=web&cd=&cad=rja&uact=8&ved=2ahUKEwj5iszgh5jwAhXN7GEKHeYqBHcQFjAAegQIBRAD&url=https%3A%2F%2Fjsai.ixsq.nii.ac.jp%2Fej%2F%3Faction%3Drepository_action_common_download%26item_id%3D8635%26item_no%3D1%26attribute_id%3D22%26file_no%3D1&usg=AOvVaw3-ka5J3G5rNhJJ4Mrp9Qaf)
[behavior_tree_v3 APIドキュメント](https://docs.ros.org/en/melodic/api/behaviortree_cpp_v3/html/namespaceBT.html)
[behavior_tree_v3 チュートリアル](https://www.behaviortree.dev/tutorials_summary/)


# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)