Gazeboでディファレンシャルドライブ、カメラをシミュレートする (ROS2-Foxy)

はじめに
前提条件
IMUとLIDARについて
ディファレンシャルドライブについて
URDFにディファレンシャルドライブプラグインを追加
URDFにカメラプラグインを追加
Gazeboでカメラをシミュレート
おわりに

はじめに

前回の記事ではURDFファイルを Rviz2, Gazebo で表示する方法を説明しました。

今回はシミュレーションに必ず必要となる IMU, LIDAR, カメラをシミュレートする方法を説明します。

また、Gazebo で色を指定する方法も説明します。

前提条件

前提条件は以下の通りです。

URDF ファイルを Rviz2, Gazebo で表示できていること
URDF ファイルにIMU, LIDAR, カメラの link と joint が追加されていること

IMUとLIDARについて

IMU についてはWikipediaのこちらのページを参考にしてください。
今回、Gazebo で取り扱う IMU は3軸の加速度センサと3軸のジャイロによって構成されており、出力周期等の細かいパラメータを URDF 上で指定することができます。

これによって、

どの方向に進んでいるか
どの方向を向いているか

が、ある程度分かります。(IMU単体では方向が不安定です)

LIDAR についてはローム株式会社のサイトが分かりやすくまとまっています。
今回、Gazebo で取扱う LIDAR は2Dレーザータイプのものであり、IMU と同様に出力周期等の細かいパラメータをURDF上で指定することができます。

これによって

どこに障害物があるか
格子地図上のどこにロボットがいるかをセンサパターンから把握

することができます。(LIDARや地図によっては”退化”が起こる可能性があります)
今回の連載の最後には退化が起きている様子を観察することができます。

ディファレンシャルドライブについて

センサ類によってロボットの位置や進んでいる方向を特定することができますが、肝心の駆動が今のままではできません。
今回のロボットはWikipediaのこちらのページを基に駆動します。

ROS2では diff_drive_controller というパッケージが用意されています。
ROS公式の解説ページはこちらになります。Gazebo公式の解説ページはこちらです。

これによって

どの方向にどれだけ進んだか

が、分かるようになります。

URDFにディファレンシャルドライブプラグインを追加

早速、URDFにセンサ類のプラグインを追加していきます。

まずは、ディファレンシャルドライブのプラグインを追加します。
first_robot.urdf の</robot>の上に以下のコードを追加してください。

<!-- front diff drive status -->
<gazebo>
    <plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so">
        <ros>
            <remapping>cmd_vel:=cmd_vel</remapping>
            <namespace>/first_robot</namespace>
            <remapping>odom:=odom</remapping>
        </ros>
        <left_joint>wheel_front_left_joint</left_joint>
        <right_joint>wheel_front_right_joint</right_joint>
        <wheel_separation>0.4</wheel_separation>
        <wheel_diameter>0.15</wheel_diameter>
        <max_wheel_torque>20</max_wheel_torque>
        <max_wheel_acceleration>1.0</max_wheel_acceleration>
        <publish_odom>true</publish_odom>
        <publish_odom_tf>false</publish_odom_tf>
        <publish_wheel_tf>true</publish_wheel_tf>
        <update_rate>30</update_rate>

        <odometry_topic>odom</odometry_topic>
        <odometry_frame>odom</odometry_frame>
        <base_frame_id>base_footprint</base_frame_id>
        <robot_base_frame>base_link</robot_base_frame>
        <command_topic>cmd_vel</command_topic>
    </plugin>
</gazebo>

上から説明していきます。

<gazebo>
    <plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so">

<gazebo>タグ内は Gazebo へ適用する内容を示します。
<plugin>タグは Gazebo へ適用するプラグインを指定します。今回は libgazebo_ros_diff_drive.so というディファレンシャルドライブ用のプラグインを使用します。

<ros>
    <remapping>cmd_vel:=cmd_vel</remapping>
    <namespace>/first_robot</namespace>
    <remapping>odom:=odom</remapping>
</ros>

<ros>タグでは ROS2 の topic名を設定します。
<remapping>xxx:=yyy</remapping>はプラグインが発行する topic /xxx を /yyyにして発行します。
<namespace>/zzz</namespace>ではプラグインが発行する topic の頭に/zzzを付けます。
上記の例では、

/first_robot/cmd_vel
/first_robot/odom

が、発行されるようになります。 cmd_vel と odom は後程説明します。

<left_joint>wheel_front_left_joint</left_joint>
<right_joint>wheel_front_right_joint</right_joint>

こちらはディファレンシャルドライブで動かす左右の車輪の joint を指定します。

次は、左右の車輪のパラメータを記載します。

<wheel_separation>0.4</wheel_separation>
<wheel_diameter>0.15</wheel_diameter>
<max_wheel_torque>20</max_wheel_torque>
<max_wheel_acceleration>1.0</max_wheel_acceleration>

<wheel_separation>は左右車輪の距離を指定します。
<wheel_diameter>は車輪の直径を指定します。
URDFは半径を指定しますが、こちらでは直径を指定するので注意が必要です。
<wheel_radius>を使用すると車輪の半径を指定することができます。
<max_wheel_torque>と<max_wheel_acceleration>は車輪のトルクと加速度を指定します。
20と1.0は公式の数値なので、そのまま使用します。

次はオドメトリ(走行距離計測法による位置推定)関係の記述です。

<publish_odom>true</publish_odom>
<publish_odom_tf>false</publish_odom_tf>
<publish_wheel_tf>true</publish_wheel_tf>
<update_rate>30</update_rate>

