今までは、”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を出力するコードを作成する必要がある。