VII. ROSによるAMRのシミュレーション（２）
4. ROS1パッケージの修正
本節では「2.2 ドライブする4台の中で1台がナビ」で述べた3大問題、①ダラダラバック、②TB3同士のおしくらまんじゅう、③他のTB3のゴーストがゴールを隠して完全に停止を始めとする問題点を解決するため、ROS1/TB3パッケージのソースコードを改良していった経緯を述べます。まずは完成したプログラムを動作させた時のキャプチャ動画をお見せします。2.2節では説明しませんでしたが、RVizウィンドウ上でtb3_0からゴールに向って伸びる線が大域（グローバル）経路、tb3_0から出ているチョロチョロ動く短い線が局所（ローカル）経路です。今回は各行程の所要時間が左上にグラフで表示されるようになっています。
ダラダラバックはせず、TB3同士の軽い接触はありますが押し合うということはありません。1分8秒後に最初のゴールをする時も、無理に割込まずにゴール近辺が空くのを待っています。１分22秒後に19回目のゴールをしてからの帰路に画面がスキップし、20回目のゴールをした1分54秒後にゴーストによるゴール隠しが発生います。しかし、12秒経つとゴーストは消えて再出発です。2分9秒後に画面がまたスキップしますが、グラフを見ると48回目のゴールに200秒以上を要していることが分かります。この200秒は何かと言うと、2分37秒後にスキップする77回目のゴール後の帰路で同じ現象が発生します。
他のTB3に大域経路を塞がれてしまった結果、通れるはずのない隙間を大域経路として選び、ウロウロ彷徨います。4分16秒後に何とか正しい大域経路を見つけますが、78回目のゴールには350秒を要することになります。もう少し何とかできないのという気はしますが、基本的には他のTB3に大域経路を塞がれていることが原因です。長時間邪魔され続けますが、意地悪なTB3が去るとtb3_0はちゃんと走行を再開するので「運が悪かった」ということです。運が良かった場合は以下のようになり、全ての行程時間が100秒以下で平均行程時間は37.5秒です。
以上のROS1プログラムのソースコードはTARで丸ごとダウンロードできます。Ubuntu 20.04でNoeticをインストールしてあるか、22.04で「JammyでもNoeticしたい」をしてあるのが前提ですが、以下のコマンドを打つだけで上記のキャプチャ動画は再現できます。ただし、いつも同じ動きをするという訳ではなく、ドライブで動くTB3の軌跡は毎回変ります。それで運の良し悪しという話になります。ソースコードの修正箇所は多岐に渡るので細大漏さずというのは難しいですが、主だったものについて順次説明していきます。
$ cd $ mkdir -p ~/catkin_ws/src $ cd ~/catkin_ws/src $ catkin_init_workspace $ cd ~/catkin_ws $ catkin_make --- Move the downloaded file "ros1_src.tar.gz" here (~/catkin_ws). --- $ tar -xvf ros1_src.tar.gz $ catkin_make --- Wait until "[100%] Built target move_base_node" appears. --- $ cd $ source .bashrc $ roslaunch mtb3_sim navi_drive3.launch
4.1 move_baseノード
move_baseノードはnavigationスタック内のmove_baseパッケージに含まれており、ソースコードはTARを解凍した中にある/src/navigation/move_base/src/move_base.cppです。上記コマンドを実行していれば、~/catkin_ws以下に配置されています。このmove_base.cppは駄待ち狐が修正したものであり、元のコードからの修正箇所は//--- DELETE ---と//--- INSERT ---というコメントを入れて明記しています。ただし、このように丁寧な対応はmove_base.cpp以外のファイルを修正した際には行っていないので、悪しからずご了承下さい。
move_baseはナビの要となるノードで、amclノードから取得した自姿勢データ、costmap_2dノードが生成したコストマップ、受信した目標姿勢から、グローバルプランナーとローカルプランナーという別のノードを順番に使って経路計画を行い、速度指令cmd_velを出力します。経路計画自体はプランナーが行うので、move_base自身の役割は経路が設定できなかった時に回復行動（recovery action）を取ることです。回復行動が必要になるのは、大域経路を見つけられなかった時（PLANNING_R）、局所経路を見つけれれなかった時（CONTROLLING_R）、左右に旋回を繰返す振動時（OSCILLATION_R）とされています。
元のmove_baseではどの場合も回復行動は同じパターンで、move_base.cppの1150行目からのconservative_reset、1156行目からのrotate_recovery、1164行目からのaggressive_reset、1170行目からのrotate_recoveryを順番に試します。ところが実際にはrecovery_index_が増えて行かず、conservative_reset以外の回復行動を取りません。707行目と723～726行目を削除することでこの問題は解決しました。どこかの段階で問題が解消すれば、そこで回復行動は終りです。最後まで回復できないと1020～1031行目にあるようにROS_ERROR("Aborting ...");を出力して1032行目のresetState();を行います。
*_resetであれ、resetStateであれ、コストマップをクリアして障害物のゴーストは消えるはずですが、問題③発生時にはいつまでもゴーストが残ります。その原因はよく分りませんが、最後の手段として1034行目にdelete this;を入れました。これはノードが自分自身を破棄（destroy）するコマンドで、ローンチファイルでノードを起動する際にrespawn="true"としておくと、そのノードは自動的に再生成されます。move_baseノードの破棄-再生成をやるとさすがにゴーストは消えて、問題③は解決です。その前後でtb3_0の姿勢は不変なので、シミュレーションの継続性が失れることはありません。
上記以外にもmove_base.cppの修正点はあり、TB3のナビ関連パラメータを集めたフォルダである/src/turtlebot3/turtlebot3_navigation/paramの中のmove_base_params.yamlも修正していますが、細かい話なので割愛します。より大きな話として、rotate_recovery（旋回回復）があります。その動作は/src/navigation/rotate_recovery/src/rotate_recovery.cppで規定されていますが、元は左方向の360°旋回でした。4ステップの回復行動でリセット以外の動作は2回目と4回目の旋回回復だけなのに、これが何れも360°旋回というのはいただけません。それでは元の姿勢に戻るだけです。
他のTB3はその間も動き回るので、旋回している間に状況が変ることもありますが、それならじっと待っていれば済む話です。経路が見つけられないという状況を打破するのであれば、左あるいは右に90°旋回する方がはるかに合理的です。そこで、rotate_recovery.cppにrotate_left_という変数を導入し、trueであれば左旋回、falseであれば右旋回で、旋回後にrotate_left_を反転するようにしました。また、元のrotate_recovery.cppではgot_180がtrueになった後さらに元の向きになるまで旋回を続けるという建付けだったのをgot_90がtrueになったら回復行動を終了としました。
4.2 dwa_plannerノード
dwa_plannerノードはnavigationスタック内のdwa_local_plannerパッケージにあり、ソースコードは/src/navigation/dwa_local_planner/src/dwa_planner.cppです。move_baseが使うローカルプランナーで、局所経路を決定します。DWA（dynamic window approach）というのは、並進速度と角速度の組合せの中から選択可能なものを抽出し、大域経路に最も近いものを選ぶという手法です。このことはROSの教科書で勉強して知っていたのですが、move_base.cpp、そのROSラッパーであるmove_base_node.cpp、ヘッダファイルであるmove_base.hのどこを探してもdwa_plannerは出て来ません。
実はmove_baseのローンチファイル（改良後は/src/mtb3_sim/launch/navi_drive3.launch、元の建付けであれば/src/turtlebot3/turtlebot3_navigation/launch/move_base.launch）で、<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />と指定されています。「大事なことは上流で決める」という考え方なのだと思いますが、コードの細部を追いかけているとその大事なことが見つけられません。move_baseがdwa_plannerを使っていると確認できたので、次はdwa_plannerの中身の検討です。
dwa_plannerノードは軌跡（並進速度と角速度の組合せ）候補の中から最適のものを選択するのにまた別の2つのノードを使います。どちらも/src/navigation/base_local_planner/srcにソースコードがあり、simple_trajectory_generator.cppが物理的に実行可能な軌跡に対して設定された条件で候補を絞込み、simple_scored_sampling_planner.cppが候補の中から最適のものを選びます。その条件というのが並進速度と角速度それぞれの絶対値に対する最大値と最小値で、上述のTB3ナビ関連パラメータフォルダにあるdwa_local_planner_params_waffle_pi.yamlの中でmax_vel_trans他として規定されています。
ここで問題になるのが、min_vel_transの元の値0.13（m/s）です。dwa_..._pi.yamlには、加速度上限acc_lim_xが2.5（m/s2）、 制御周期controller_frequencyが10.0（Hz）とも規定されているので、1周期で速度を変えられるのは0.25m/sまでとなります。tb3_0が後退している時の速度は少なくとも-0.13m/sなので、前進に転じようとしても1周期後の速度は0.12m/s以下にしかなりません。そうすると、前進は全て並進速度の最小値未満なので軌跡候補から除外されてしまい、後退しかできなくなってしまうのです。これが問題①ダラダラバックの原因でした。min_vel_transを0.10にしたらダラダラバックはピタッと治りました。
これはもはやTB3パッケージのバグだとしか言いようがありません。「大事なことは上流で決める」で、dwa_..._pi.yamlのmin_vel_transを0.13から0.10に変更すればいいだけの話だったのですが、move_base、dwa_planner、simple_trajectory_generatorと辿っていかないとこの結論には達しません。いい加減にしてよと言いたいところですが、この経験を通じて学んだのは、複雑怪奇なROSパッケージのジャングルも一歩ずつ分け入っていけばゴールに辿り着けるということです。それが、この後さらに大きなコード改変をしていく上での自信になりました。
4.3 ロボットモデル
最後に残った3大問題が②おしくらまんじゅうです。これを解決しようと最初に取組んだのがドライブの改良です。ドライブで勝手気ままに走行するTB3が執拗に絡んでこなければ、tb3_0が困ることもないだろうと考えたのです。ドライブの要はturtlebot3_driveノードで、ソースコードは/src/turtlebot3_simulations/turtlebot3_gazebo/src/turtlebot3_drive.cppです。このコードはPythonに移植して全面的に改修し、最終的には/src/mtb3_sim/src/base_drive_18.pyになっています。これについては次項で説明しますが、その途中段階で得た結論がロボットモデルを変更しないとどうにもならないということです。
TB3はどのモデルでも頂上（平面視で真ん中の一番高いところ）にLiDARを搭載していますが、このLiDARは水平面内しかスキャンしません。その結果、他のTB3を検知できるのはそのLiDAR部のみになり、全体像は検出できないのです。それを前提に障害物を避ける距離を広く取るという考え方もありますが、そうすると静止障害物に対しても同じように距離を取ることになるため、楽々通れるはずの静止障害物の間を通過できなくなってしまいます。この問題を解決するためにはLiDARをロボット本体の高さに取付ける必要があります。それがキャプチャ動画に出てくる黄色い首（進行方向正面の突起）を持つWaffle Piです。
この黄色い部分がLiDARなので、他のTB3の全体像を捉えることができます。ただし全周360°を検知することはできないので、頂上のLiDARは残して首にもLiDARを備えたwaffle_pi2をまず考えました。ところがそれでは尾側の角の衝突を避けられないので、首と尾に2台のLiDARを備えたwaffle_pi3にしました。Gazebo上で前後が分るように尾のLiDARは黒のままにしています。ナビで動くtb3_0もwaffle_pi3に変更したので、キャプチャ動画のRVizで他のTB3が前より大きくなっています。TB3のロボットモデルは、/src/turtlebot3/turtlebot3_description/urdf内にあるturtlebot3_モデル名.urdf.xacroとturtlebot3_モデル名.gazebo.xacroです。
モデル名がwaffle_piのファイルは元のTB3パッケージのままで、これをコピーして修正することでwaffle_pi3のファイルを作成しています。urdf.xacroでは157～186行目がLiDARに関する記述で、160行目の<origin xyz=... />がLiDARの取付け位置になります。waffle_pi3では、x座標を大きくして前に出し、ｚ座標を小さくして高さを下げています。また、157～186行目と同じ構成の記述を188～223行目に追加して、尾側のLiDARを記述しています。両者を識別するため、首側はjoint名をscan_joint3、link名をbase_scan3とし、尾側はそれぞれscan_join4、base_scan4としています。
一方、gazebo.xacroでは113～144行目がLiDARの記述で、waffle_pi3では114行目でLiDARの色を黄色に変えています。122～125行目でスキャンする角度範囲と分解能を設定していますが、waffle_piでは全周スキャンであったものをwaffle_pi3では両サイドの後方を少し加えた半周に限定しています。ドライブだけであれば全周をスキャンして前方のデータだけを使うということでいいのですが、ナビにも使うのでamclノードで問題が生じます。自身の本体を検知して至近距離の障害物があるというのでは、自己位置が推定できません。146～177行目は尾側のLiDARでスキャン範囲は同じですが、色は黒になっています。
4.4 ドライブノード
前述の通り、元のドライブノードのソースコードはturtlebot3_drive.cppですが、TARを解凍して出てくるコードは既に修正されています。これは改良の途中段階なので、特に説明しません。オリジナルのソースコードは例えばこのサイトで見られるので、まずこのコードについて説明します。72行目で指定した3方向に対するLiDARのスキャンデータ（障害物までの距離）をそれぞれscan_data_[CENTER] 、scan_data_[LEFT]、scan_data_[RIGHT]としています。{0, 30, 330}は何番目のスキャンデータかを示していますが、gazebo.xacroで全周を360分割して測定すると設定しているので、順番がそのままdegree単位の角度になります。
3つのscan_data_を使って104～154行目でTB3の進路を決定します。turtlebot3_drive.hの中で識別子の定義としてGET_TB3_DIRECTIONは0、TB3_DRIVE_FORWARDは1、TB3_RIGHT_TURNは2、TB3_LEFT_TURNは3だとしているので、102行目で設定された初期値0によってまずは106行目のケースになります。CENTERが設定値よりも大きい場合は、LEFTかRIGHTが設定値よりも小さければ反対方向へのターンを選択し、どちらも設定値より大きければ直進を選択します。CENTERが設定値よりも小さい場合は右ターンを選択します。
これで新しいケースが決定されるので、次の周期で直進もしくは右/左ターンが実行されます。直進の場合はすぐにGET_TB3_DIRECTIONに戻りますが、右/左ターンの場合はescape_range_以上旋回するまで同じケースを維持します。ケースによるswitchは167行目にあるloop_rateで繰返されるので、周期は125Hzです。静止障害物だけであれば、このような簡単な制御でTB3はスイスイと進みます。しかし、他のTB3が動的障害物として登場するとそうはいきません。waffle_piモデルで他のTB3のLiDAR部までの距離しか測定できないとなればなおさらです。
そこで色々な修正を重ねてbase_drive_18.pyに至る訳けですが、base_drive_18.pyはROSのノードではなくPythonのクラスで、ノードである/src/mtb3_sim/src/tb3_drive_18によってそのインスタンスが生成されます。なぜこのような建付けにしたかと言うと、base_drive_18.pyを継承したact_drive_18.pyを作りたかったからです。act_drive_18.pyを使うのは「IX. 深層強化学習をROS/AnyLogicに導入」の「4. DRLで進路を決定」の中です。本サイトでは離れ離れの記述となっていますが、実際のプログラミングは相前後して行いました。
base_drive_18.pyではピンポイントの3方向の距離ではなく、一定の角度範囲のスキャンデータの最小値を取得します。getScan3()では首のLiDARが取得したデータを使い、角度範囲は正面、前方、広前方、側方をそれぞれ左右に分けた8通りとしています。gazebo.xacroで±1.64061 rad（±94°）の間を両端を含めて189点に分割していることを踏えて、scan_data3の添字範囲を角度範囲にすると以下に赤字で示すようになります。Pythonのranges[m:n]はmからn-1までの添字範囲です。0°を左側に含めているので、1°だけ左右非対称になっています。getScan4()では尾のLiDARが取得したデータを使いますが、角度範囲は後方角のみです。
def getScan3(self, scan_data3): scan_angle = [84, 104, 74, 114, 44, 144, 0, 189] # front(正面) scan_front_right = min(scan_data3.ranges[scan_angle[0]:94]) # [84] = -10° ～ [93] = -1° scan_front_left = min(scan_data3.ranges[94:scan_angle[1]]) # [94] = 0° ～ [103] = 9° # fwd(前方) scan_fwd_right = min(scan_data3.ranges[scan_angle[2]:94]) # [74] = -20° ～ [93] = -1° scan_fwd_left = min(scan_data3.ranges[94:scan_angle[3]]) # [94] = 0° ～ [113] = 19° # wide(広前方) scan_wide_right = min(scan_data3.ranges[scan_angle[4]:94]) [44] = -50° ～ [93] = -1° scan_wide_left = min(scan_data3.ranges[94:scan_angle[5]]) [94] = 0° ～ [143] = 49° # side(側方) scan_side_right = min(scan_data3.ranges[scan_angle[6]:scan_angle[4]]) [0] = -94° ～ [43] = -51° scan_side_left = min(scan_data3.ranges[scan_angle[5]:scan_angle[7]]) [144] = 50° ～ [188] = 94° def getScan4(): scan_angle = [0, 5, 184, 189] # corn(後方角) scan_bc_left = min(scan_data4.ranges[scan_angle[0]:scan_angle[1]]) [0] = -94° ～ [4] = -90° scan_bc_right = min(scan_data4.ranges[scan_angle[2]:scan_angle[3]]) [184] = 90° ～ [188] = 94°
スキャンデータから取得した最小距離に基づいてTB3の動きを制御します。まずは80～114行目のpubVel()で基本的な動きを決めます。コメントにある通り、広前方と側方の障害物までの距離が所定値（check_wide_dist = 0.31）以上の場合は、62～78行目のsetVel()で直進、緩旋回進行、急旋回進行の何れかを選択し、速度司令cmd_velを設定します。広前方か側方の障害物までの距離が所定値未満の場合はその場旋回させますが、ここでself.keep_turnという変数を導入しています。左右の旋回を繰返す振動状態になるのを避けるため、後方角が障害物に接近するまでは同じ方向への旋回を続けます。
制御周期ごとの判断で、広前方と側方の障害物までの距離が所定値以上になれば、前進を再開します。制御周期は86行目にある「self.getScan3()がコールバックされてself.scan_minが書換えられるまで待つ」で決っており、首のLiDARのデータ取得周期（5 Hz）となります。以上、かなり複雑な制御となっていますが、他のTB3との衝突を回避しつつ静止障害物間の隘路を通過できるようにするという観点で試行錯誤を繰返して得た結論です。それでも立方体と円柱の間を通り抜ける際に進入角度が悪いと、反転して戻って行くという現象があってイラっとしますが、気ままに動くドライブなのでこれもご愛嬌でしょう。
4.5 ナビノード
4節で説明してきたプログラム全体は/src/mtb3_sim/launch/navi_drive3.launchでローンチされるので、その6行目を<arg name="model" default="waffle_pi3" />にすれば、102～112行目の<!-- move_base --/>の中の$(arg model)はwaffle_pi3となり、move_baseのモデルもwaffle_pi3になります。そこで、TB3ナビ関連パラメータフォルダでモデル名が指定されているファイルであるcostmap_common_params_モデル名.yamlおよびdwa_local_planner_params_モデル名.yamlについては、モデル名をwaffle_pi3としたファイルが必要になります。
dwa_..._モデル名.yamlはダラダラバックの原因となった因縁のファイルですが、waffle_piのファイルをそのままコピーしてwaffle_pi3とするだけです。一方、costmap_..._モデル名.yamlについては変更が必要です。コストマップの元となる障害物を検知するLiDARが変更されているからです。ここも「大事なことは上流で決める」で、コストマップを生成するcostmap_2dノードのソースコードをいじる必要はなく、パラメータファイルを変更するだけでOKです。waffle_piのファイル12行目にあるscan:をscan3:に変更して、同じ書式のscan4:を併記すれば、それだけで両方のliDARが検知した障害物を合成してくれます。
LiDAR変更の影響を受けるノードには、もう一つamclがあります。これも上流のローンチファイルを変更するだけでいいのですが、元は全体のローンチファイルm5_w2.launchがamcl.launchを起動する建付けでした。これでは二度手間になるので、navi_drive3.launchの中にamcl.launchの内容をそのまま埋込んであります。navi_drive3.launchの8行目で<arg name="scan_topic" default="scan3" />としているので、65行目からの<!-- AMCL -->の中の81行目でscanデータがscan3にremapされます。scan4のデータは使っておらず、scan3はtb3_0の前半分の領域しかスキャンしませんが、ROS1のAMCLはこれで問題なく動作します。
最後に目標姿勢を設定するノードについて説明します。これはwaffle_pi3の導入とは関係ありませんが、元のgoal_navi.cppを/src/mtb3_sim/src/mtb3_navi_2にしています。mtb3_navi_2はPythonプログラムですが、ROS1では.pyを付けなければそのままノードとして扱われます。goal_navi.cppをPythonに移植しただけのpy_naviについては「I. 人生の余技として作成したプログラム」の「10. Python」で紹介済みですが、そこに各行程の所要時間をグラフ表示する機能と、指定した走行回数でプログラムを終了する機能を付加しました。
mtb3_navi_2の主要部をクラスにしているのは、「IX. 深層強化学習をROS/AnyLogicに導入」の「2. 画像DLでAMR出発判断」で同様の位置付けとなるnavi_rec_14が2つのクラスを選択的に使う建付けであることに呼応しますが、ここでは特に意味はありません。ドライブとナビのローンチ関連ファイルを整理すると、以下のようになります。
navi_drive3.launch ⇒ tb3_drive_18 ⇒ base_drive_18.py (baseDriveクラス)
│ ドライブノード生成 インスタンス生成
│
└⇒ mtb3_navi_2 (プログラム内にNaviDriveNodeクラス)
ナビノード生成