<publish_odom>と<publish_odom_tf>はオドメトリの結果を出力するかどうか指定します。
<publish_odom_tf>は他の tf と競合する可能性があるため、false にしておきます。
<publish_wheel_tf>は左右車輪の tf を出力するかどうか指定できます。
Rviz2で表示するために、true にしておきます。
<update_rate>は出力の間隔を Hz で指定します。この場合、30Hz です。

<odometry_topic>odom</odometry_topic>
<odometry_frame>odom</odometry_frame>
<base_frame_id>base_footprint</base_frame_id>
<robot_base_frame>base_link</robot_base_frame>
<command_topic>cmd_vel</command_topic>

<odometry_topic>は topic 名の指定、<odometry_frame>は<remapping>タグのodomに合わせてください。
<base_frame_id>は計算したオドメトリを適用する link を指定します。
<robot_base_frame>はどの link を基にオドメトリを計算するかを指定します。
<command_topic>は<remapping>タグの cmd_vel に合わせてください。

URDFにカメラプラグインを追加

次に、カメラのプラグインを追加します。

コードは以下の通りです。

<!-- camera plugin -->
<gazebo reference="camera_link">
    <sensor type="camera" name="camera1">
        <alwaysOn>true</alwaysOn>
        <visualize>true</visualize>
        <update_rate>30.0</update_rate>
        <camera name="head">
            <horizontal_fov>1.3962634</horizontal_fov>
            <image>
                <width>800</width>
                <height>800</height>
                <format>R8G8B8</format>
            </image>
            <clip>
                <near>0.02</near>
                <far>300</far>
            </clip>
            <noise>
                <type>gaussian</type>
                <mean>0.0</mean>
                <stddev>0.007</stddev>
            </noise>
        </camera>
        <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
            <cameraName>/camera</cameraName>
            <imageTopicName>image_raw</imageTopicName>
            <cameraInfoTopicName>camera_info</cameraInfoTopicName>
            <frameName>camera_link</frameName>
            <hackBaseline>0.07</hackBaseline>
        </plugin>
    </sensor>
    <material>Gazebo/Blue</material>
</gazebo>

上から順に説明していきます。

<gazebo reference="camera_link">
<sensor type="camera" name="camera1">

<gazebo reference=”xxx”>は xxxのリンクに Gazeboプラグインを適用することを意味しています。
<sensor_type>でどのセンサを使用するか示します。この場合はカメラセンサを使用します。

<alwaysOn>true</alwaysOn>
<visualize>true</visualize>
<update_rate>30.0</update_rate>

<alwaysOn>はロボットが Gazebo に表示されるのと同時にカメラを起動します。
<visualize>はカメラの映像を Gazebo 内に表示するかどうかを指定します。
<update_rate>は更新周期です。今回の場合は 30fps を指定します。

<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>

<camera name=”xxx”>は Gazebo 内でカメラを xxx という名称で取扱います。
<horizontal_fov>は画角を表し、1.396rad = (1.396 * 180) / 3.14 ≒ 80° となります。

<image>
  <width>800</width>
  <height>800</height>
  <format>R8G8B8</format>
</image>

<image>タグはカメラの画像の設定を行います。この場合、800×800 の解像度を持つカメラとなります。
<format>はカメラのピクセルのRGB値の型を指定します。この場合、全て int8 です。

<clip>
  <near>0.02</near>
  <far>300</far>
</clip>

<clip>タグはカメラから near より近い部分と far より遠い部分を映さないようにできます。
この場合、カメラは0.02m ~ 300m を映します。

<noise>
  <type>gaussian</type>
  <mean>0.0</mean>
  <stddev>0.007</stddev>
</noise>

<noise>タグはカメラのノイズを指定します。カメラのピクセル値に直接ノイズを加えます。
この場合、平均0、標準偏差0.007 のガウシアン分布のノイズに従います。

<plugin name="camera_controller" filename="libgazebo_ros_camera.so">

これは Gazebo へ適用するプラグインを指定します。

<cameraName>/camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>

<cameraName>は topic の先頭につける文字列を指定します。
<imageTopicName>はカメラの映像データが入る topic 名を指定します。
<cameraInfoTopicName>はカメラ自体の情報が入る topic 名を指定します。
カメラの topic を受取りたい場合、

/camera/camera_info ・・・カメラ自体の情報
/camera/image_raw ・・・カメラの映像データ

を、取得することができます。

<frameName>は URDF のカメラの link を指定します。
<hackBaseline>は、公式を調べたのですが分かりませんでした。。。
おそらく、何かの閾値であると思います。分かり次第追記します。

<material>Gazebo/Blue</material>

<material>タグは Gazebo に表示する際の色を指定できます。この場合はカメラが青色になります。

Gazeboでカメラをシミュレート

少し長くなりすぎたので、IMUとLIDARは次回します。
最後にGazebo 上でカメラをシミュレートして今回の記事は完了とさせていただきます。

launch ファイルは前回作成した first_gazebo.launch.py を使用してください。
まずは、パッケージをビルドしてsetup.bashを読み込みます。

cd ~/ros_ws
colcon build --packages-select ros2_first_test
source ~/.bashrc
ros2 launch ros2_first_test first_gazebo.launch.py

Rviz2も表示されますが、 Gazebo の画面のみ確認をしてください。

このように表示されていれば問題ありません。
また、コンソール画面に

[rviz2-3] Warning: TF_OLD_DATA ignoring data from the past for frame wheel_front_left at time 104.245000 according to authority Authority undetectable

と、表示されているかもしれませんが、これはシミュレーション時間内の各 topic の時間が合致しないことにより発生します。
その場合、下記画像の赤枠をクリックすると