Gazebo turtlebot3 burger stereo camera plugin

1. プラグイン作成の手順は、最近覚えたのですが、
1) urdf(xacro) で、 Rviz の表示で、プラグインの筐体とする、<link></link> と、
それを親link に接続する、<joint></joint> を作ります。
ここで、例えば、
カメラの筐体となる、ボックス型の link を定義します。
  box のサイズ、向き、色 の定義
joint タグで、その筐体ボックスを、親の筐体(リンク) のどこに配置するかを、
記述します。

2)  1) で作った<link name="xxxx"> の xxxx名 を指定して、<gazebo></gazebo> タグの中で、
<plugin name="xxxx" filename="yyyyy.so"></plugin> と言う様に、名前を一致させて、
filename="yyyyy.so" で、公開されているプラグインを指定して使います。

なおかつ、前述のサンプル、sim2_lecture/xacro/camera_macro.xacro は、
<xacro:macro name="camera_macro" params="parent prefix xyz"> は、マクロを
定義して、その中で、urdf の部分と gazeb の部分を同時に記述しています。

2. マルチカメラのプラグインのサンプルは、下記にあります。
No.6-6:GazeboをROSに繋ぐ (ROSにおけるGazeboのプラグイン 編)
>マルチカメラ
このページにも、プラグインの作り方があるので、参考になります。
この、ページの、サンプルコードを、
camera_macro.xacro に取り込んで、モノカメラ用とステレオカメラ用の2つのマクロを
作れば、便利です。

早速取り込んで作ってみます。
camera_macro.xacro
<?xml version="1.0"?> <robot name="turtlebot3_burger_cam" xmlns:xacro="http://ros.org/wiki/xacro"> <material name="red2"> <color rgba="1.0 0.0 0.0 2.0"/> </material> <xacro:macro name="camera_macro" params="parent prefix xyz"> <joint name="${prefix}_joint" type="fixed"> <parent link="${parent}"/> <child link="${prefix}_link"/> <origin xyz="${xyz}" rpy="${radians(-90)} 0 ${radians(-90)}"/> </joint> <link name="${prefix}_link"> <visual> <geometry> <box size="0.03 0.03 0.03" /> </geometry> <origin xyz="0 -0.03 -0.07" rpy="0 0 0 " /> <material name="red2"/> </visual> </link> <gazebo reference="${prefix}_link"> <material>Gazebo/Red</material> <sensor type="camera" name="${prefix}"> <update_rate>10.0</update_rate> <camera name="${prefix}"> <pose>0 0 0 0 ${radians(-90)} ${radians(90)}</pose> <horizontal_fov>1.3962634</horizontal_fov> <image> <width>800</width> <height>600</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>${prefix}</cameraName> <imageTopicName>image_raw</imageTopicName> <cameraInfoTopicName>camera_info</cameraInfoTopicName> <frameName>${prefix}_link</frameName> <hackBaseline>0.0</hackBaseline> <distortionK1>0.0</distortionK1> <distortionK2>0.0</distortionK2> <distortionK3>0.0</distortionK3> <distortionT1>0.0</distortionT1> <distortionT2>0.0</distortionT2> </plugin> </sensor> </gazebo> </xacro:macro> <!-- prefix = stereo_camera --> <xacro:macro name="stereo_camera_macro" params="parent prefix xyz"> <joint name="${prefix}_joint" type="fixed"> <parent link="${parent}"/> <child link="${prefix}_frame"/> <!-- <origin xyz="${xyz}" rpy="0 0 0"/> --> <origin xyz="${xyz}" rpy="0 -0.02967 0"/> <!-- -1.7[deg] --> </joint> <link name="${prefix}_frame"> <visual> <geometry> <box size="0.03 0.06 0.03" /> </geometry> <origin xyz="0 0 0" rpy="0 0 0 " /> <material name="red2"/> </visual> </link> <gazebo reference="${prefix}_frame"> <material>Gazebo/Red</material> <sensor type="multicamera" name="${prefix}"> <update_rate>30.0</update_rate> <camera name="left"> <pose>0 0.035 0 0 0 0</pose> <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> <camera name="right"> <pose>0 -0.035 0 0 0 0</pose> <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="stereo_camera_controller" filename="libgazebo_ros_multicamera.so"> <alwaysOn>true</alwaysOn> <updateRate>0.0</updateRate> <cameraName>${prefix}</cameraName> <imageTopicName>image_raw</imageTopicName> <cameraInfoTopicName>camera_info</cameraInfoTopicName> <frameName>${prefix}_optical_frame</frameName> <!--<rightFrameName>right_camera_optical_frame</rightFrameName>--> <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> </gazebo> </xacro:macro> <!-- rgbd_camera_macro2 prefix = rgbd_camera --> <!-- http://gazebosim.org/tutorials?tut=ros_gzplugins --> <!-- http://gazebosim.org/tutorials/?tut=ros_depth_camera --> <xacro:macro name="rgbd_camera_macro2" params="parent prefix xyz"> <joint name="${prefix}_joint" type="fixed"> <parent link="${parent}"/> <child link="${prefix}_link"/> <!-- <origin xyz="${xyz}" rpy="0 0 0"/> --> <origin xyz="${xyz}" rpy="${radians(-90)} 0 ${radians(-90)}"/> <!-- changed by nishi --> </joint> <link name="${prefix}_link"> <visual> <geometry> <box size="0.03 0.03 0.03" /> </geometry> <origin xyz="0 0 0" rpy="0 0 0 " /> <material name="red2"/> </visual> </link> <gazebo reference="${prefix}_link"> <material>Gazebo/Red</material> <sensor name="${prefix}_camera" type="depth"> <update_rate>20</update_rate> <camera> <pose>0 0 0 0 ${radians(-90)} ${radians(90)}</pose> <!-- add by nishi --> <horizontal_fov>1.047198</horizontal_fov> <image> <width>640</width> <height>480</height> <!-- <format>R8G8B8</format> --> <format>B8G8R8</format> </image> <clip> <near>0.05</near> <far>3</far> </clip> </camera> <plugin name="${prefix}_controller" filename="libgazebo_ros_openni_kinect.so"> <baseline>0.2</baseline> <alwaysOn>true</alwaysOn> <updateRate>1.0</updateRate> <cameraName>camera_ir</cameraName> <imageTopicName>/camera/rgb/image_raw</imageTopicName> <cameraInfoTopicName>/camera/rgb/camera_info</cameraInfoTopicName> <depthImageTopicName>/camera/depth/image_raw</depthImageTopicName> <depthImageInfoTopicName>/camera/depth/camera_info</depthImageInfoTopicName> <pointCloudTopicName>/camera/depth/points</pointCloudTopicName> <frameName>camera_link</frameName> <!-- <pointCloudCutoff>0.5</pointCloudCutoff> --> <pointCloudCutoff>0.2</pointCloudCutoff> <!-- chaned by nishi --> <pointCloudCutoffMax>3.0</pointCloudCutoffMax> <distortionK1>0.00000001</distortionK1> <distortionK2>0.00000001</distortionK2> <distortionK3>0.00000001</distortionK3> <distortionT1>0.00000001</distortionT1> <distortionT2>0.00000001</distortionT2> <CxPrime>0</CxPrime> <Cx>0</Cx> <Cy>0</Cy> <focalLength>0</focalLength> <hackBaseline>0</hackBaseline> </plugin> </sensor> </gazebo> </xacro:macro> </robot>
注1) カメラが少し下向きなので、上に向けました。by nishi 2022.7.10
<origin xyz="${xyz}" rpy="0 -0.02967 0"/> <!-- -1.7[deg] -->

