1.现象
在参考ros1的gazebo深度相机仿真时,在urdf文件中使用了类似下面的代码块
<plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
此时gazebo会报找不到 libgazebo_ros_openni_kinect.so的错误
2.原因
因为ros2的gazebo_ros_pkgs中,已经将该插件移除,或者说将该插件的功能合并到libgazebo_ros_camera.so中,
这里是作者的说明。
合并后,深度相机的用法参考这里。
主要是将type改成depth。还有其他类型的相机,可以参考具体的说明。
3.注意事项
在gazebo中添加完深度相机之后,假如在rviz中查看点云,会发现点云的坐标有问题。据说这是个gazebo的bug,解决办法看这里。
修改完之后,可能还需要到install目录里面删除相应的urdf文件,然后再编译,才会刷新。
4.demo
这里提供一下我这边的部分代码,仅供参考:
...
<xacro:property name="M_PI" value="3.14159265"/>
<!-- 搞个相机仿真 -->
<link name="link_camera">
<visual>
<origin xyz="0 0 0" rpy="0 ${M_PI/2.0} 0" />
<geometry>
<!-- <box size="0.05 0.03 0.03" /> -->
<cylinder radius="0.02" length="0.05"/>
</geometry>
<material name="black">
<color rgba="0.5 0.6 0.7 0.95"/>
</material>
</visual>
</link>
<joint name="joint_camera" type="fixed">
<origin xyz="-0.022 -0.015 0" rpy="0 ${-M_PI / 2.0} 0" />
<parent link="link6"/>
<child link="link_camera"/>
</joint>
<!-- 在RVIZ中显示点云位姿错误,此处加入一个空连杆和空关节,用于调整点云tf位姿 -->
<link name="link_camera_fake"></link>
<joint name="joint_camera_fake" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI / 2.0} 0 ${-M_PI / 2.0}"/>
<parent link="link_camera"/>
<child link="link_camera_fake"/>
</joint>
<gazebo reference="link_camera">
<sensor type="depth" name="my_sensor">
<!-- Set always_on only sensor, not on plugin -->
<always_on>1</always_on>
<!-- Set update_rate only sensor, not on plugin -->
<update_rate>1</update_rate>
<camera name="my_camera">
<image>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>30</far>
</clip>
<distortion>
<k1>0.0</k1>
<k2>0.0</k2>
<k3>0.0</k3>
<p1>0.0</p1>
<p2>0.0</p2>
<center>0.0 0.0</center>
</distortion>
</camera>
<plugin name="plugin_name_whatever" filename="libgazebo_ros_camera.so">
<!-- Change namespace, camera name and topics so -
* Raw images are published to: /custom_ns/custom_camera/custom_image
* Depth images are published to: /custom_ns/custom_camera/custom_image_depth
* Raw image camera info is published to: /custom_ns/custom_camera/custom_info_raw
* Depth image camera info is published to: /custom_ns/custom_camera/custom_info_depth
* Point cloud is published to: /custom_ns/custom_camera/custom_points
-->
<ros>
<namespace>custom_ns</namespace>
<remapping>custom_camera/image_raw:=custom_camera/custom_image</remapping>
<remapping>custom_camera/image_depth:=custom_camera/custom_image_depth</remapping>
<remapping>custom_camera/camera_info:=custom_camera/custom_info_raw</remapping>
<remapping>custom_camera/camera_info_depth:=custom_camera/custom_info_depth</remapping>
<remapping>custom_camera/points:=custom_camera/custom_points</remapping>
</ros>
<!-- Set camera name. If empty, defaults to sensor name (i.e. "sensor_name") -->
<camera_name>custom_camera</camera_name>
<!-- Set TF frame name. If empty, defaults to link name (i.e. "link_name") -->
<frame_name>link_camera_fake</frame_name>
<hack_baseline>0.07</hack_baseline>
<!-- No need to repeat distortion parameters or to set autoDistortion -->
<min_depth>0.001</min_depth>
</plugin>
</sensor>
</gazebo>
...
下图是使用后的结果。
可以看到,彩色图、深度图、点云都是基本问题的。除了点云的颜色(爪子是黄色的,但是不知道为何点云那边是蓝色的)。