Go to Qiita Advent Calendar 2024 Top
LoginSignup
34
22

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@srs(project_srs)

ROS講座128 BehaviorTreeを使う

Posted at

環境

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

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

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

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

behavior tree

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

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

behaviortree_cpp_v3

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

インストール

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

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

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

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

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

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

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

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の出力の実行部分に分かれます。

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してください。以下のコマンドで起動します。

grootの起動
rosrun groot Groot

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

オリジナルノードの登録

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

bt_custom_node.png

ノードをつなげる

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

bt_edit.png

portとblack boardをつなげる。パラメーターを入れる。

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

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

ビルド

cd ~/catkin_ws
catkin_make

実行

move_base_bt.launchで一度にsimとmove_baseとbehavior treeを起動できます。

roslaunch bt_lecture move_base_bt.launch

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

bt_ros2.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
ゲームAI
behavior_tree_v3 APIドキュメント
behavior_tree_v3 チュートリアル

目次ページへのリンク

ROS講座の目次へのリンク

34
22
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
34
22

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?