3. gazebo/turtlebot3 burger への取り込み。
ここで、gazebo turtlebot3 のファイル構成をみてみます。
起動の流れ
$ roslaunch turtlebot3_gazebo turtlebot3_wolrd.launch

turtlebot3_simulation/turtlebot3_gazebo/launch/
  turtlebot3_world.launch が起動 launch で、
    |
turtlebot3/turtlebot3_description/urdf/
 turtlebot3_$(arg model).urdf.xacro が launchファイルからスポーンされます。
注) $(arg model) は、 burger, waffle, waffle_pi になる。

なので、今回の目標は、burger へのステレオカメラの導入なので、
turtlebot3_burger.urdf.xacro へ今回作成した、 camera_macro.xacro を
インクルードして、マクロコールしてあげれば、OK となります。

3.1 camera_macro.xacro ファイルを
~/catkin_ws/src/turtlebot3/turtlebot3_description/urdf/ へセット。

3.2 turtlebot3_burger.urdf.xacro の修正。
( ~/catkin_ws/src/turtlebot3/turtlebot3_description/urdf/turtlebot3_burger.urdf.xacro )
上記ファイルに、camera_macro.xacro をインクルードして、
<xacro:stereo_camera_macro...../>
を記述すれば、OKです。

<?xml version="1.0" ?> <robot name="turtlebot3_burger" xmlns:xacro="http://ros.org/wiki/xacro"> <xacro:include filename="$(find turtlebot3_description)/urdf/common_properties.xacro"/> <xacro:include filename="$(find turtlebot3_description)/urdf/turtlebot3_burger.gazebo.xacro"/> <xacro:include filename="$(find turtlebot3_description)/urdf/camera_macro.xacro"/> .... <xacro:stereo_camera_macro prefix="stereo_camera" parent="base_link" xyz="0.06 0.0 0.08"/> </robot> </texarea> </p>

4. 動作確認。
$ roscore

$ export TURTLEBOT3_MODEL=burger
$ roslaunch turtlebot3_gazebo turtlebot3_house.launch

$ rqt_image_view で確認
 /stereo_camera/left/image_raw
 /stereo_camera/right/image_raw

または、rviz で
$ rviz
add > by Topic > /streo_camera > lefft > image_raw - Image
add > by Topic > /streo_camera > right > image_raw - Image

どうぞね、上手くいったかね!