ROS 2: Behavior TreeでTurtlebot3を制御する
海洋ロボコンをやってた人です。
ROS 2アドカレには2年目の投稿になります。
背景として、ある事情でBehavior Treeを使うことになった後輩に向け、Behavior Treeのプログラム構成や使い方を知ってもらうためのシミュレーターを作成するよ！という話になったのがキッカケになります。
この記事のゴールは、Behavior Treeを使ってTurtlebot3を制御/動かすところまでです。
さらに詳しく学びたい方は、BehaviorTree.CPPなどを参照してください。
動作環境は以下です。
- OS: Ubuntu 22.04
- ROS Distributions: Humble
- Behavior Tree ver 3.8
使用するパッケージは以下に置いてあるので、cloneして利用してください。
事前に必要な環境として、
-
Gazeboシミュレーション & Turtlebot3の準備
-
Behavior Tree ver3.8 / Grootのインストール
が必要になります。こちらは私の方でも
とまとめているので、良ければご参照ください。
本記事に対するコメントも積極的に募集しますので、よろしくお願いいたします。
1: Behavior Treeとは
Behavior Treeを理解するために、どのような仕組みで動いているのかを知っておいたほうが良いと思います。
公式サイトやチュートリアルより学ぶ方法もありますが、ここでは[1] A survey of Behavior Trees in robotics and AIのsurvey論文をベースに記載していきたいと思います。
Behavior Tree(以下BTと呼ぶ)より以前は有限ステートマシン：Finite State Machines (FSM)が使用されていましたが、拡張性、再利用性の低さが課題でした。
そこでBTでは状態遷移ロジックが個々に分散されず、状態が枝葉上の階層ツリー構造となるように編成され、アルゴリズムによる合成と分析が簡素化されている特徴を持ちます。
1.1: BTのコア部分
BTはrootから始まりchild, parent, leaf nodesとノード(node)として分けられて、leaf nodesは実行ノード、それ以外は制御フローノードと呼ばれています。
BTの実行は一番先頭に設置されるrootノードから開始され、親のparentノード、子のchildノード、実行ノードのleaf nodesと繋がっていくイメージです。（後述のGroot GUIを参照）
ノード実行の際に、指定された周波数でtickと呼ばれる信号が生成され、tickはノード実行を有効にし、チェックされたノードの 1 つまたは複数の子ノードへ伝播されます。
ノードは、tickを受け取った場合にのみ実行され、各ノードは実行中の場合Runningを、目標達成した場合はSuccessを、そうでない場合はFailureを親ノードに返します。
これらのノードは、3つの制御フローノード(Sequence / Fallback / Parallel) と2つの実行ノード(Action / Condition) に分類することができます。
| Node type | Symbol | Succeeds | Fails | Running |
|---|---|---|---|---|
| Sequence | → | If all children succeed | If one child fails | If one child |
| Fallback | ? | If one child succeeds | If all children fail | If one child returns running |
| Parallel | ⇉ | If >= M children succeed | If > N - M children fail | else |
| Action | shaded box | Upon completion | When impossible to complete | During completion |
| Condition | white oval | If true | If false | Never |
※ [1] Table 1 The five node types of a BTより引用
Sequence
Sequence は最も単純な制御フローノード(ControlNode)で、子ノードを順番に実行し、すべてが成功した場合は Success を返します。 一つでも子ノードが失敗したら Failsを返します。
Fallback
Fallback ノードは、Success または Running のいずれかを返す子ノードが見つかるまで、左から子ノードのTick を探索し、全ての子ノードが失敗したらFailsを、一つの子ノードがSuccess または Running のときは、その状態を親ノードに返します。
Parallel
Parallelノードはすべての子ノードを同時にチェックし、If >= M children succeedという条件を満たせばSuccessを返し、If > N - M children failという条件のときにFailsを返します。
BT.cppでは、成功した子ノードの数Mの値を決めて、使用する形になります。
Action
Actionノードは、対象のアクションが正常に完了した場合はSuccessを返し、アクションが失敗した場合はFailureを返します。
Condition
Conditionノードは、tickを受け取ると指定した条件をチェックし、条件の命題が成り立つかどうかに応じて、SuccessまたはFailureを返します。
2: BTでTurtlebot3を制御するパッケージを作る
ここからはBTでTurtlebot3を制御するパッケージを作る工程から、中身について記載していきたいと思います。
他のロボットやシステムに流用する際に、作成手順を参考にしていただければ幸いです。
2.1: BTの木構造を作成
まずはBTの木構造を作成し、システムの全体像（要件定義 / 設計）を決定します。
Grootを立ち上げ、Editor > Startから編集画面へ移り、5つの主要ノードなどを使用しながらBTシステムを設計していきます。
※Grootの立ち上げは以下コマンドなどを参照。
cd ~/BehaviorTree/Groot/build/
./Groot
上図では、先程例に上げた3つの制御フローノード(Sequence / Fallback / Parallel) と2つの実行ノード(Action / Condition) を使用した一例になります。
Grootで可視化した際に、実行ノードには頭文字のAとCが記載されていることも確認できますね。
BTの木構造を作成できたら、左上にあるアイコンSave Treeを選択して.xml形式で保存すれば良いです。
実際にxmlの中身を見てみると以下のようになっており、success_threshold="3"でParallelの成功判定の閾値を設定できます。
また、{x} / {r}という記述はBlackboard and portsというBT独自のkey / valueの共通ストレージを意味しており、各ノードで値を共通することができます。
ROS 2で例えると、topic通信のデータというイメージで初めは良いと思います。
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<Parallel failure_threshold="0" success_threshold="3">
<Action ID="MoveRobot" linear_x="{x}" sleep_mtime="100"/>
<Fallback>
<Action ID="CameraDetected"/>
<Sequence>
<Condition ID="ReadingLaser" laser_status="{r}"/>
<Action ID="Rotating" laser_status="{r}" linear_x="{x}" sleep_mtime="100"/>
</Sequence>
</Fallback>
<Action ID="Stop" name="stop"/>
</Parallel>
</BehaviorTree>
</root>
2.2: BTノードの実装
木構造による要件定義を終えたら、制御フローノード(Sequence) と実行ノード(Action / Condition) のプログラムを作成します。
まずは上記xmlのActionノード<Action ID="MoveRobot" linear_x="{x}" sleep_mtime="100"/>に該当するmove_robot_action.hppを記述します。
//-----------------------------------------------------------------------------------
// MIT License
// 略 GitHub参照
//-----------------------------------------------------------------------------------
#include <rclcpp/executors.hpp>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int32.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <behaviortree_cpp_v3/basic_types.h>
#include <behaviortree_cpp_v3/tree_node.h>
#include <chrono>
#include <cmath>
#include <memory>
#include <string>
#include <iostream>
using namespace std::chrono_literals;
using std::chrono::milliseconds;
using std::placeholders::_1;
using namespace BT;
//-------------------------------------------------------------------------------------
//--------------------------Class MOVE_ROBOT-------------------------------------------
//-------------------------------------------------------------------------------------
class MoveRobot : public BT::SyncActionNode, public rclcpp::Node {
private:
void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr _msg) {
tf2::Quaternion quat_tf;
geometry_msgs::msg::Quaternion quat_msg = _msg->pose.pose.orientation;
tf2::fromMsg(quat_msg, quat_tf);
double roll{}, pitch{}, yaw{};
tf2::Matrix3x3 m(quat_tf);
m.getRPY(roll, pitch, yaw);
// Left ＋, Right −
float yaw_check = yaw * 180 / M_PI;
float robotAngle = yaw_check;
RCLCPP_INFO(this->get_logger(), "yaw: '%f'", robotAngle);
RCLCPP_INFO(this->get_logger(), "position: '%f' '%f'",
_msg->pose.pose.position.x, _msg->pose.pose.position.y);
}
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscription_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
public:
MoveRobot(const std::string &name, const BT::NodeConfiguration &config)
: BT::SyncActionNode(name, config), Node("move_node") {
auto sensor_qos = rclcpp::QoS(rclcpp::SensorDataQoS());
subscription_ = this->create_subscription<nav_msgs::msg::Odometry>(
"/odom", sensor_qos, [&](const nav_msgs::msg::Odometry::SharedPtr msg) {
odometry_callback(msg);
});
}
NodeStatus tick() override {
auto res_time = getInput<int>("sleep_mtime");
if (!res_time) {
throw RuntimeError("error reading port [sleep_mtime]:", res_time.error());
}
int sleep_mtime = res_time.value();
publisher_ =
this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
auto message = geometry_msgs::msg::Twist();
message.linear.x = 0.2;
setOutput("linear_x", float(message.linear.x));
publisher_->publish(message);
std::this_thread::sleep_for(std::chrono::milliseconds(sleep_mtime));
return NodeStatus::SUCCESS;
}
static PortsList providedPorts() {
const char *description = "Simply print the target on console...";
return {
InputPort<int>("sleep_mtime", description),
OutputPort<float>("linear_x", description)
};
}
};
前述したように、BTではノード実行時、指定周波数でtickと呼ばれる信号を生成、伝搬するためNodeStatus tick() override {}内でROS 2のトピック/cmd_velをPublishするようにしています。
コンソールログ出力用では、yawとpositionを表示できるようvoid odometry_callback(const nav_msgs::msg::Odometry::SharedPtr _msg) {}を記述しています。
また、linear.xの値はBlackBoardを利用しており、イメージとしては以下のようなDiagramになります。
BlackBoardへの登録はsetOutputを、入力はInputPortを、出力はOutputPortを使用します。
ROS 2を使用しているため、ros2 parameterとして宣言/変更できるようにしておくと便利だと思います。（TBD）
他のSequence / Action / Conditio ノードも上記と同様の形で記述していけば良いです。
2.3: BTのノード登録 / コンソール出力 / ログ出力
続いて、作成した各ノードの登録やROS 2との連携、コンソール、ログ出力を行うためのプログラムを記述します。
ノード登録 / ROS 2連携
各ノードの登録はBehaviorTreeFactoryという記述を使用し、xmlで指定したIDへカスタムノードを登録することができます。
//Node registration process
factory.registerNodeType<MoveRobot>("MoveRobot");
factory.registerNodeType<CameraDetected>("CameraDetected");
factory.registerNodeType<ReadingLaser>("ReadingLaser");
factory.registerNodeType<Rotating>("Rotating");
factory.registerNodeType<Stop>("Stop");
ROS 2との連携はPublisherZMQ publisher_zmqを利用します。
// PublisherZMQ(const BT::Tree& tree, unsigned max_msg_per_second = 25,
// unsigned publisher_port = 1666, unsigned server_port = 1667);
unsigned publisher_port = 1666;
unsigned server_port = 1667;
BT::PublisherZMQ publisher_zmq(tree, 25, publisher_port, server_port);
Grootでの描画でportを変更したい場合は上記のunsigned server_port = 1667;を任意の値へ変更します。
コンソールログ出力
コンソールログの出力はStdCoutLogger logger_cout(tree)を使用することで、以下のようにターミナルへコンソール出力できます。
ログ出力と可視化
ログ出力とログファイルの可視化をするには、以下のようにfblファイルを記録する記述を追加します。
// for logging purposes. Details later
FileLogger logger_file(tree, "src/tb3_behavior_tree/log/tb3_bts_trace.fbl");
プログラム終了後、指定したパスへfblファイルが作成されているので、Groot起動 > Log Replay > Start > Load Log > .xmlを選択肢Open > 再生ボタンをクリックして、ログリプレイでBehavior Treeの動きを可視化することもできます。
2.4: BTパッケージの実行
Behavior TreeからROS 2連携の全体像の説明は、以上のようになります。
あとは、Turtlebot3の/cmd_velや/scanトピックを使用して、動かしてみます。
今回は簡単のため、/scanデータから最小値を読み、左に障害物があれば右旋回を、右側に障害物があれば左旋回をするよう記述しています。単なるルールベースです。
ターミナルを3つ起動し、以下を実行して動作確認してみてください。
cd ~/BehaviorTree/Groot/build/
./Groot
cd ~/turtlebot3_ws/
. install/setup.bash
ros2 launch turtlebot3_gazebo turtlebot3_jp_world_empty.launch.py
cd ~/dev_ws/
. install/setup.bash
ros2 run tb3_behavior_tree tb3_behavior_node
以下のように、試すことができたらこの記事の目標は達成になります。
この記事へのLike、Gitリポジトリへのスターいただけると大変励みになりますので、よろしくお願いいたします。
以上
Discussion