こんにちは、アールティの倉澤ズズくんです。
前回の記事では、Raspberry Pi Mouseが、ROS 2 Jazzyに対応したことを扱いました。今回は、Nav2の新機能である衝突監視パッケージのCollision Monitorを試したいと思います。
Raspberry Pi Mouseの紹介
Raspberry Pi Mouse（ラズパイマウス）はメインボードにRaspberry Piを使用した左右独立二輪方式の小型移動プラットフォームロボットです。全長13cm、重さ700gと小型であるため、持ち運びや開発スペースの確保も簡単です。LiDARやカメラを搭載して幅広い開発ができ、学習や研究用途に使用可能です。
ROS 2パッケージを利用すると、次の動画のように基本的な車体の制御や周囲環境の地図を作成するSLAMなどが簡単に行えます。
Raspberry Pi Mouseの詳細な特徴については、下記の製品ページをご覧ください。
Raspberry Pi Mouseは下記の販売ページから購入できます。ROS 2パッケージは無償で公開しているため、購入前の参考にご利用ください。
Raspberry Pi Mouseのナビゲーション
Raspberry Pi Mouseには、SLAMとナビゲーションを行うサンプルプログラムが用意されています。このサンプルプログラムはLiDARのオプションパーツを搭載すればすぐに実行できます。
SLAMとナビゲーションに関する詳細は、以下のブログをご参照ください。
Raspberry Pi MouseではNav2というパッケージを使用してナビゲーションを行っています。本パッケージは、目的地までの経路計画やロボットの走行、障害物回避を制御します。
ROS 2がHumbleからJazzyに移行する間に、Nav2の標準的な起動設定にCollision Monitorが含まれるようになりました。本機能は、周囲の物体との衝突を監視してロボットの速度を制御する機能を提供します。今回は、Collision Monitorを使用し、Raspberry Pi Mouseのナビゲーションがどのように変化するかを試してみたいと思います。
Collision Monitor
概要
Collision Monitorでは、ナビゲーション時、あらかじめ設定したロボット周辺領域内に障害物を検知した際の機体の減速や停止といった動作を定義できます。
本機能はマップに依存せずにロボットに搭載されたセンサの値から障害物があるか否かを判断して制御を行うため、マップ上に存在しない未知の物体に対しても対応可能です。
一方、障害物を検知して停止した結果、停止状態から抜けられずにナビゲーションが進行しなくなることもあります。また、計算コストもかかるため、処理をするPCの性能によってはスムーズに動作しないことがある点にも注意が必要です。
サンプルプログラムの設定
Collision Monitorに関する設定はYAMLファイルに記述します。
Raspberry Pi Mouseの場合は、raspimouse_slam_navigation_ros2パッケージ内の"raspimouse_navigation/params/raspimouse.yaml"に定義されています。
Raspberry Pi Mouseサンプルプログラム内のcollision_monitorは、以下のように設定されています。
collision_monitor:
ros__parameters:
enable_stamped_cmd_vel: True
transform_tolerance: 0.1
source_timeout: 5.0
stop_pub_timeout: 2.0
base_shift_correction: False
polygons: ["DummyPolygon"]
DummyPolygon:
type: "circle"
radius: 0.0 # valid range of dummy
action_type: "none" # No action taken.
observation_sources: ["scan"]
scan:
type: "scan"
topic: "/scan"
設定内容解説
設定内容について、いくつか抜粋して解説します。
polygons: ["DummyPolygon"]
polygonsでは衝突判定を行う領域を任意の名前で定義します。今回は、"DummyPolygon"と指定しました。
DummyPolygon:
type: "circle"
radius: 0.0 # valid range of dummy
action_type: "none" # No action taken.
polygonsで指定した領域（"DummyPolygon"）の設定を行っています。本項目の各種設定は以下の通りです。
-
-
type:
ポリゴンモデルを指定します。今回は
"circle"を指定し、ロボットの中心から円形の領域を衝突監視の対象範囲としています。他にも、多角形の形状を指定する"polygon"や、ロボットの形状をもとにした"footprint"を定義できます。 -
radius:
type: "circule"を指定しているので、radiusで領域となる円の半径を指定します（単位はメートル）。今回は、0.0と指定しているため、実質的にポリゴンの範囲は指定していないことになります。type: "polygon"と指定していた場合はpoints:<任意の数の[x,y]のポイント郡>、type: "footprint"と指定していればfootprint_topic: "<footprintのトピック名>"を指定します。 -
action_type:
動作モデルを指定します。
noneと指定しているため、何も動作は発生しません。制御停止処理のstopや、減速処理のslowdownなどがあります。
-
詳細なパラメータの設定方法は以下の公式リファレンスをご覧ください。
Raspberry Pi Mouseで試してみる
Raspberry Pi MouseでCollision Monitorの実験をします。
collision_monitorの動作が未定義であるサンプルプログラムと、停止・減速処理を追加したプログラムで、動作がどのように変化するのか確認します。
動作未定義の場合（サンプルプログラムの場合）
設定ファイルの解説項で説明した通り、Raspberry Pi Mouseの設定内容collision_monitorの機能は実質的に無効化されています。
予めSLAMを実施して作成した以下のマップを使用し、ナビゲーションを開始します。使用するLiDARはURGです。
次のコマンドでナビゲーションを開始します。
# ロボット側で以下のコマンドを実行 $ ros2 launch raspimouse_navigation robot_navigation.launch.py lidar:urg # PC側で以下のコマンドを実行 $ ros2 launch raspimouse_navigation pc_navigation.launch.py map:$HOME/my_map.yaml
Collision Monitor未設定時の動作は、以下の動画（動画時間0秒〜23秒付近）のようになりました。
ナビゲーション中のRaspberry Pi Mouseに突然人が近付いた結果、回避が間に合わずに靴に衝突してしまいました。
停止処理と減速処理を設定した場合
collision_monitorを以下のように編集し、停止処理と減速処理を追加します。
collision_monitor:
ros__parameters:
enable_stamped_cmd_vel: True
transform_tolerance: 0.1
source_timeout: 5.0
stop_pub_timeout: 2.0
base_shift_correction: False
polygons: ["StopPolygon", "SlowPolygon"]
StopPolygon:
type: "circle"
radius: 0.15
action_type: "stop"
min_points: 10
visualize: True
polygon_pub_topic: "stop_polygon"
SlowPolygon:
type: "polygon"
points: "[[0.4, 0.25], [0.4, -0.25], [-0.3, -0.25], [-0.3, 0.25]]"
action_type: "slowdown"
slowdown_ratio: 0.25
min_points: 10
visualize: True
polygon_pub_topic: "slowdown_polygon"
observation_sources: ["scan"]
scan:
type: "scan"
topic: "/scan"
主な変更点は以下のとおりです。
polygons: ["StopPolygon", "SlowPolygon"]
停止処理と減速処理を行うポリゴンを１つずつ定義しています。配列で記述することで、複数のポリゴンを定義可能です。
StopPolygon:
type: "circle"
radius: 0.15
action_type: "stop"
min_points: 10
visualize: True
polygon_pub_topic: "stop_polygon"
停止処理を行う領域を定義します。以下のような設定を追加しています。
-
-
polygon_pub_topic:ここで指定した名前のトピックでポリゴンの情報が発行され、Rvizで可視化できるようになります。今回は、
"stop_polygon"と指定しています。実際にトピックを発行するには、併せてvisualize: Trueと指定する必要があります。 -
min_points: 10障害物を検知したと判断するセンサの点群数の閾値を設定しています。今回の場合は、10個以上の点群を検知すると、減速・停止処理に入るようにしています。使用するセンサによって取得できる点群の総数は違うので、適宜自分の環境に併せた数値に調整が必要です。参照するセンサのデータは
observation_sourcesで指定します。
-
SlowPolygon:
こちらの項目では、減速処理を行う領域を定義します。
observation_sources: ["scan"]
scan:
type: "scan"
topic: "/scan"
observation_sources: ["scan"]では、本機能が参照するセンサのデータを指定します。今回は、type: "scan"とtopic: "/scan"としました。本設定項目では、障害物検知に使用するセンサのメッセージ型と発行されているトピックを使用し、今回のRaspberry Pi Mouseに搭載したLiDARのセンサデータを参照します。
先程と同様のコマンドを実行して動作を確認します。Collision Monitor設定時の動作は、以下の動画（動画時間25秒〜52付近）のようになりました。
↓動画の開始地点を25秒付近に指定しています
画面右上にあるRviz上にRaspberry Pi Mouseを囲うように青色と黄緑色のラインが描画されていることが分かります。青色がStopPolygonの領域、黄緑色がSlowPolygonの領域です。これらの中に障害物を検知すると、それぞれ減速と停止処理が行われます。
動画中の34秒付近の減速や、43秒付近で停止の動作が見られました。減速や停止により、突然現れた未知の障害物に対して衝突しないナビゲーションとなりました。
おわりに
Collision Monitorを設定し、障害物に対する減速や停止の動作を確認しました。
本機能を使用することで、障害物の位置を事前に把握できない環境でも、適切に設定すれば減速や停止の動作を行うことが可能になります。今回の検証でも、未知の障害物との衝突を防ぐナビゲーションを実現できました。ロボットのサイズや用途に応じた調整は必要ですが、YAMLファイルに設定を記述するだけで導入できるため、活用しやすい機能だと感じました。
また、今回紹介したCollision Monitorノードは、停止や減速といった予め用意された動作をするものでしたが、Collision Detectorノードを使用すれば自由に動作の定義も可能になります。例えば、指定領域内に障害物を検知するとアラートを鳴らしたりLEDを点滅させるといった動作を定義できます。Raspberry Pi MouseにはブザーやLEDが搭載されているので、こちらも色々試してみてください。
