VII. ROSによるAMRのシミュレーション（３）
5. ROS2への移植
「2.3 ROS2で5台のドライブ」で予告した通り、本節では「ドライブする4台の中で1台がナビ」がROS2でもちゃんと動くようになった経緯を述べます。まずは完成したプログラムを動作させた時のキャプチャ動画を見ていただきます。4節冒頭でお見せしたROS1と比較すると、RVizの印象がかなり違っています。ROS1では障害物のLiDAR像を示していたものが、ROS2ではコストマップの侵入禁止領域が障害物の外側に広く表示されます。また、ROS1では選択された局所経路がチョロチョロ動く短い線で示されていましたが、ROS2では局所経路候補がtb3_0からたくさん出ています。黄緑色が選択可能な局所経路になります。
ROS2では元々ダラダラバックしませんが、TB3同士が接触して跳ね飛ばされる事故もなくなりました。画面が2回スキップした後の2分18秒から2分42秒までtb3_0は動けませんが、これは衝突モニタが作動したためです。その後に発動する回復行動で後退すると、また移動を開始します。この衝突モニタと回復行動がROS1からの大きな変更点になっています。全ての行程時間が100秒以下ですが、回復行動の待ち時間があって平均行程時間は43.3秒とROS1よりも長くなっています。ソースコードはTARでダウンロードできるので、Ubuntu 22.04にHumbleをインストールしてあれば、以下のコマンドを打つだけで上記のキャプチャ動画を再現できます。
$ cd $ mkdir ~/ros2_ws $ cd ~/ros2_ws --- Move the downloaded file "ros2_src.tar.gz" here (~/ros2_ws). --- $ tar -xvf ros2_src.tar.gz $ colcon build --symlink-install --- Wait until "Summary: 57 packages finished" appears. --- --- Ignore "1 package had stderr output" --- $ cd $ source .bashrc $ source /usr/share/gazebo/setup.sh $ ros2 launch mtb3_sim navi_drive9.launch.py
以下のファイルツリーは、完成したプログラムで使っているソースコードの依存関係を示しています。ただし、駄待ち狐が新たに作成（赤字）あるいは修正（青字）したファイルのみを載せていて、TB3パッケージやnavigation2（Nav2）スタックに元からあって変更していないファイルは省略してあります。[ ]内はTARを解凍したsrcフォルダ内のパスです。空白行で5つのブロックに区切っていますが、上から全体のローンチとロボットモデル、ドライブノード、ナビノード、回復行動、衝突モニタです。以下、この順番に従ってコードの説明をしていきます。
[mtb3_sim/launch/] navi_drive9.launch.py
⇒ [mtb3_sim/launch/] spawn_turtlebot3_ns8.launch.py
(w/ [mtb3_sim/models/turtlebot3_waffle_pi4/] model.sdf)
⇒ [mtb3_sim/launch/] turtlebot3_drive_ns12.launch.py
⇒ [mtb3_py/mtb3_py/] tb3_drive_5.py
⇒ [mtb3_py/mtb3_py/] base_drive_5.py
⇒ [mtb3_py/mtb3_py/] mtb3_navi_2.py
⇒ [navigation2/nav2_simple_commander/nav2_simple_commander/]
robot_navigator.py
⇒ [navigation2/nav2_bringup/launch/] bringup_launch_2.py
(w/ [turtlebot3_navigation2/param/] waffle_pi4.yaml)
⇒ [navigation2/nav2_bringup/launch/] navigation_launch_2.py
⇒ [navigation2/nav2_bt_navigator/src/] bt_navigator.cpp
(w/ [navigation2/nav2_bt_navigator/behavior_trees/]
navigate_to_pose_w_replanning_and_recovery.xml)
⇒ [navigation2/nav2_collision_monitor/launch/]
collision_monitor_node.launch.py
(w/ [navigation2/nav2_collision_monitor/params/]
collision_monitor_params.yaml)
5.1 ローンチファイルとロボットモデル
上記プログラム全体のローンチファイルはツリー最上位のnavi_drive9.launch.pyです。ROS1のローンチファイルはXML形式ですが、ROS2ではPythonの書式で書かれています。*.launch.pyの実体はgenerate_launch_description()関数で、LaunchDescription()の引数は処理内容を要素とするPythonのリストです。この要素には、ノードを実行するNode()、他のローンチファイルを取込むIncludeLaunchDescription()、自身の引数を宣言するDeclareLaunchArgument()などがあります。"return LaunchDescription([...])"としてリスト内に処理を書き連ねるやり方もありますが、navi_drive9.launch.pyにある書き方が一般的です。
まず、22～96行目でリストの要素に当る処理を*_cmdとして定義します。次に98行目で"ld = LaunchDescription()"とした上で、99～107行目で*_cmdをすべてldに合体します。108行目で"return ld"とすれば、処理を全て含んだ LaunchDescription()を返せます。13～19行目はIncludeLaunchDescription()で使うパッケージ名を指定しており、20行目は引数の宣言で75行目でリストに加えられます。ファイルツリーでインデントなしの"⇒"はspawn_turtlebot_cmd、tb3_drive_cmd、tb3_navi_cmd、nav2_bringup_cmd、collision_monitor_cmdに対応しており、それ以外の*_cmdはツリーでは省略されています。
ファイルツリー2行目のspawn_turtlebot3_ns8.launch.pyは5台のTB3を出現させるだけのローンチファイルで、例えば24～34行目のNode()でtb3_0を出現させます。gazebo_rosパッケージのspawn_entity.pyでノードを生成するのですが、gazebo_rosはROS2と一緒にインストールされるので、spawn_entity.pyのソースコードは/opt/ros/humble/lib/gazebo_rosにあります。argumentsとしてロボット名'-entity'、ロボットモデルファイル'-file'、初期姿勢を指定します。tb3_1～4についての同様の記載が続き、90～97行目で全ノードを合体して返します。
簡単な話のように見えますが、5台のTB_3を識別可能とするために非常に苦労しました。1つ目のポイントは、tb3_1～4についてはargumentsに'-robot_namespace'を指定するということです。これはROS1のXMLローンチファイルでもやったことですが、Node()の中にこのような書式で書けばいいというのがなかなか分りませんでした。2つ目のポイントは'-file'です。よく見ていただくと、tb3_0のファイル名はmodel.sdfなのに対し、tb3_1～4はmodel_1.sdf1になっています。どちらも/src/mtb3_sim/models/turtlebot3_waffle_pi4にあるので見比べてみて下さい。
「全く同じでしょ！」と思われるかもしれませんが、違いは706行目と707行目です。model.sdfではodom、base_footprintとなっていますが、tb3_1～4ではtb3/odom、tb3/base_footprintです。この変更をする前は、RViz上に全てのTB3がフラッシュ表示されるという問題が発生していました。'-robot_namespace'を指定しているんだから、自動的にtb3_1/odomとかになって欲しいものだと思いますが、そうはならないようです。tb3_0だけを「名なしの権兵衛」として区別すればいいので、tb3_1～4に付ける接頭辞は全てtbにしても問題ありません。
model.sdfはフォルダ名にwaffle_pi4とある通り、ROS1のwaffle_pi3とは違います。waffle_pi4では首側のscan3と尾側のscan4だけでなく、頂上のscanも復活しています。ROS1ではscan3の前半分の領域のスキャンだけでAMCLが問題なく動作したのですが、ROS2では誤差がどんどん大きくなります。これは前半分のスキャンであることが原因ではなく、他のTB3を検知することによるもののようです。そこでscanを復活させたという訳です。シミュレーションでLiDARを増やすのは簡単ですが、実機であれば原価構成の中でLiDARは大きな割合を占めており、3台のLiDARを搭載するというのはとんでもない贅沢です。
5.2 ドライブノード
ファイルツリーの2番目のブロックはtb3_1～4のドライブを実行するもので、「4.5 ナビ関連ノード」の最後に示したROS1のツリーの上の行に対応します。後者ではドライブノードの生成はnavi_drive3.launchに含めていましたが、前者ではturtlebot3_drive_ns12.launch.pyとして独立させています。また、ROS1ではドライブノードtb3_drive_18が/src/mtb3_sim/src内にありましたが、ROS2ではtb3_drive_5.pyを/src/mtb3_py/mtb3_pyフォルダ内に置いています。ROS1ではC++のソースコードと同じフィルダに入れたPythonファイルから識別子.pyを除けばそのままノードになりましたが、ROS2ではもう一手間必要です。
ROS２でもC++のソースコードはパッケージ内のCMakeLists.txtとpackage.xmlによってビルドされますが、Pythonファイルはパッケージ内のsetup.pyとpackage.xmlによってビルドされます。このためフォルダをmtb3_simとmtb3_pyに分ける必要があるのです。ビルドされると言っても~/ros2_ws/install/mtb3_py/lib/mtb3_pyフォルダ内に新たに生成されるtb3_drive_5ノードはentry_pointとしてtb3_drive_5.pyを指定しているだけです。mtb3_simにソースコードとして残っているのは今は使っていないtb3_drive.cppだけなので、これを削除してmtb_simフォルダをPythonパッケージとした方がすっきりしたかもしれません。
ROS2のtb3_drive_5.py、base_drive_5.pyは、ROS1のtb3_drive_18、base_drive_18.pyと同じことをやっていますが、ROS1とROS2の違いでコードの書換えが必要になります。一番大きな違いはROS1のrospyの代りにROS2ではrclpyを使うという点です。単に名前が変ったということではなく、建付けが違います。base_drive_18.pyの17～19行目では、rospyのメソッドでパブリッシャとサブスクライバを生成していますが、rclpyにはそんなメソッドはないので単純に置換えただけではエラーになります。create_publisher()メソッドとcreate_subscription()メソッドがあるのは、rclpy.node.Nodeクラスです。
そこでまず、baseDriveクラスがNodeクラスを継承して、22～24行目のself.create_...でパブリッシャとサブスクライバを生成します。ノード自体の生成についても、ROS1ではtb3_drive_18の9行目にあるrospy.init_node('drive')でdriveという名前でノードを初期化しますが、ROS2のtb3_drive_5.pyでは10行目のrclpy.init()にはノード名の指定がなく、base_drive_5.pyの13行目でNodeクラスの__init__()を実行する際にノード名driveを指定しています。ここでは出てきませんが、rospy.Timeとrospy.Durationはself.get_clockとrclpy.duration.Durationに置換え、rospy.loginfoはself.get_logger().infoに置換えます。
5.3 ナビノード
ROS1ではnavigationスタックのmove_baseノードがナビの要でしたが、ROS2のNav2スタックは下図に示す構成になっており、move_baseはありません。move_baseはグローバルプランナーとローカルプランナーを使って経路計画を行い、経路が設定できなかった時に回復行動を取ると説明しましたが、Planner Serverがグローバルプランナー、Controller Serverがローカルプランナーに相当し、回復行動を取るのはBehavior Serverです。これらのサーバのコーディネートをするのがBT Navigator Serverなので、move_baseの機能がBT Navigator ServerとBehavior Serverに分けられたと言えます。
「Nav2」サイトから引用
ファイルツリーの3番目のブロックで、冒頭のmtb3_navi_2.pyはROS1のmtb3_navi_2をROS2に移植したものです。この移植における最大の変更点は、設定した目標姿勢をどのように送信するかです。ROS1のmtb3_navi_2では'move_base_simple/goal'型のメッセージをパブリッシュして、move_baseに向けて目標姿勢を伝えています。一方、ROS2ではBT Navigator Serverに目標姿勢を送信するのに3番目のブロックの2行目にあるrobot_navigator.pyを使います。ROS2のmtb3_navi_2では、21行目でrobot_navigator.pyにあるBasicNavigator()クラスのインスタンスnaviを生成し、50行目でgoToPose()メソッドを実行しています。
このメソッドはrobot_navigator.pyの138～161行目にあって、68行目で宣言したアクションクライアントself.nav_to_pose_clientが150行目でgoal_msgを送信するアクション通信を行います。ROS1ではパブリッシャとして一方的に発信するトピック通信だったものが、アクションクライアントとしてアクションサーバの応答を待つアクション通信に変っているのです。アクションクライアントはアクションサーバにタスクを送信した後は進行状況を監視し、タスクの完了を待ちます。これをさらに監視しているのがROS2のmtb3_navi_2の53～59行目です。
navi.isTaskComplete()がTrueになればwhileループを終了しますが、それ以外に"if navi.status == 6:"でも終了します。これはタスクが失敗した時の処理になりますが、これについては「5.4 回復行動」で説明します。一方、元のrobot_navigator.pyにはバグがあって、タスクの途中なのにisTaskComplete()がTrueになることがありました。今のプログラムの290～292行目のFalseを返すelse節がなくて、293～297行目のelse節を抜けた後は無条件にTrueを返すとなっていたのです。これでは、新たに追加したelse節の部分がTrueを返すのと同じことで、ゴールしてないのにゴールしたことになってしまいます。
ファイルツリーでこの後にあるbringup_launch_2.pyとnavigation_launch_2.pyはNav2スタックの全ノードを起動します。bringup_launch_2.pyはnavigation_launch_2.pyを実行するように修正しただけで、パラメータとしてwaffle_pi4.yamlを使うことはnavi_drive9.launch.pyの64行目に記載されています。waffle_pi4.yamlではlocal_costmapとglobal_costmapでscan以外にscan3、scan4も使うという点に加え、元のwaffle_pi.yamlにあったrecoveries_server:が正しくはbehavior_server:であるという修正もしています。navigation_launch_2.pyの修正は衝突モニタを動作させるためで、「5.5 衝突モニタ」で説明します。
5.4 回復行動
Nav2の回復行動にはwait、back_up、spinの3種類があります。waitは文字通り待機するのですが、back_upは何かをバックアップするということではなくロボットが後退するという意味です。spinもコールバック関数を呼び続けるというspin()ではなく、ロボットが旋回するという意味です。360°左旋回しか芸がなかったROS1に比べると、大きな進歩です。これらの回復行動のソースコードは/src/navigation2/nav2_behaviors/pluginsにあるC++ファイルです。wait.cppでは待機時間、back_up.cppでは後退距離と後退速度、spin.cppでは旋回角度を指定します。
回復行動の手順を決めているのは、ファイルツリーの4番目のブロックにあるbt_navigator.cppが使うnavigate_to_pose_w_replanning_and_recovery.xmlです。navigator.cppは自律型エージェントの挙動を構造化するBehaviorTree（BT）という汎用パッケージに依拠しており、XMLファイルにはBTのルールに従って経路探索を含めた挙動ツリー（behavior tree）が記載されています。ros2_src.tar.gzに含まれるnavigate_..._recovery.xmlは修正後のものになりますが、修正前のファイルとその解説は「Detailed Behavior Tree Walkthrough」にあります。
簡単に言うと、大域経路が見つけられなければ大域コストマップをクリアし、局所経路が見つけられなければ局所コストマップをクリアしますが、どちらかで失敗して経路設定ができないということが6回続いたらRecoveryFallbackに移ります。RecoveryFallbackでは、両方のコストマップのクリア、90°左旋回、5秒待機、0.15m後退を順番に一つ試しては経路探索に戻り、それでダメだったら次を試すというRoundRobinを実行します。この挙動ツリーは、tb3_0以外のTB3が移動障害物として経路設定の邪魔をするという状況に対してはあまり効率的ではありません。
そこでnavigate_..._recovery.xmlを修正した訳ですが、まず12～18行目で大域経路を見つけられない時は2秒待ってから大域コストマップをクリアします。他のTB3が大域経路を隠している場合が多いからです。他のTB3がすぐにどいてくれない場合もあるので3回までトライします。局所経路が見つけられない時は従来と同じで局所コストマップをクリアするだけです。どちらかで失敗して経路設定ができないということが6回続いたらRecoveryFallbackに移るという部分は、3回に減らしました。2秒×3回×6回では36秒のタイムロスになり、長過ぎるからです。RoundRobinは両方のコストマップのクリアと0.2m後退だけにしました。
待機は十分にした後だし、残る旋回はこの後の最後の手段にしました。「5.3 ナビノード」に出てきたmtb3_navi_2.pyでタスクが失敗した時の処理になります。RoundRobinが全て失敗すると、アクションサーバは"navi.status == 6:"を返します。この場合はmtb3_navi_2.pyの57行目となり、0.78rad（45°）左旋回して、再度アクションサーバに同じタスクを送信します。検討の途中ではRoundRobinに45°右旋回を入れてみたりしましたが、右旋回と左旋回で元に戻るみたいなことになったので、今の形に落着きました。IX章の最後にはRoundRobinではなく深層強化学習で後退、左旋回、右旋回を選択するドライブが登場します。
5.5 衝突モニタ
ドライブでは前進・旋回時に常に他のTB3との接触をモニタしていますが、ナビでは局所経路が見つかった後は決定した経路を進むだけです。この段階で他のTB3と接触しても対応はできません。そこで登場するのがNav2スタックの構成図にあるCollision Monitor（衝突モニタ）です。衝突モニタは速度指令（cmd_vel）を出す直前に作動し、近くに障害物があると減速もしくは停止するようにcmd_velを書換えます。衝突モニタを使うためには、まずnavi_drive9.launch.pyでcollision_monitor_node.launch.pyを実行します。ファイルツリーでは5番目のブロックになりますが、実際には*_cmdの最初に実行しないとエラーが出ます。
元のNav2ファイルは衝突モニタを使う設定にはなっておらず、これも修正する必要があります。navigation_launch_2.pyの173～183行目でvelocity_smootherノードを起動する際に182行目にあるremappings指定から('cmd_vel_smoothed', 'cmd_vel')を削除します。これでVelocity Smootherが直接cmd_velを出力することはなくなります。構成図の通りcmd_vel_smoothedを衝突モニタの入力とするため、collision_monitor_params.yamlの6行目のcmd_vel_in_topicを"cmd_vel_raw"から"cmd_vel_smoothed"に変更します。これで衝突モニタが動作するようになります。
collision_monitor_params.yamlでは、停止エリアと減速エリアの設定もしています。20行目もしくは27行目にある8要素の配列は2つずつ組になって4点の座標を示しており、この4点に囲まれたpolygonが停止・減速エリアとなります。当然のことながら減速エリアは停止エリアよりも広く設定します。ロボットの中心から見てこの範囲内に障害物があると、有無を言わさず停止・減速します。元のエリア設定はかなり広めに取られていたので、なるべく自由に移動できるよう衝突を回避できるギリギリの大きさまでエリアを狭くしました。衝突モニタの導入でROS2最大の課題であったTB3同士の接触による跳ね飛び事故は完全になくなりました。
ROSによるAMRのシミュレーションは、面白そうだからという発想でスタートし、自分で勝手に設定した「ドライブする4台の中で1台がナビ」という課題に取組んできました。最初はROS1の出来合いのソースコードにローンチファイルと目標姿勢設定プログラムを追加するというだけでしたが、そのソースコードをいじり、ROS2に移植してさらに深みにはまりながら何とか目標は達成できたものと思います。この間、実働484日を要しました。在職中は「何か仕事の役に立つかも」との下心もありましたが、退職後は純粋に趣味としての楽しみです。
ですから、この話に関しては「駄待ち狐あるある」はありません。ただ、「趣味とはいえ」というか「趣味だからこそ」人様にお見せしたいという思いはありました。実はそれこそがこの駄待ち狐のサイトを始めた理由です。この趣味の成果を広く世の中に知らしめたいという思いは、どうやって知らしめたらいいかという難問に阻まれていました。そこで、「駄待ち狐こと松田賢一が作成した色々なプログラムを紹介します」というサイトを立上げ、そのトリとしてROSを取上げることにしたのです。
「駄待ち狐のサイトはまだ続くんでしょ？」と思っていただければ光栄です。実はROSをいじり出した当初から、さらに大きな目標として深層（強化）学習をROSに取込むという目論見がありました。本章ではこの件について全く触れていませんが、ローンチファイルと目標姿勢設定プログラムを追加しただけだった当時から、このチャレンジは始めていました。この後のVIII章で紹介するAnyLogicでも同様の思いがあり、最終的にはROS2と深層強化学習の融合に結実します。ROSとAnyLogicの深層（強化）学習に関する話は全てIX章のお楽しみです。