VII. ROSによるAMRのシミュレーション（１）
1. プログラム作成の背景
駄待ち狐が2020年にROSの勉強を始めた経緯は、「I. 人生の余技として作成したプログラム」の「9. C++」で書いた通りです。この時は、何かやりたいことがあって手持ちのリソースで何とかするというスタートではなく、ROSが面白そうだからいじってみようという発想でした。UNIXとCの勉強を始めた時と同じ感覚で、駄待ち狐にとっては"面白そう" == "難しそう"です。ROSはロボット（近頃はクローラーロボットやアンドロイドOSという言葉が出てきて混乱していますが本来の意味のロボット）を動かすためのミドルウエアですが、ROS自体に付属しているGazeboというパッケージでロボットの動きをシミュレーションすることもできます。
ミドルウエアというのはOSとアプリケーションソフトの橋渡しをするソフトウエアです。ROSはOSと密接に連携して動作するため、OSの種類（Linux、Windows、MacOS）とそのバージョンに対応するROSのバージョンを選んでインストールする必要があります。WindowsやMacで動作するROSもありますが、Linuxの一種であるUbuntu上で動作させるのが定番で、一番安定しています。UbuntuはOSとしてWindows PCにインストールできますが、BIOSがOSを起動する時点でWindowsとは別個に立上るデュアルブートになります。
Windows-Ubuntuデュアルブートについてはグーグればいくらでも解説が出てきますが、下手をするとWindows自体が起動しなくなってしまうという恐しさがあります。駄待ち狐もUbuntuのことを全く知らなければ取敢えずWindowsで動かしてみようとなって無駄な労力を費したことだろうと思いますが、幸い自前のレッツノートをデュアルブートにしていました。なぜそんなことをしていたかと言うと、「VI. グループメンバーの弁当注文取り纏め」でチラッと書いたリモートPC-シンクライアントが関係しています。
駄待ち狐の会社では情報システム部門が一時期シンクライアントを推進していました。社用ノートPCを紛失するとその時点で情報セキュリティ事故として扱われ、経営幹部を巻込んだ大騒動になるためです。シンクライアントでは会社の機密情報はリモートPC上にあり、シンクライアントとの接続方法を厳重にセキュリティ管理していればシンクライアント自体からの情報漏洩の心配はありません。このため、私物のPCをシンクライアントとして使うことも認められていました。シンクライアントとして使いたいPCにCD-ROMを入れて起動すると、リモートPCと接続するための画面が立上ります。
ところがどっこいです。2017年春モデルのレッツノードではUEFIが導入されてCD-ROM起動ができなくなったのです。駄待ち狐はコロナ禍のずっと前からシンクライアントでリモートワーク（自宅・出張先・新幹線移動中を含めたどこでもワーク）を実践していたので、シンクライアントを使えなくなっては一大事です。情報システム部門に問合せても「そのレッツノートはシンクライアントとして使えません」と言うだけです。自力で解決するしかないのでCD-ROM起動のシンクライアントが何をしているのかを子細に検討した結果、Ubuntuが起動してCitrix ReceiverというアプリでリモートPCに接続していることが分りました。
自前で買ったばかりのレッツノートには何もデータを入れてなかったので、最悪でもWindowsの再インストールをすればいいという気楽さもあり、Ubuntuのデュアルブートに挑戦です。Citrix Receiverもインストールして何とかリモートPCに接続できるようになりました。この顛末は詳細に情報システム部門にレポートしましたが、特段の回答はありませんでした。何にしても「輪廻は巡る糸車」、このシンクライアントが弁当注文取り纏めでどこでも発注チェックの救世主になり、デュアルブートがROSいじりの足がかりになるとは不思議な巡り合せです。
2. 当初のシミュレーションとその課題
広義のROSには狭義のROS（ここではROS1と表記します）とROS2があります。ROS2はROS1の進化形ですが、全く互換性はありません。以下に示す通り、それぞれにUbuntuのバージョンに対応するバージョンがあります。最初はレッツノートに入れたUbuntu 20.04でROS1を使いました。その時にROS2も試したのですがダメダメでした。2023年にGPUが入っていないレッツノートに限界を感じ、Lenovo Legionを購入してUbuntu 22.04を入れました。対応するROS1は公式にはもうないので、何とかROS2を使いこなしました。ただ、本稿作成にはROS1も使えないと困るので、このサイトにある裏技でROS1を使えるようにしました。
[Ubuntu 20.04] Focal Fossa ROS1: Noetic Ninjemys ROS2: Foxy Fitzroy (given up!) [Ubuntu 22.04] Jammy Jellyfish ROS2: Humble Hawkbill ROS1: Noetic Ninjemys (by a trick)
ROS1あるいはROS2をインストールする方法はグーグれば出てきますが、ROSは非常に複雑なので体系的な知識を得ようとすれば教科書で勉強する必要があります。ROSの基本はノード（実行可能なプログラム単位で、C++あるいはPythonのソースコードからビルドされる）間で通信をすることです。ロボットを制御するのであれば、ロボットに搭載されたシングルボードコンピュータ（SBC）とリモートPCにROSをインストールして、それぞれのノード間で通信をします。PCだけでシミュレーションをする場合は、同一PC上の異なるノード間で通信をします。
駄待ち狐が使った教科書では、プログラミングの話に入るとこのノード間通信のコード解説が延々と続き、全体像が把握できていないのでうんざりしました。教科書で次に出てくるのがロボットメカ（センサ、モータ）を通信によって制御する方法です。ROS以前のロボット制御でも、センサで取得した情報に基づいてモータを駆動しています。ROSではセンサの取得データとモータの駆動データをそれぞれ処理するパッケージがあり、それらのノード間で通信をすることでロボットを制御します。ROS本体はノード間通信の規格であり、パッケージ群が個々のハードウエアを通信可能なノードにするというイメージです。
この後は、TurtleBot3（TB3）を例にした移動ロボットの話になります。最後にアームロボットも出てきますが、駄待ち狐の興味は自律移動ロボット（autonomous mobile robot: AMR）です。TB3はROSの標準ロボットプラットフォームで、市販されています。ロボットにはSBCが搭載されており、左右2輪の差動駆動輪によって前進・後退・左右旋回ができます。LiDARセンサで全方位の障害物を検知しますが、2次元LiDARなので設置高さの水平面のみのスキャンです。TB3にはBerger、Waffle、Waffle Piの3つのモデルがあり、カメラなし、深度カメラ付き、通常カメラ付きとなっています。本稿のシミュレーションではWaffle Piを使っています。
2.1 ROS1によるドライブとナビ
移動ロボットを制御するためには、LiDARのデータを地図と照合して自位置を推測するamclノードや、経路を決定するmove_baseノードが必要ですが、これらのノードはROSをインストールすればその中に含まれています。一方、TB3のロボットモデルやTB3に特化した制御/シミュレーションプログラムは、ROSとは別にインストールする必要があります。TB3パッケージのソースコードをgit cloneでダウンロードして、ROS1であればcatkin_make、ROS2ではcolcon buildでビルドします。因みに、ROSと同時にインストールされるノードを修正したい時にも、そのパッケージのソースコードをダウンロードして修正した後にビルドします。
TB3パッケージのインストールだけで動作するシミュレーションの中に、TB3が障害物を避けながら自由気ままに走行するturtlebot3_drive（ドライブ）と、指定された目的地を目指して走行するturtlebot3_navigation（ナビ）があります。それぞれ、以下の3つないしは2つのコマンドをターミナルから入力すれば動作しますが、コマンドは同時並行で実行されるので新しいターミナルを次々に開いて入力していきます。どのコマンドもローンチファイルを起動しており、最初のコマンドであればturtlebot3_gazeboパッケージの中にあるturtlebot3_world.launchが実行されます。turtlebot3_gazeboパッケージはTB3パッケージに含まれるパッケージです。
[ドライブ] $ roslaunch turtlebot3_gazebo turtlebot3_world.launch $ roslaunch turtlebot3_gazebo turtlebot3_simulation.launch $ roslaunch turtlebot3_gazebo turtlebot3_gazebo_rviz.launch [ナビ] $ roslaunch turtlebot3_gazebo turtlebot3_world.launch $ roslaunch turtlebot3_navigation turtlebot3_navigation.launch
ローンチファイルは複数のノードを同時に実行するためのファイルで、ROS1ではXMLで記述されます。個々のノードを実行するコマンドもありますが、ローンチファイルを使えば複数のノードを同時に生成することができ、パラメータも指定できるので便利です。ドライブあるいはナビを実行すると、以下のキャプチャ動画のようになります（前半がドライブ、後半がナビ）。全画面表示にしていただくと見やすいですが、画面右側がGazeboというシミュレータで、TB3が六角形のエリア内に9本の円柱が立った環境モデルの中を移動するのが表示されます。画面中央はRVizという3次元可視化ツールでLiDARのスキャンとカメラ映像が表示されます。
2.2 ドライブする4台の中で1台がナビ
ここからは駄待ち狐が作ったプログラムの動作です。上記のドライブを見てまず思ったのは、3つのコマンドをいちいち入力するのは面倒だということです。そこでドライブのローンチファイルを一つにまとめました。さらに、TB3が1台だけでは面白くないので、3台、5台が同時に動くようにしました。次に考えたのが、5台同時に動く中の1台がナビで動作するということです。4台の気ままに動くTB3の中を目的地に向って走行するというのが面白いと思ったのです。ところが、9本の円柱が立っただけの環境モデルでは、TB3同士があまり干渉しません。そこで環境の障害物を増やすことにして、大きな立方体4個を配置しました。
キャプチャ動画を見ていただくと上記のことが実現されているのが分ると思います。ここで新たに作成したコードについては3節で紹介しますが、TB3の動作には改良すべき点もあります。動画の開始12秒後にナビで走行するTB3（tb3_0）がドライブで走行するTB3と鉢合せするとtb3_0はバックしますが、そのままダラダラと後退を続けて28秒後に後方障害物にぶつかるとようやく前進に転じます。また、53秒後にはtb3_0が他のTB3に接触しますが、その後方には3台目のTB3もいて、おしくらまんじゅう状態でしばらく動けません。1分5秒後にtb3_0がバックしてようやく膠着状態を脱し、1分29秒後にかなり強引に突っ込んでゴールします。
1分35秒で動画がスキップしますが、1分55秒後に新たな目標姿勢が設定されるとtb3_0はゴールまでの経路を見つけられずに止ってしまいます。よく見ると、ゴール地点近くに他のTB3がいて、これが立去った後に障害物のゴーストが残るために経路が見つけられないのです。このゴーストは時間が経っても消えることはなく、tb3_0は永久に停止してしまいます。これらの問題行動は、3節で述べる自作コードをひねくり回すだけではどうにもなりません。git cloneでダウンロードしたソースコードそのものを修正する必要があり、その内容は4節で述べます。
2.3 ROS2で5台のドライブ
ROS2でもROS1と同様に以下のコマンドでドライブとナビが可能なはずです。しかし、レッツノートのUbuntu 20.04でFoxyを使っている時は、動作が全く安定しませんでした。複数台のTB3をドライブで走行させると、TB3が壁にぶつかって転げ回り出します。1台だけのナビでは、ゴールまでの移動を開始した後、後退してギクシャクした動きになり、結局止ってしまいます。問題を検討しようという意欲さえ失せてしまうダメダメぶりです。LegionのUbuntu 22.04でHumbleを使い始めると、ちゃんとした動きで課題が見えてきました。FoxyかHumbleかという問題ではなく、レッツノートかLegionかという問題でしょう。
[ドライブ] $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py $ ros2 run turtlebot3_gazebo turtlebot3_drive $ ros2 launch turtlebot3_bringup rviz2.launch.py [ナビ] $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py $ ros2 launch turtlebot3_navigation2 navigation2.launch.py
LegionのHumbleで見えてきた課題というのが、以下のキャプチャ動画です。5台のTB3が全てドライブで走行するのですが、RVizではその中の1台であるtb3_0の動きを見ています。動画の開始2秒後にtb3_0が別のTB3と衝突します。相手方のTB3は揺れ始めて14秒後には横倒しになり、そのまま転げ回って行きます。ROS1では2台のTB3が衝突しても動けなくなるだけですが、ROS2ではこのような転覆事故になります。「重さ1.8kgのTB3がこんな風に転げ回るの？」という疑問はありますが、衝突すれば跳ね飛ばされるというのは、ただじっとして動かなくなるというよりもリアリティがあります。
ROS1ではおしくらまんじゅうになって動けなくなるという問題が、ROS2では跳ね飛ばされるという問題になっただけで、どちらもTB3同士が衝突することが原因です。これについては4節で述べるROS1の課題検討の中で衝突しないようにすることができました。同じ手法をROS2に適用することで、ROS2でも「ドライブする4台の中で1台がナビ」がちゃんと動作するようになりました。ただ、ROS1とROS2ではナビのプログラム構成が全く異なるので、手法が同じというだけでコードは全く別物になります。ROS2については5節で述べます。
3. 最初のROS1プログラム
2.2項で紹介した動画でプログラムを起動するコマンドは、冒頭画面のターミナル内に表示される"rlm m5_w2.launch"です。rlmは.bashrcで定義したエイリアスで"roslaunch mtb3_sim"の略記になります。mtb3_simは駄待ち狐が新たに作成したパッケージです。最初はTB3のパッケージの中に新たに作成したローンチファイルを置いていましたが、バージョンがどんどん増えて行くので自作のファイルはmtb3_simパッケージに入れることにしました。ここで新たに作成したファイルは、m5_w2.launchのXML、目標姿勢を自動設定するC++、環境モデルのSDF、地図のYAMLとPGMです。以下、これらのコードについて説明していきます。
3.1 ローンチファイル
m5_w2.launchのコードは以下の通りで、2～36行目の<!-- Arguments -->はコマンド入力時の引数です。3行目のmodelであれば"rlm m5_w2.launch"の後にmodel:="waffle_pi"と指定します。省略すればdefault値が使用され、$(env TURTLEBOT3_MODEL)は.bashrcで設定可能な環境変数です。4行目のmap_fileはナビで使用する地図で、$(find turtlebot3_navigation)は当該パッケージのディレクトリです。5行目のmove_forward_onlyはtrueであればナビでTB3が後退しません。7～11行目は各TB3の名前で、どのTB3のノードかを識別するのに使われます。13～36行目は各TB3の初期姿勢（位置と向き）を設定する引数です。
コードの表示にはPrism.jsを使用しています。prism.cssの設定は団塊爺ちゃんの備忘録を参考にしました。
<launch>
<!-- Arguments -->
<arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
<arg name="map_file" default="$(find turtlebot3_navigation)/maps/map2.yaml"/>
<arg name="move_forward_only" default="false"/>
<arg name="first_tb3" default="tb3_0"/>
<arg name="second_tb3" default="tb3_1"/>
<arg name="third_tb3" default="tb3_2"/>
<arg name="forth_tb3" default="tb3_3"/>
<arg name="fifth_tb3" default="tb3_4"/>
<arg name="first_tb3_x_pos" default="-2.0"/>
<arg name="first_tb3_y_pos" default="-0.8"/>
<arg name="first_tb3_z_pos" default=" 0.0"/>
<arg name="first_tb3_yaw" default=" 0.0"/>
<arg name="second_tb3_x_pos" default=" 2.0"/>
<arg name="second_tb3_y_pos" default="-0.8"/>
<arg name="second_tb3_z_pos" default=" 0.0"/>
<arg name="second_tb3_yaw" default=" 1.57"/>
<arg name="third_tb3_x_pos" default=" 1.0"/>
<arg name="third_tb3_y_pos" default=" 2.0"/>
<arg name="third_tb3_z_pos" default=" 0.0"/>
<arg name="third_tb3_yaw" default=" 0.0"/>
<arg name="forth_tb3_x_pos" default="-1.5"/>
<arg name="forth_tb3_y_pos" default=" 1.5"/>
<arg name="forth_tb3_z_pos" default=" 0.0"/>
<arg name="forth_tb3_yaw" default=" 0.0"/>
<arg name="fifth_tb3_x_pos" default=" 0.0"/>
<arg name="fifth_tb3_y_pos" default="-0.5"/>
<arg name="fifth_tb3_z_pos" default=" 0.0"/>
<arg name="fifth_tb3_yaw" default=" 0.0"/>
<!-- world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_world2.world"/>
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="headless" value="false"/>
<arg name="debug" value="false"/>
</include>
<!-- tb3_0 in the world -->
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
<param name="publish_frequency" type="double" value="50.0" />
</node>
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model $(arg first_tb3) -x $(arg first_tb3_x_pos) -y $(arg first_tb3_y_pos) -z $(arg first_tb3_z_pos) -Y $(arg first_tb3_yaw) -param robot_description" />
<!-- Map server -->
<node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>
<!-- AMCL -->
<include file="$(find turtlebot3_navigation)/launch/amcl.launch"/>
<!-- move_base -->
<include file="$(find turtlebot3_navigation)/launch/move_base.launch">
<arg name="model" value="$(arg model)" />
<arg name="move_forward_only" value="$(arg move_forward_only)"/>
</include>
<!-- rviz -->
<node pkg="rviz" type="rviz" name="rviz" required="true"
args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/>
<!-- goal navi -->
<node pkg="mtb3_sim" name="goal_navi" type="goal_navi"/>
<group ns = "$(arg second_tb3)">
<param name="tf_prefix" value="$(arg second_tb3)" />
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
<param name="publish_frequency" type="double" value="50.0" />
</node>
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model $(arg second_tb3) -x $(arg second_tb3_x_pos) -y $(arg second_tb3_y_pos) -z $(arg second_tb3_z_pos) -Y $(arg second_tb3_yaw) -param robot_description" />
<param name="cmd_vel_topic_name" value="/tb3_1/cmd_vel"/>
<node name="tb3_1_drive" pkg="turtlebot3_gazebo" type="turtlebot3_drive" required="true" output="screen"/>
</group>
<group ns = "$(arg third_tb3)">
<param name="tf_prefix" value="$(arg third_tb3)" />
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
<param name="publish_frequency" type="double" value="50.0" />
</node>
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model $(arg third_tb3) -x $(arg third_tb3_x_pos) -y $(arg third_tb3_y_pos) -z $(arg third_tb3_z_pos) -Y $(arg third_tb3_yaw) -param robot_description" />
<param name="cmd_vel_topic_name" value="/tb3_2/cmd_vel"/>
<node name="tb3_2_drive" pkg="turtlebot3_gazebo" type="turtlebot3_drive" required="true" output="screen"/>
</group>
<group ns = "$(arg forth_tb3)">
<param name="tf_prefix" value="$(arg forth_tb3)" />
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
<param name="publish_frequency" type="double" value="50.0" />
</node>
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model $(arg forth_tb3) -x $(arg forth_tb3_x_pos) -y $(arg forth_tb3_y_pos) -z $(arg forth_tb3_z_pos) -Y $(arg forth_tb3_yaw) -param robot_description" />
<param name="cmd_vel_topic_name" value="/tb3_3/cmd_vel"/>
<node name="tb3_2_drive" pkg="turtlebot3_gazebo" type="turtlebot3_drive" required="true" output="screen"/>
</group>
<group ns = "$(arg fifth_tb3)">
<param name="tf_prefix" value="$(arg fifth_tb3)" />
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
<param name="publish_frequency" type="double" value="50.0" />
</node>
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model $(arg fifth_tb3) -x $(arg fifth_tb3_x_pos) -y $(arg fifth_tb3_y_pos) -z $(arg fifth_tb3_z_pos) -Y $(arg fifth_tb3_yaw) -param robot_description" />
<param name="cmd_vel_topic_name" value="/tb3_4/cmd_vel"/>
<node name="tb3_2_drive" pkg="turtlebot3_gazebo" type="turtlebot3_drive" required="true" output="screen"/>
</group>
</launch>
38～53行目はナビのturtlebot3_world.launchに相当します。<!-- world -->ではSDF形式で書かれた環境モデルturtlebot3_world2.worldを用いてGazeboを起動します。gazebo_rosはTB3パッケージではなくROS1に元からあるパッケージで、~/catkin_wsではなく/opt/ros/noeticの中にあるのですが、findはちゃんと見つけてくれます。<!-- tb3_0 in the world -->はナビで走行するTB3を環境モデル上に出現させますが、ここでは名前を指定していません。他のTB3のノードにはtb3_i/が接頭辞として付くので、「名なしの権兵衛」としてtb3_0を識別します。
55～69行目はナビのturtlebot3_navigation.launchです。<!-- Map server -->では引数で指定した地図ファイルを用いてmap_serverノードを実行します。次の<!-- AMCL -->ではadaptive Monte Carlo localizationによる自己位置推定を行います。<!-- move_base -->はコストマップ（障害物からの距離に応じてコスト値を設定した地図）に基づいて経路計画を行うノードで、ナビの要になります。<!-- rviz -->はRVizを起動します。ナビの元のローンチファイルはここで終りで、RVizを使って目標姿勢を手動で設定します。しかし、m5_w2.launchではこの後に実行されるgoal_naviノードによって目標姿勢を設定します。
75～84行目は2台目のTB3であるtb3_1のドライブに関する部分で、turtlebot3_world.launchでTB3を出現させる部分（77～81行目）と、turtlebot3_simulation.launch（82～83行目）を合体させたものです。全体をns（名前空間）が$(arg second_tb3)の<group>とすることで、他のTB3のノードと区別します。$(arg second_tb3)は8行目でdefault="tb3_1"とされています。また、76行目でtf（位置座標）、82行目でcmd_vel（速度司令）の接頭辞にtb3_1を付けることで、どのTB3の通信メッセージかを識別します。86～95行目、97～106行目、108～117行目は3～5台目のTB3に対するドライブです。
3.2 目標姿勢設定プログラム
目標姿勢を設定するgoal_naviノードは、C++のソースコードgoal_navi.cppをビルドしたものです。goal_navi.cppは「I. 人生の余技として作成したプログラム」の「9. C++」で紹介済みですが、再掲します。5行目のfwd_flgは行きか帰りかを示すフラッグで、7～11行目のmbrCb()は17行目で宣言されたサブスクライバsubが呼出すコールバック（cb）関数です。subは引数で指定されたメッセージmove_base/resultを受信すると、cb関数を実行します。メッセージの有無は31行目でspinOnce()を実行した時に確認されます。8行目はresultのstatusが3（ゴールした）の場合というif文で、fwd_flgを反転します。
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <move_base_msgs/MoveBaseActionResult.h>
bool fwd_flg = true;
void mbrCb(const move_base_msgs::MoveBaseActionResult::ConstPtr& result){
if(result->status.status == 3){
fwd_flg = !fwd_flg;
}
}
int main(int argc, char **argv){
ros::init(argc, argv, "goal_navi");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("move_base/result", 1, mbrCb);
ros::Publisher pub = nh.advertise<geometry_msgs::PoseStamped>("move_base_simple/goal", 1);
ros::Rate loop_rate(1);
ros::Time time = ros::Time::now();
geometry_msgs::PoseStamped goal_point;
goal_point.pose.position.z = 0.0;
goal_point.pose.orientation.x = 0.0;
goal_point.pose.orientation.y = 0.0;
goal_point.header.stamp = time;
goal_point.header.frame_id = "map";
while(ros::ok()){
ros::spinOnce();
if(fwd_flg){
goal_point.pose.position.x = 1.7;
goal_point.pose.position.y = -1.0;
goal_point.pose.orientation.z = -sqrt(2) / 2;
goal_point.pose.orientation.w = sqrt(2) / 2;
}else{
goal_point.pose.position.x = -2.0;
goal_point.pose.position.y = -0.8;
goal_point.pose.orientation.z = 0.0;
goal_point.pose.orientation.w = 1.0;
}
pub.publish(goal_point);
loop_rate.sleep();
}
return 0;
}
13行目からのメイン関数では、まずgoal_naviというノード名で初期化したノードにノードハンドルnhを設定します。17行目のsubは先程述べた通りです。18行目で宣言されたパブリッシャpubはmove_base_simple/goalというメッセージを発信しますが、実際に発信するのは44行目のpub.publish()を実行した時です。30～46行目のwhile節は20行目で指定されたloop_rate(1)で45行目のsleep()をするので、1秒毎にメッセージを発信することになります。23～28行目では目標姿勢goal_pointをPoseStamped型であると宣言し、while節の中で設定するフィールド以外のフィールドを設定しています。
3.3 環境モデルの作成
Gazeboはシミュレータですが環境モデルのグラフィックエディタとしても使えます。今回の例では、まず以下のコマンドでturtlebot3_world.worldを出現させます。パラメータworld_nameは環境変数GAZEBO_RESOURCE_PATHからの相対アドレスなので、worlds以下の指定で問題ありませんが、実際には~/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo/worldsです。この後、Gazeboのエディットメニューから立方体を選んでモデル上に追加し、4個とも追加したらFileメニューの"Save World As"でturtlebot3_world2.worldとして保存します。
$ roslaunch gazebo_ros empty_world.launch \ world_name:="worlds/turtlebot3_world.world"
保存したファイルは以下のようになっており、拡張子はworldとしていますが形式はXMLを使ったSDF（simulation description format）です。3～32行目は空っぽのempty.worldにもある部分で、太陽、地面のモデルをインクルードし、ODE（物理演算エンジン）のパラメータを設定しています。34～37行目でインクルードしているのは六角形のエリア内に9本の円柱が立ったTB3の環境モデルで、この後の39～91行目、93～145行目、146～199行目、201～253行目がそれぞれ上・左・下・右の立方体になります。255～259行目では場面の明るさを設定し、261～266行目ではシミュレータカメラの位置と向きを指定しています。
<sdf version='1.4'>
<world name='default'>
<!-- A global light source -->
<include>
<uri>model://sun</uri>
</include>
<!-- A ground plane -->
<include>
<uri>model://ground_plane</uri>
</include>
<physics type="ode">
<real_time_update_rate>1000.0</real_time_update_rate>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<ode>
<solver>
<type>quick</type>
<iters>150</iters>
<precon_iters>0</precon_iters>
<sor>1.400000</sor>
<use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
</solver>
<constraints>
<cfm>0.00001</cfm>
<erp>0.2</erp>
<contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
<contact_surface_layer>0.01000</contact_surface_layer>
</constraints>
</ode>
</physics>
<!-- Load world -->
<include>
<uri>model://turtlebot3_world</uri>
</include>
<model name='unit_box_0'>
<pose>1.74717 0.204408 0.5 0 -0 0.014298</pose>
<link name='link'>
<inertial>
<mass>100</mass>
<inertia>
<ixx>16.6667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>16.6667</iyy>
<iyz>0</iyz>
<izz>16.6667</izz>
</inertia>
<pose>0 0 0 0 -0 0</pose>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<model name='unit_box_1'>
<pose>0.002488 1.76801 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>100</mass>
<inertia>
<ixx>16.6667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>16.6667</iyy>
<iyz>0</iyz>
<izz>16.6667</izz>
</inertia>
<pose>0 0 0 0 -0 0</pose>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<model name='unit_box_2'>
<pose>-1.77393 0.322143 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>100</mass>
<inertia>
<ixx>16.6667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>16.6667</iyy>
<iyz>0</iyz>
<izz>16.6667</izz>
</inertia>
<pose>0 0 0 0 -0 0</pose>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<model name='unit_box_3'>
<pose>0.249969 -1.75311 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>100</mass>
<inertia>
<ixx>16.6667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>16.6667</iyy>
<iyz>0</iyz>
<izz>16.6667</izz>
</inertia>
<pose>0 0 0 0 -0 0</pose>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>true</shadows>
</scene>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose>0.8 0.0 12.0 0 1.5708 0</pose>
<view_controller>orbit</view_controller>
</camera>
</gui>
</world>
</sdf>
3.4 地図ファイルの作成
環境モデルを変更したら、ナビのための地図を新たに作成する必要があります。以下のコマンドを順次実行していきますが、１行目のturtlebot3_world2.launchは、turtlebot3_world.launchを修正して新たに作成します。turtlebot3_world.worldではなく、turtlebot3_world2.world上にTB3が出現するようにworld_nameを変更します。2行目のturtlebot3_slam.launchでSLAMによる地図作成を行うslam_gmappingノードを起動しますが、本稿作成時に確認のため実行するとslam_gmappingがないというエラーが出ます。Ubuntu 22.04に裏技でNoeticをインストールしたことが影響しています。
$ roslaunch turtlebot3_gazebo turtlebot3_world2.launch $ roslaunch turtlebot3_slam turtlebot3_slam.launch $ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch --- Move the TB3 with teleop keys for drawing a new map. --- $ rosrun map_server map_saver -f \ ~/catkin_ws/src/turtlebot3/turtlebot3_navigation/maps/map2
こういう場合は"sudo apt install ros-noetic-slam-gmapping"でパッケージをインストールすればいいはずなのですが、今度は"パッケージ ros-noetic-slam-gmapping が見つかりません"と言われます（2025年1月28日時点）。なぜ無くなってしまったのかは分りませんが、次の一手はgit cloneしてソースビルドです。このサイトの「gmappingのセットアップ」に記載されている通り、slam_gmappingだけでなくその依存パッケージであるopenslam_gmappingも必要ですが、問題なくソースビルドできました。
3行目はキータイプでTB3を移動させるコマンドで、制御方法はターミナルに表示されます。環境モデル上でTB3を隈なく移動させると、RViz上に地図が作成されます。これを6行目に指定したファイル名で保存しますが、実際にはmap2.yamlとmap2.pgmという2つのファイルが生成されます。前者はm5_w2.launchで地図ファイルとして指定するもので、後者はmap2.yaml内にイメージファイルとして記載されています。これらを以下に示しますが、map2.pgmはPNGファイルに変換して掲載しています。また、Gazebo上では上方向がX軸で左方向がY軸ですが、PGMでは右方向がX軸、上方向がY軸となっています。
image: ./map2.pgm
resolution: 0.050000
origin: [-10.000000, -10.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
map2.pgm
