#プログラミング ROS< 移動ロボットの作成（5） >
はじめに
1つの参考書に沿って，ROS(Robot Operating System)を難なく扱えるようになることが目的である．その第31弾として，「移動ロボットの作成（5）」を扱う．
環境
仮想環境
| ソフト | VMware Workstation 15 |
| 実装RAM | 2 GB |
| OS | Ubuntu 64 ビット |
| isoファイル | ubuntu-mate-20.04.1-desktop-amd64.iso |
コンピュータ
| デバイス | MSI |
| プロセッサ | Intel(R) Core(TM) i5-7300HQ CPU @ 2.50GHz 2.50GHz |
| 実装RAM | 8.00 GB (7.89 GB 使用可能) |
| OS | Windows (Windows 10 Home, バージョン：20H2) |
ROS
| Distribution | noetic |
| シミュレーション | gazebo |
移動ロボットの作成
ROSを使って，ほとんどの新しいロボットを制御する手順は次のようである．
1. ROSのメッセージインタフェースを決める．
2. ロボットのモータ用ドライバを書く．
3. ロボットの物理構造を書く．
4. Gazeboのシミュレーションで使用できるようにモデルに物理的特性を追加する．
5. tfを介して座標変換データを配信し，rvizでそれを可視化する．
6. センサを追加する．ドライバとシミュレーションのサポートも必要．
7. ナビゲーション等の標準的なアルゴリズムを適用する．
移動ロボットを例にその流れを確認していく．
今回は，7についてまとめて移動ロボットの作成（5）として扱うこととする．
手順7:ナビゲーション
スタックを設定する
ロボットにナビゲーション機能を追加するためには，新たに3つノードを起動する必要がある
-
map_server
- ロボットが位置推定や経路計画に使用するための静的な地図を提供する．
-
amcl
- 静的な地図に対してロボットの位置を推定する
-
move_base
- ロボットの大局的な経路計画と局所的な制御を行う
なお，ここで扱うマップは以下に示すようなオフィスを想定したものである．
実装
以下では，先ほど示した3つのノード起動できるようにlaunchファイルを編集して，そのときの様子を示していく．
※使うワールドが前回と異なるため，その部分を変えておく．さらにロボットがいい感じのところに出現させられるように座標の引数も追加しておく．
ソースコード：map_serverの起動
<launch>
<!--TortoiseBotのURDFモデルをパラメータサーバにロードする-->
<param name = "robot_description" textfile = "$(find make_robot1)/urdf/tortoisebot.urdf"/>
<!--空のワールドでGazeboを開始する-->
<include file = "$(find gazebo_ros)/launch/willowgarage_world.launch"/>
<!--GazeboでTortoiseBotを生成し，パラメータサーバからのその記述を受ける-->
<node name = "spawn_urdf" pkg = "gazebo_ros" type = "spawn_model" args = "-param robot_description -urdf -model tortoisebot -x 8 -y -8"/>
<!--ロボットの関節状態を配信-->
<node name = "robot_state_publisher" pkg = "robot_state_publisher" type = "robot_state_publisher"/>
<!--地図サーバロード-->
<node name="map_server" pkg="map_server" type="map_server" args="$(find make_robot1)/maps/willow.yaml"/>
</launch>
これにより，以下に示すようにrviz上でマップを反映させることができる．
ソースコード：amclの起動
**amclは設定する項目がとても多く，良い性能を引き出すためには調整が必要．**今回は，amclパッケージのlaunchファイルとして差動駆動型のロボット設定例をそのまま使うことにする．
<launch>
<!--TortoiseBotのURDFモデルをパラメータサーバにロードする-->
<param name = "robot_description" textfile = "$(find make_robot1)/urdf/tortoisebot.urdf"/>
<!--空のワールドでGazeboを開始する-->
<include file = "$(find gazebo_ros)/launch/willowgarage_world.launch"/>
<!--GazeboでTortoiseBotを生成し，パラメータサーバからのその記述を受ける-->
<node name = "spawn_urdf" pkg = "gazebo_ros" type = "spawn_model" args = "-param robot_description -urdf -model tortoisebot -x 8 -y -8"/>
<!--ロボットの関節状態を配信-->
<node name = "robot_state_publisher" pkg = "robot_state_publisher" type = "robot_state_publisher"/>
<!--地図サーバロード-->
<node name="map_server" pkg="map_server" type="map_server" args="$(find make_robot1)/maps/willow.yaml"/>
<!--自己位置推定のためのamclを起動する-->
<include file="$(find amcl)/examples/amcl_diff.launch"/>
</launch>
これにより，以下に示すようにrviz上でマップに加えてロボットを反映させることができる．見づらいが，マップ上の右上にあるグリッドの中心にロボットがある．
move_baseの起動
最後にmove_baseはさまざまな設定項目を持つ複雑なノードである．幸いにも，デフォルトの設定がここで必要な設定ととても近いらしく修正すべき設定は少しだけのようだ．以下に4つほど必要なYAMLファイルを示した後，launchファイルを示す．
footprint: [[0.35, 0.15], [0.35, -0.15], [-0.35, -0.15], [-0.35, 0.15]]
observation_sources: laser_scan_sensor
laser_scan_sensor:
sensor_frame: hokuyo_link
data_type: LaserScan
topic: scan
marking: true
clearing: true
最初にロボットの形状を長方形で定義している（多角形でも可）．次に，観測の情報源として先ほどのレーザを設定している．scanトピックに配信されるデータで障害物の追加（marking）とフリースペースの宣言（clearing）の両方を行い，コストマップを更新する．
global_costmap:
global_frame: map
robot_base_frame: base_link
static_map: true
ここでは，
- グローバルなコストマップに静的な地図（map_serverによって提供される）を使うこと
- コストマップの座標系としてmap座標系を使うこと
- ロボットの主要な座標系としてbase_linkを使うこと
を設定している．
local_costmap:
global_frame: odom
robot_base_frame: base_link
rolling_window: true
グローバルコストマップには大きな静的な地図を使うが，局所的なコストマップは小さな循環する領域を参照している．ロボットは常にこの領域の中央にいてロボットが動くに従って領域の園に出ていった障害物は消されて，また入ってきたら再び観測される．局所的なコストマップにはodom座標系を使うことにする．odom座標系ではロボットの姿勢は徐々にズレていく可能性があるが，姿勢が離散的にジャンプする可能性のあるmap座標に比べると滑らかに変化する傾向がある．これが，局所的な障害物回避に対してローカルコストマップのほうが適切に機能する理由．
TrajectoryPlannerROS:
holonomic_robot: false
経路計画と制御コマンドの計算を実際に実行するベースローカルプランナーも設定する必要がある．今回の場合，TortoiseBotがホロノミックではないことを伝えるためのパラメータ1つだけを設定している．
<!--TortoiseBotのURDFモデルをパラメータサーバにロードする-->
<param name = "robot_description" textfile = "$(find make_robot1)/urdf/tortoisebot.urdf"/>
<!--空のワールドでGazeboを開始する-->
<include file = "$(find gazebo_ros)/launch/willowgarage_world.launch"/>
<!--GazeboでTortoiseBotを生成し，パラメータサーバからのその記述を受ける-->
<node name = "spawn_urdf" pkg = "gazebo_ros" type = "spawn_model" args = "-param robot_description -urdf -model tortoisebot -x 8 -y -8"/>
<!--ロボットの関節状態を配信-->
<node name = "robot_state_publisher" pkg = "robot_state_publisher" type = "robot_state_publisher"/>
<!--地図サーバロード-->
<node name="map_server" pkg="map_server" type="map_server" args="$(find make_robot1)/maps/willow.yaml"/>
<!--自己位置推定のためのamclを起動する-->
<include file="$(find amcl)/examples/amcl_diff.launch"/>
<!--move_baseを起動する-->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find make_robot1)/yaml/costmap_common_params.yaml" command="load" ns="global_costmap"/>
<rosparam file="$(find make_robot1)/yaml/costmap_common_params.yaml" command="load" ns="local_costmap"/>
<rosparam file="$(find make_robot1)/yaml/local_costmap_params.yaml" command="load"/>
<rosparam file="$(find make_robot1)/yaml/global_costmap_params.yaml" command="load"/>
<rosparam file="$(find make_robot1)/yaml/base_local_planner_params.yaml" command="load"/>
</node>
</launch>
ここでは，先ほど作成したYAMLファイル読み込んでいる．注意すべきところは，costmap_common_params.yamlファイルを2回読み込んでいることである．1回目はglobal_costmapの名前空間として読み込み，2回目はlocal_costmap名前空間として読み込んでいる．
###rvizでロボットの位置を推定し命令する
先ほどまでで，ナビゲーションスタックの設定を終えた．あとはrviz上での操作でナビゲーションを行うことができる．ここではまず，ナビゲーション用にrvizの設定を保存して，いつでも呼び出せるようにlaunchファイルにrvizを起動させるコードを追加する．
#####ソースコード：ナビゲーション
<launch>
<!--TortoiseBotのURDFモデルをパラメータサーバにロードする-->
<param name = "robot_description" textfile = "$(find make_robot1)/urdf/tortoisebot.urdf"/>
<!--空のワールドでGazeboを開始する-->
<include file = "$(find gazebo_ros)/launch/willowgarage_world.launch"/>
<!--GazeboでTortoiseBotを生成し，パラメータサーバからのその記述を受ける-->
<node name = "spawn_urdf" pkg = "gazebo_ros" type = "spawn_model" args = "-param robot_description -urdf -model tortoisebot -x 8 -y -8"/>
<!--ロボットの関節状態を配信-->
<node name = "robot_state_publisher" pkg = "robot_state_publisher" type = "robot_state_publisher"/>
<!--地図サーバロード-->
<node name="map_server" pkg="map_server" type="map_server" args="$(find make_robot1)/maps/willow.yaml"/>
<!--自己位置推定のためのamclを起動する-->
<include file="$(find amcl)/examples/amcl_diff.launch"/>
<!--move_baseを起動する-->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find make_robot1)/yaml/costmap_common_params.yaml" command="load" ns="global_costmap"/>
<rosparam file="$(find make_robot1)/yaml/costmap_common_params.yaml" command="load" ns="local_costmap"/>
<rosparam file="$(find make_robot1)/yaml/local_costmap_params.yaml" command="load"/>
<rosparam file="$(find make_robot1)/yaml/global_costmap_params.yaml" command="load"/>
<rosparam file="$(find make_robot1)/yaml/base_local_planner_params.yaml" command="load"/>
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find make_robot1)/tortoisebot_nav.rviz"/>
</launch>
以下にナビゲーションの成功例と失敗例を示す．
失敗例
まとめ
移動ロボットのシミュレーションモデルから始めて，それを自律ナビゲーションロボットに変更した．このときコードを1行も書かずに設定情報をXMLやYAMLを介して提供するだけで実現した．これがROSの力のようだ．しかし，上で失敗例を挙げたように，このナビゲーションは完璧ではないことに気づく．戸口を通り抜けられないことがあったり，ときどき迷子（間違った位置を推定）になったり，ときには立ち往生するかもしれない．次の段階は，ナビゲーションスタックのドキュメントを徹底的に調べて，自分のロボットに合わせて注意深く設定することになる．今回はとりあえず動作するシステムを作るには十分な設定で行ったが，本当にしっかりとしたナビゲーション性能を発揮するには，それぞれのロボットに対するパラメータ調整が必要なようだ．
感想
今回でようやく移動ロボットの作成を終えた．完璧ではないもののナビゲーションの基盤となる内容を獲得できたと思う．基盤といえど，実用的なナビゲーションシステムの構築に必要不可欠な要素であると感じている．移動ロボットについてはここまでだが，これからも移動ロボットを活用することがあるだろうから，その中で経験を積んでいきたい．
参考文献
プログラミングROS Pythonによるロボットアプリケーション開発
Morgan Quigley, Brian Gerkey, William D.Smart 著
河田 卓志 監訳
松田 晃一，福地 正樹，由谷 哲夫 訳
オイラリー・ジャパン 発行