ROS——关于摄像头在gazebo的仿真

1. 前言

本文旨在实现摄像头在gazebo的仿真
实现具体流程为：
照相机在rviz的显示——>添加摄像头在gazebo的外观属性——>添加gazebo提供的摄像头插件

2. 照相机在rviz的简单示意

创建名为usb_camera.xacro的文件，用于表示摄像头本体
关于xacro文件的具体含义，可以浏览该文章：ROS——xacro文件解读

<?xml version="1.0"?>
<robot name="usb_camera" xmlns:xacro="http://www.ros.org/wiki/xacro" >

    <!-- COLOR -->
    <material name="Black">
         <color rgba="0 0 0 1" />
    </material>

    <material name="White">
         <color rgba="1 1 1 1" />
    </material>

    <!-- PROPERTY-->
    <xacro:property name="mass" value="0.1" />

    <xacro:macro name="usb_camera" >
       <link name="base_link">
            <inertial>
                <mass value="${mass}" />
                <origin xyz="0 0 0" />
                <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                         iyy="0.01" iyz="0.0"
                         izz="0.01" />
            </inertial>    
            
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="0.005 0.02 0.02" />
                </geometry>
                <material name="White"/>
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
                <geometry>
                    <box size="0.005 0.02 0.02" />
                </geometry>
            </collision>

       </link>

       <joint name="base_to_camera_joint" type="fixed">
            <parent link="base_link"/>
            <child link="usb_camera_link"/>
            <origin xyz="0 0 0" rpy="0 0 0" /> 
       </joint>

       <link name="usb_camera_link">
            <inertial>
                <mass value="${mass}" />
                <origin xyz="0 0 0" />
                <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                         iyy="0.01" iyz="0.0"
                         izz="0.01" />
            </inertial>

            <visual>
                <origin xyz=" 0 0 0 " rpy="0 0 0" />
                <geometry>
                    <box size="0.01 0.04 0.04" />
                </geometry>
                <material name="Black"/>
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
                <geometry>
                    <box size="0.01 0.04 0.04" />
                </geometry>
            </collision>

        </link>
    </xacro:macro>

</robot>

设置摄像头为box杆件，是最为简单的示意图
通过将不同模块（诸如摄像头，kinetic等）写成相应的xacro文件，并利用总xacro文件引用这些模块，方便后期进行机器人模块组装和修改
创建名为robot.xacro文件，其引用usb_camera.xacro文件：

<?xml version="1.0"?>
<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <xacro:include filename="$(find 包名)/urdf/usb_camera.xacro" />
    <usb_camera />

</robot>

创建名为display的launch文件加载robot.xacro。在本文即加载摄像头：

<?xml version = "1.0"?>
<launch>
    <arg name = "model" default = "$(find xacro)/xacro --inorder '$(find 包名)/urdf/robot.xacro'" />
    <arg name = "gui" default = "true" />
    <param name = "robot_description" command = "$(arg model)" />
    <param name = "use_gui" value = "$(arg gui)" />
    <node name = "joint_state_publisher" pkg = "joint_state_publisher" type = "joint_state_publisher" />
    <node name = "robot_state_publisher" pkg = "robot_state_publisher" type = "state_publisher" />
    <node name = "rviz" pkg = "rviz" type = "rviz" args = "-d $(find urdf_tutorial)/urdf.rviz" />
</launch>

rivz显示的图像如下：

3. 为摄像头添加gazebo的外观属性

要让杆件在gazebo有外观属性，必须在xacro文件中添加gazebo的material标签
具体形式如下：

<gazebo reference="杆件名">
    <material>Gazebo/颜色</material>
</gazebo>

因此，usb_camera.xacro文件修改为如下：

<?xml version="1.0"?>
<robot name="usb_camera" xmlns:xacro="http://www.ros.org/wiki/xacro" >

    <!-- COLOR -->
    <material name="Black">
         <color rgba="0 0 0 1" />
    </material>

    <material name="White">
         <color rgba="1 1 1 1" />
    </material>

    <!-- PROPERTY-->
    <xacro:property name="mass" value="0.1" />

    <xacro:macro name="usb_camera" >
       <link name="base_link">
            <inertial>
                <mass value="${mass}" />
                <origin xyz="0 0 0" />
                <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                         iyy="0.01" iyz="0.0"
                         izz="0.01" />
            </inertial>       
       
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="0.005 0.02 0.02" />
                </geometry>
                <material name="White"/>
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
                <geometry>
                    <box size="0.005 0.02 0.02" />
                </geometry>
            </collision>

       </link>
       
       <gazebo reference="base_link">
            <material>Gazebo/White</material>
       </gazebo>
       
       <joint name="base_to_camera_joint" type="fixed">
            <parent link="base_link"/>
            <child link="usb_camera_link"/>
            <origin xyz="0 0 0" rpy="0 0 0" /> 
       </joint>

       <link name="usb_camera_link">
            <inertial>
                <mass value="${mass}" />
                <origin xyz="0 0 0" />
                <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                         iyy="0.01" iyz="0.0"
                         izz="0.01" />
            </inertial>

            <visual>
                <origin xyz=" 0 0 0 " rpy="0 0 0" />
                <geometry>
                    <box size="0.01 0.04 0.04" />
                </geometry>
                <material name="Black"/>
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
                <geometry>
                    <box size="0.01 0.04 0.04" />
                </geometry>
            </collision>
        </link>
        
        <gazebo reference="usb_camera_link">
            <material>Gazebo/Black</material>
        </gazebo>  
    
    </xacro:macro>

