ROS入門 (52) - Unityによるナビゲーション用の地図の作成

2022年1月10日 07:30

Unityによるナビゲーション用の地図の作成の手順をまとめました。

・Noestic
・Unity 2020.3

前回

1. ナビゲーション用の地図

ROSの移動ロボットの主なタスクとして「ナビゲーション」があります。ゴール地点を指定することで、その場所まで障害物を避けながら移動するタスクになります。

ナビゲーションには地図が必要で、「グレースケール画像」と「YAMLファイル」のペアで表現します。「グレースケール画像」は、濃い色ほど障害物で占有される確率が高いことを示します。「YAMLファイル」は、セルのメートル単位の大きさ、地図の原点の場所、セルが障害物で占有されているかどうかを決めるための閾値などを記述します。

・map.pgm

・map.yaml

image: map.pgm
resolution: 0.050000
origin: [-100.000000, -100.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

・image　: 画像ファイル
・resolution : 解像度 (meters/pixel)
・origin : マップの左下端の座標 (x, y, θ)
・occupied_thresh : この値より大きいものを障害物とみなす
・free_thresh : この値より小さいものをフリースペースとみなす
・negate : 白黒反転 (0/1)

2. 実験環境の準備

ナビゲーション用の地図は、ロボットを実際に走らせて収集した、ロボットの姿勢(/tf)とレーザースキャナー(/scan)から生成することができます。

前回ROS2用に作成した環境をROS1用に修正して、実験環境を準備します。

(1) ROS1のエンドポイントの準備。
「ROS入門 (16) - ROS-Unity間のトピックによる通信」と同様です。
(2) 地図の作成環境の準備。
「ROS入門 (51) - toio風ロボットをURDFで作成して、Unityに配置して、Nav2で操作」と同様の環境を準備します。

(3) 「Robotics → ROS Settings」で「ROS1」を選択。

(4) ROSPublishersの「Global FrameIds」に「map」を追加。
地図の作成時には必要ありませんが、ナビゲーションで必要になるので、ROS1対応しておきます。

(5) UnityのPlayボタンで実行。
ROS1とROS2ではTimeMsgとHeaderMsgの型が若干異なるのでエラーが発生します。
(6) エラーメッセージに従って、TimeMsgとHeaderMsgの型をROS1向けに修正。

・TimeMsgのsec
int → uint

public static implicit operator TimeMsg(TimeStamp stamp)
{
    return new TimeMsg(stamp.Seconds, stamp.NanoSeconds);
}

public static implicit operator TimeStamp(TimeMsg stamp)
{
    return new TimeStamp(stamp.sec, stamp.nanosec);
}
    ↓
public static implicit operator TimeMsg(TimeStamp stamp)
{
    return new TimeMsg((uint)stamp.Seconds, stamp.NanoSeconds);
}

public static implicit operator TimeStamp(TimeMsg stamp)
{
    return new TimeStamp((int)stamp.sec, stamp.nanosec);
}

・HeaderMsgのコンストラクタ
HeaderMsg(BuiltinInterfaces.TimeMsg stamp, string frame_id)→
HeaderMsg(uint seq, BuiltinInterfaces.TimeMsg stamp, string frame_id)

new HeaderMsg(new TimeStamp(Clock.time), m_GlobalFrameIds[i - 1]),
    ↓

private int count = 0;
   :
new HeaderMsg(count++, new TimeStamp(Clock.time), m_GlobalFrameIds[i - 1]),

(7) 再度実行して、ROSと接続されること（ROS IPの矢印が青）を確認。

3. ロボットの姿勢(/tf)とレーザースキャナー(/scan)の収集

「rosbag」を使って、ロボットの姿勢(/tf)とレーザースキャナー(/scan)を収集します。

(1) 「rostopic list」で「/tf」と「/scan」が配信されてることを確認。

$ rostopic list
/clock
/cmd_vel
/map
/rosout
/rosout_agg
/scan
/tf

(2) 「rosbag」でロボットの姿勢(/tf)とレーザースキャナー(/scan)の情報を収集を開始。

$ rosbag record -O data.bag /scan /tf

(3) Unity環境の移動ロボットを操作して、全ての道を巡回。

(4) rosbagをCtrl-Cで終了。
収集されたロボットの姿勢(/tf)とレーザースキャナー(/scan)の情報は、「data.bag」に保存されます。
(5) 「rosbag info <bagファイル名>」で収集情報の確認。

$ rosbag info data.bag
path:        data.bag
version:     2.0
duration:    41.4s
start:       Jan 09 2022 20:55:23.85 (1641761723.85)
end:         Jan 09 2022 20:56:05.28 (1641761765.28)
size:        335.1 KB
messages:    618
compression: none [1/1 chunks]
types:       sensor_msgs/LaserScan [90c7ef2dc6895d81024acba2ac42f369]
             tf2_msgs/TFMessage    [94810edda583a504dfda3829e70d7eec]
topics:      /scan   205 msgs    : sensor_msgs/LaserScan
             /tf     413 msgs    : tf2_msgs/TFMessage

4. 収集したデータを地図に変換

収集したデータを地図に変換します。

(1) 地図作成用のパッケージのインストール

$ sudo apt update
$ sudo apt install ros-noetic-slam-gmapping ros-noetic-map-server

(2) bagファイルに記録されたタイムスタンプの利用することを指定。

$ rosparam set use_sim_time true

(3) slamp_mappingを実行。
slamp_mappingは、/tfと/scanを受信して地図を作成します。

$ rosrun gmapping slam_gmapping

(4) bagファイルを使って、/tfと/scanを配信。
配信完了するまで待ちます。

$ rosbag play --clock data.bag

(5) 「map_server」の「map_saver」で地図を保存。
保存完了するまで、slam_gmappingは起動したままにしておいてください。「map_server」と「map_saver」はつづりは似てるので注意してください。

$ rosrun map_server map_saver

「map.pmg」と「map.yaml」が生成されます。

5. 地図の配信

地図を配信する方法は、次のとおりです。

(1) 地図の配信開始。
/mapに地図が配信されます。「map_server」と「map_saver」はつづりは似てるので注意してください。

$ rosrun map_server map_server map.yaml

(2) UnityのVisualizationsで/mapを表示。

次回

この記事が気に入ったらサポートをしてみませんか？