ROS2 Foxy環境でgazeboを使ったデプスセンサのシミュレーション

今までは、"libgazebo_ros_openni_kinect.so" を使ってデプスセンサをシミュレーションしていた。Gazeboの公式チュートリアルでも"libgazebo_ros_openni_kinect.so"を使った方法が記載されている。

http://gazebosim.org/tutorials/?tut=ros_depth_camera

しかし、Foxy環境では"libgazebo_ros_openni_kinect.so"がなくなっている。かわりに"libgazebo_ros_camera.so"でデプスカメラのシミュレーションが可能になっているようである。type="depth"とすれば良い。

<!-- Depth camera -->
<link name="camera_link">
  <pose>0 0 1.2 0 0.349066 0</pose>
  <inertial>
    <mass>0.01</mass>
    <pose>0 0 0 0 -0 0</pose>
    <inertia>
      <ixx>1.8e-7</ixx>
      <ixy>0</ixy>
      <ixz>0</ixz>
      <iyy>2.1733e-6</iyy>
      <iyz>0</iyz>
      <izz>1.8e-7</izz>
    </inertia>
  </inertial>
  <collision name="collision">
    <geometry>
      <box>
        <size>0.03 0.08 0.03</size>
      </box>
    </geometry>
  </collision>
  <visual name="camera_visual">
    <geometry>
      <box>
        <size>0.03 0.08 0.03</size>
      </box>
    </geometry>
    <material>
      <script>
        <name>Gazebo/DarkGrey</name>
        <uri>file://media/materials/scripts/gazebo.material</uri>
      </script>
    </material>
  </visual>
  <sensor type="camera" name="camera">
    <update_rate>30.0</update_rate>
    <camera name="head">
      <horizontal_fov>1.5708</horizontal_fov>
      <image>
        <width>1280</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">
      <alwaysOn>true</alwaysOn>
      <updateRate>0.0</updateRate>
      <cameraName>momo/camera</cameraName>
      <imageTopicName>image_raw</imageTopicName>
      <cameraInfoTopicName>camera_info</cameraInfoTopicName>
      <frameName>camera_link</frameName>
      <hackBaseline>0.07</hackBaseline>
      <distortionK1>0.0</distortionK1>
      <distortionK2>0.0</distortionK2>
      <distortionK3>0.0</distortionK3>
      <distortionT1>0.0</distortionT1>
      <distortionT2>0.0</distortionT2>
    </plugin>
  </sensor>
  <sensor type="depth" name="depth">
    <update_rate>30.0</update_rate>
    <camera name="head">
      <horizontal_fov>1.51844</horizontal_fov>
      <image>
        <width>1280</width>
        <height>720</height>
        <format>R8G8B8</format>
      </image>
      <clip>
        <near>0.5</near>
        <far>6.0</far>
      </clip>
      <noise>
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.07</stddev>
      </noise>
    </camera>
    <plugin name="depth_controller" filename="libgazebo_ros_camera.so">
      <visualize>true</visualize>
      <alwaysOn>true</alwaysOn>
      <updateRate>0.0</updateRate>
      <cameraName>depth/camera</cameraName>
      <imageTopicName>image_raw</imageTopicName>
      <cameraInfoTopicName>camera_info</cameraInfoTopicName>
      <frameName>camera_link</frameName>
      <hackBaseline>0.07</hackBaseline>
      <distortionK1>0.0</distortionK1>
      <distortionK2>0.0</distortionK2>
      <distortionK3>0.0</distortionK3>
      <distortionT1>0.0</distortionT1>
      <distortionT2>0.0</distortionT2>
    </plugin>
  </sensor>
</link>

センサーのタイプをdepthに設定することでpointcloud2を得られた。今回は通常のカメラと深度カメラの2つのセンサーを追加している。

depthの方にもいちおうnoiseタグを入れてみたが、ノイズには対応していないとのことだった。実際の出力を見てもやはりノイズは得られなかった。自分で、pointcloud2を受け取り、ガウシアンノイズを追加したpointcliud2を出力するコードを作成する必要がある。