Gazebo のセンサーが publish しているトピックを調べる方法

前回の記事では camera_sensor と depth_camera を取り上げましたが、その際 rgbd_camera が上手く扱えなかったのですが、どうやら rgbd_camera は / では何も広告していないことが sensors_demo.sdf をみてわかりました。

センサーがどんなトピックを広告するかを調べる方法を調べました。 センサーが広告するトピックがドキュメントで見つけられなかったのですが、その理由の一旦がわかりました。

トピックを確認する方法

まずは、何でもいいので正しく構成された .sdf を読み込んでおきます。

別の端末から以下のコマンドを実行します。ここでは /rgbd が rgbd_camera が publish しているトピックですが、
その下に、camera_info、depth_image などがあることがわかります。

$ gz topic -l
/clock
/gazebo/resource_paths
/gui/camera/pose
/gui/currently_tracked
/gui/track
/rgbd/camera_info
/rgbd/depth_image
/rgbd/image
/rgbd/points
/sensors/marker
/stats
/world/rgbd_world/clock
/world/rgbd_world/dynamic_pose/info
/world/rgbd_world/pose/info
/world/rgbd_world/scene/deletion
/world/rgbd_world/scene/info
/world/rgbd_world/state
/world/rgbd_world/stats
/world/rgbd_world/light_config
/world/rgbd_world/material_color

上記は、この .sdf を読み込んだ結果です

<?xml version="1.0" ?>
<sdf version="1.10">
  <world name="rgbd_world">
    <plugin
        filename="gz-sim-physics-system"
        name="gz::sim::systems::Physics">
    </plugin>

    <physics name="1ms" type="ignored">
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1.0</real_time_factor>
    </physics>
    <gravity>0 0 -9.8</gravity>

    <light type='directional' name='sun'>
      <cast_shadows>true</cast_shadows>
      <pose>0 0 10 0 0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
    </light>

    <model name="ground_plane">
        <static>true</static>
        <link name="link">
            <collision name="collision">
            <geometry>
                <plane>
                <normal>0 0 1</normal>
                </plane>
            </geometry>
            </collision>
            <visual name="visual">
            <geometry>
                <plane>
                <normal>0 0 1</normal>
                <size>100 100</size>
                </plane>
            </geometry>
            <material>
                <ambient>0.8 0.8 0.8 1</ambient>
                <diffuse>0.8 0.8 0.8 1</diffuse>
                <specular>0.8 0.8 0.8 1</specular>
            </material>
            </visual>
        </link>
    </model>

    <model name='sphere1'>
      <pose>1.5 1.5 0.5  0 0 0</pose>
      <link name='link'>
        <collision name='collision'>
          <geometry>
            <sphere>
              <radius>0.5</radius>
            </sphere>
          </geometry>
        </collision>
        <visual name='visual'>
          <geometry>
            <sphere>
              <radius>0.5</radius>
            </sphere>
          </geometry>
        </visual>
      </link>
    </model>

    <!-- RGB-D camera-->
    <model name='rgbd_camera'>
      <pose>0 0 0.5  0 0 0</pose>
      <link name='link'>
        <inertial>
          <mass>0.1</mass>
          <inertia>
            <ixx>0.000166667</ixx>
            <iyy>0.000166667</iyy>
            <izz>0.000166667</izz>
          </inertia>
        </inertial>

        <collision name='collision'>
          <geometry>
            <box>
              <size>0.1 0.1 0.1</size>
            </box>
          </geometry>
        </collision>

        <visual name='visual'>
          <geometry>
            <box>
              <size>0.1 0.1 0.1</size>
            </box>
          </geometry>
          <material>
            <ambient>0.1 0.1 0.1 1</ambient>
            <diffuse>0.1 0.1 0.1 1</diffuse>
            <specular>0.01 0.01 0.01 1</specular>
          </material>
        </visual>
        
        <sensor name='rgbd_camera' type='rgbd_camera'>
          <always_on>1</always_on>
          <update_rate>1</update_rate>
          <visualize>true</visualize>
          <camera>
            <horizontal_fov>1.047</horizontal_fov>
            <image>
              <width>640</width>
              <height>480</height>
              <!-- <format>R8G8B8</format> -->
            </image>
            <clip>
              <near>0.1</near>
              <far>100</far>
            </clip>
            <save enabled='true'>
              <path>images</path>
            </save>
          </camera>
          <topic>rgbd</topic>
        </sensor>
      </link>
    </model>
    <!-- RGB-D camea end -->

    <!-- load sensors plugin  -->
    <plugin filename='gz-sim-sensors-system'
      name='gz::sim::systems::Sensors'>
    </plugin>
  </world>

</sdf>

ROS / ROS2 を使っている人にとっては、ノードが発信している情報を M2M 的なアプローチで外部から確認すれば OK っていう方式になじみがあるのだろうが、gazebo のドキュメントにはあまり書いていない気がするのはちょっとなーって文句の 1 つもいいたい。