ロボット群制御：実践編 ～ ROS/ROS 2混在環境でOpen-RMFを使って複数のturtlebot3を動かしてみた

はじめに

前回の投稿で、Free Fleetを使って、複数のTurtelbot3を動かすSimulation環境を構築しました。
今回は、下の図にあるようにFree Fleet ServerとOpen-RMFを接続して、TerminalからOpen-RMFタスク（以下、タスク）の指示を出してみます。

環境

・OS : Ubuntu20.04
・ROS Version : ROS → Noetic, ROS 2→Foxy

環境構築

下記URLにあるOffice Worldを参考に構築しました。
Gazebo環境は構築してあるので、Traffic Editorを使ったOpen-RMFの地図生成やタスクの実行の部分を参考にしました。
https://github.com/open-rmf/rmf_demos/tree/foxy#Office-World

Simulation環境のハマりポイント

今回もハマりポイントはありましたが、サマリーを記載します。

バッテリー状態の更新

バッテリの状態（％）をOpen-RMFに通知する必要があります。
前回の環境では、バッテリの状態が通知されませんので、Gazeboのbattery pluginをインストールしました。 インストールすると、バッテリの状態が通知されるようになります。
しかし、Open-RMFの満充電は1.0で、Gazebo Battery Pluginは100とバッテリーの単位が違います。今回は、簡単な動作確認が主なので、ソースコードを修正して強引にOpen-RMFの単位に合わせました。

ROS 2/ROS間のROS Time同期

「use_sim_time」オプションを使うので、/clockのトピックベースのROS Timeが使われます。
今回の構成では、/clockはROS側のgazebo_ros packageが出力します。ROS 2側でもROS::Timeを使っているので、/clockをROS 2に伝える必要があります。伝える方法ですが、ros1_brigeを使いました。

経路設定

Open-RMFで使用する経路データを作成する必要があります。
Turtelbot3の開発環境に付いてくる「house」ワールドベースにTraffic Editorで作成します。
Traffic Editorで編集した地図は、グラフィック座標系なのでROSの地図座標に変換する必要があります。「rmf_demos_map」パッケージに追加するとcolcon build時にフォルダーを検索して、自動で変換するようになっています。今回は、これを利用します。

RMF経路データ作成

最初に、SLAMの地図データをpngに変換します。
私は、pgm→png変換は、Visual Studio Codeのpluginで変換しました。

SLAM地図データの場所

~/Work/open-rmf/free_fleet/ros1/src/turtlebot3_simulations/turtlebot3_gazebo/maps/house.pgm

pgmファイルをpngファイルに変換して、以下のフォルダーに置きます。

保存フォルダー作成＆pngファイルコピー

mkdir ~/Work/open-rmf/free_fleet/ros2/src/demonstrations/rmf_demos/rmf_demos_maps/maps/house
cp house.png ~/Work/open-rmf/free_fleet/ros2/src/demonstrations/rmf_demos/rmf_demos_maps/maps/house

Traffic Editorによる経路設定

次にTraffic Editorを使ってで経路を設定します。

Traffic Editor起動手順

cd ~/Work/open-rmf/free_fleet/ros2
source install/setup.bash
traffic-editor

さきほど、コピーしたpngファイルを読み込ませます。
使い方は、公式に詳しく記載してありますので、そちらを参考にして下さい。

https://osrf.github.io/ros2multirobotbook/traffic-editor.html

重要なのがLevel Nameで、ここではOpen-RMF Demoに合わせて「L1」にしています。
作成した地図情報は、以下の場所に保存します。

地図保存フォルダー

~/Work/open-rmf/free_fleet/ros2/src/demonstrations/rmf_demos/rmf_demos_maps/maps/house

保存名を「house」にすると「house.building.yaml」ファイルが出力されます。
また、他のフォルダーのoffice.project.yaml参考にhouse.project.yamlを作成しました。

最後に、colcon buildすると経路設定完了です。

build

cd ~/Work/open-rmf/free_fleet/ros2
source install/setup.bash
colcon build --symlink-install

上手くビルドできない場合は、「colcon build --cmake-clean-cache」してから再度ビルドして下さい。

