Gazebo turtlebot3 burger Depth Camera plugin

早速ファイルの作成に入ります。

1. ~/catkin_ws/src/turtlebot3/turtlebot3_description/urdf/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> ... <!-- 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>

注) サンプルをそのまま使うと、レーザー(PointCloud2) が天空になっているので、下記修正を加えます。
1)
<origin xyz="${xyz}" rpy="${radians(-90)} 0 ${radians(-90)}"/> <!-- changed by nishi -->
2)
<pose>0 0 0 0 ${radians(-90)} ${radians(90)}</pose> <!-- add by nishi -->

2. ~/catkin_ws/src/turtlebot3/turtlebot3_description/urdf/turtlebot3_burger.urdf.xacro へ追記。

<?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"/> .... <!-- add by nishi --> <!-- <xacro:stereo_camera_macro prefix="stereo_camera" parent="base_link" xyz="0.06 0.0 0.08"/> <xacro:camera_macro prefix="camera" parent="base_link" xyz="0.09 0.0 0.08"/> --> <xacro:rgbd_camera_macro2 prefix="camera" parent="base_link" xyz="0.04 0.0 0.12"/> </robot>

3. 動作確認。
$ roscore

$ export TURTLEBOT3_MODEL=burger
$ roslunch turtlebot3_gazebo turtlebot3_house.launch

$ rqt_image_view で確認
 Fixed Frame: odom
 add > RobotModel
 add > By Topic > /camera > /depth > /image_raw > Image
 add > By Topic > /camera > /depth > /points > PointCloud2

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