</robot>

上述所创建display.launch是在rviz中显示，为使摄像头能在gazebo显示，需修改对应内容，如下所示：

<launch>

    <!-- 运行gazebo仿真环境 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" />

    <!-- 加载机器人模型描述参数 -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find 包名)/urdf/robot.xacro'" /> 

    <!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

    <!-- 运行robot_state_publisher节点，发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

    <!-- 在gazebo中加载机器人模型-->
    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
          args="-urdf -model 机器人名 -param robot_description"/> 

</launch>

gazebo显示的图像如下：

4. 为xacro文件添加摄像头插件

截至到目前为止，camera杆件只是一个示意图
gazebo提供了摄像头插件，能够使camera杆件进行摄像等功能
示例插件代码如下：

<gazebo reference="杆件名">
    <sensor type="camera" name="camera_node">
        <update_rate>30.0</update_rate>
        <camera name="head">
            <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="gazebo_camera" filename="libgazebo_ros_camera.so">
            <alwaysOn>true</alwaysOn>
            <updateRate>0.0</updateRate>
            <cameraName>/camera</cameraName>
            <imageTopicName>image_raw</imageTopicName>
            <cameraInfoTopicName>camera_info</cameraInfoTopicName>
            <frameName>usb_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>
</gazebo>

4.1 传感器的类型和名字

<sensor type="camera" name="****">
...
</sensor>

由于模拟摄像头，故类型为“camera”
“name"的名字必须是唯一

4.2 图片更新的最大频率

<update_rate>****</update_rate>

4.3 摄像头的具体配置

<horizontal_fov>****</horizontal_fov>
<image>
  <width>****</width>
  <height>****</height>
  <format>****</format>
</image>
<clip>
  <near>****</near>
  <far>****</far>
</clip>

4.4 链接到gazebo提供的摄像头插件

<plugin name="gazebo_camera" filename="libgazebo_ros_camera.so">
...
</plugin>

4.4.1 指定插件发布的话题名称

<cameraName>camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>

指定/camera/image_raw是摄像头发布的图像主题话题名称
指定/camera/camera_info是摄像头发布的图像信息话题名称
（名称无强制要求，可修改）

4.4.2 为图像指定坐标系

<frameName>杆件名</frameName>

指定是基于哪个杆件坐标系发布的图像
因此，在usb_camera.xacro文件引用摄像头插件，修改得到：

<?xml version="1.0"?>
<robot name="usb_camera" xmlns:xacro="http://www.ros.org/wiki/xacro" >

    <!-- COLOR -->
    <material name="Black">
         <color rgba="0 0 0 1" />
    </material>

    <material name="White">
         <color rgba="1 1 1 1" />
    </material>

    <!-- PROPERTY-->
    <xacro:property name="mass" value="0.1" />

    <xacro:macro name="usb_camera" >
       <link name="base_link">
            <inertial>
                <mass value="${mass}" />
                <origin xyz="0 0 0" />
                <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                         iyy="0.01" iyz="0.0"
                         izz="0.01" />
            </inertial>       
       
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="0.005 0.02 0.02" />
                </geometry>
                <material name="White"/>
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
                <geometry>
                    <box size="0.005 0.02 0.02" />
                </geometry>
            </collision>

       </link>
       
       <gazebo reference="base_link">
            <material>Gazebo/White</material>
       </gazebo>
       
       <joint name="base_to_camera_joint" type="fixed">
            <parent link="base_link"/>
            <child link="usb_camera_link"/>
            <origin xyz="0 0 0" rpy="0 0 0" /> 
       </joint>

       <link name="usb_camera_link">
            <inertial>
                <mass value="${mass}" />
                <origin xyz="0 0 0" />
                <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                         iyy="0.01" iyz="0.0"
                         izz="0.01" />
            </inertial>

            <visual>
                <origin xyz=" 0 0 0 " rpy="0 0 0" />
                <geometry>
                    <box size="0.01 0.04 0.04" />
                </geometry>
                <material name="Black"/>
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
                <geometry>
                    <box size="0.01 0.04 0.04" />
                </geometry>
            </collision>
        </link>
        
        <gazebo reference="usb_camera_link">
            <material>Gazebo/Black</material>
        </gazebo>  
        
        <gazebo reference="usb_camera_link">
            <sensor type="camera" name="camera_node">
                <update_rate>30.0</update_rate>
                <camera name="head">
                    <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="gazebo_camera" filename="libgazebo_ros_camera.so">
                    <alwaysOn>true</alwaysOn>
                    <updateRate>0.0</updateRate>
                    <cameraName>/camera</cameraName>
                    <imageTopicName>image_raw</imageTopicName>
                    <cameraInfoTopicName>camera_info</cameraInfoTopicName>
                    <frameName>usb_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>
       </gazebo>   
    </xacro:macro>

</robot>

运行display.launch文件加载robot.xacro
运行命令rostopic list，查看当前发布的话题，可以看到话题/camera/image_raw被发布
运行命令rqt_image_view订阅话题/camera/image_raw，可以看到摄像头拍摄的图像
由于暂时环境为空，所以显示灰色
后期可通过添加gazebo环境来体现摄像头的功能