車両スペック

車両スペックは、tinyRobot設定ファイルをベースに変更します。

車両スペック設定ファイル作成

cd ~/Work/open-rmf/free_fleet/ros2/src/demonstrations/rmf_demos/rmf_demos/launch/include/adapters
cp tinyRobot_adapter.launch.xml turtelbot3_adapter.launch.xml

スペックは、Turtelbot3の公式HPを参考に設定しました。

Fleet Adapterと接続

Fleet Name = "tb3"、Robot Preifx = "tb3_"としています。

Open-RMFとFleet Adapterの起動用launchファイルを準備します。
下記の様に、office.launch.xmlをhouse.launch.xmlにコピーして、パラメータを変更します。

Open-RMF起動ファイル作成

cd ~/Work/open-rmf/free_fleet/ros2/src/demonstrations/rmf_demos/rmf_demos/launch
cp office.launch.xml house.launch.xml

「freet_name」と「robot_prefix」と「車両スペックファイル」を設定に合わせて変更します。
ファイルを作成しましたので、colcon buildを実行します。

実行手順

Free Fleet関連のモジュールを起動します。
4つTerminalを開き、順番に起動していきます。

terminal(1:ROS turtelbot3 simulation/Free Fleet Client起動)

cd ~/Work/open-rmf/free_fleet/ros1
source install/setup.bash
export TURTLEBOT3_MODEL=burger; roslaunch ff_examples_ros1 multi_turtlebot3_ff.launch

terminal(2:ROS 2 Free Fleet Server起動)

cd ~/Work/open-rmf/free_fleet/ros2
source install/setup.bash
ros2 launch ff_examples_ros2 turtlebot3_world_ff_server.launch.xml

terminal(3:ROS 2 ros1_bridge起動)

cd ~/Work/open-rmf/free_fleet/ros1_bridge_ws
source /opt/ros/noetic/setup.bash
source /opt/ros/foxy/setup.bash
source install/setup.bash
ros2 run ros1_bridge dynamic_bridge

terminal(4:ROS2 Open-RMF起動)

cd ~/Work/open-rmf/free_fleet/ros2
source install/setup.bash
ros2 launch rmf_demos house.launch.xml

タスク実行

Terminalから以下のコマンドを実行すると、ロボットが動き出します。

terminal(5:ROS2 ros1_bridge起動)

cd ~/Work/open-rmf/free_fleet/ros2
source install/setup.bash
ros2 run rmf_demos_tasks dispatch_loop -s goal0 -f goal1 --use_sim_time
ros2 run rmf_demos_tasks dispatch_loop -s goal1 -f goal2 --use_sim_time
ros2 run rmf_demos_tasks dispatch_loop -s goal2 -f goal0 --use_sim_time

上の右側の図について、ピンク色の印がロボットの現在位置で、黄色がOpen-RMF側で計画している位置になります。
左側の図にある赤色の矢印が、各ロボットがどこをGoalにしているかを示しています。

今回は、簡単な「Loop」タスクを実行しました。
タスクの実行は場所の指定だけで、Open-RMFが移動させるロボットを選択して目的地までアシストします。
経路が重なってもOpen-RMF側でどちらを優先させるかを判断します。 判断のアルゴリズムは、「greedy algorithm」などが使われているようです。

ただし、黄色の印が先行しすぎて、ピンクの印との距離が離れすぎてエラーになることがありました。
Open-RMFの経路計画は、リアル時間で計算されています。ロボット側はSimulation時間で計算されているので、PCが重くなるとSimulation時間が遅くなり、リアル時間が先行してしまうのが原因の１つではないかと推測しています。

まとめ

SLAMで作った地図をOpen-RMFに適応させてタスクを実行させることができました。

Free FleetのOpen-RMFへの適応のさせ方も理解できました。
次は、複数Fleetを使って種類が違うロボットを混在させて制御してみます。

環境構築でいろいろハマってしまったのですが、いろいろ対策していく中でOpen-RMFの理解が深まりました。実際に動かして自力で考えることが理解への近道ということを再認識しました。

応用編へつづく