Gazebo ionic で SDF にカメラを追加した
少し前から続いている gazebo ネタですが、シミュレーションの中にカメラを置きたくて試行錯誤しました。結果からいうと github の gz-sim に example がありそこを参考にしました。
カメラを Gazabo のワールドにいれてある特定のことをしたいのですが、自分には、 ROS / ROS2 の経験はほぼない状態なので URDF ではなくひとまず SDF で書くことにしています。そうでないと 2 重 3 重の障害となりなかなか全体の理解がすすまないだろうという判断です。
Gazebo 公式のチュートリアルにもセンサーのサンプルはありますが、あれはシステムプラグインなので <plugin> タグの中の filename をどう書いていいのかイマイチわかりませんでした。
公式ドキュメントを漁っているといつのまに Github の example にたどり着き world フォルダに色々なサンプルがあることに気づきました。
CameraSensor の例
最低限のモデルしかおいていないです。このコードのままだとどうもカメラの向きが悪いようで ワールドの中にある球体をカメラの画角に捉えられていません。が、座標適当に調整すればカメラで捉えるはずです。
<?xml version="1.0" ?>
<sdf version="1.10">
<world name="camera_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 camera-->
<model name='rgb_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='camera' type='camera'>
<always_on>1</always_on>
<update_rate>2</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>rgb</topic>
</sensor>
</link>
</model>
<!-- RGB camea end -->
<!-- load sensors plugin -->
<plugin filename='gz-sim-sensors-system'
name='gz::sim::systems::Sensors'>
</plugin>
</world>
</sdf>
カメラの画像は images フォルダに保存されます。もしかすると予めフォルダを作成する必要があるかもしれません。
gazebo と一緒に組み込まれる sensors は、どうやら gz-sim-sensors-system というファイル名で参照できるようです。
動作を確認するには、ターミナルを 2 つ立ち上げ、一方の端末でシミュレータを起動します。
$ gz sim camera.sdfもう一方の端末でトピックをリッスンします。
gz topic -e -t /rgbシミュレータの左下の開始？再生？ボタンをクリックすると、トピックを聴いている端末なにやらデータが流れ出し images フォルダにカメラが捉えた映像が保存されます。
depth_camera の例
基本的に、通常のカメラと同じなのですが、まずは sdf を提示します。
<?xml version="1.0" ?>
<sdf version="1.10">
<world name="depth_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>
<!-- depth camera-->
<model name='depth_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='depth_camera' type='depth_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>L8</format>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
<save enabled='true'>
<path>images</path>
</save>
</camera>
<topic>depth</topic>
</sensor>
</link>
</model>
<!-- depth camea end -->
<!-- load sensors plugin -->
<plugin filename='gz-sim-sensors-system'
name='gz::sim::systems::Sensors'>
</plugin>
</world>
</sdf>
先程の camera_sensor のときとほぼ同じですが、 <sensor> の type 属性がdepth_camera になっています。更に距離情報はカラーだと困るので、SDFormat の仕様を見て適当に L8 としたがこれが正しいのかは全くわかっていません。
一応つぶやく トピック depth に変えていますが、まあ変えなくてもいいです。
動作確認はcamera_sensor のときと同様におこないますが、 リッスンするトピックと SDF 内で指定した depth にしてやる必要があります。
ようやく、カメラデータの吐き出しまでできました。
とはいえ、rgbd_camera は同じようにやってもダンマリなのでなかなか先に進めない。同じフォルダにある sensors_demo.sdf には rgbd_camera の例があるのでこれを参考にやってみるか。
参考
gz-sim/examples/worlds at gz-sim9 · gazebosim/gz-sim
g