[028] Gazebo构建Kinect模型，在RVIZ中显示点云PointCloud2出错：点云位姿错误，浮在空中

一、Bug描述

1、发生错误的.urdf代码（也不是代码错误，是gazebo的bug）

<link name="camera_link">
     <visual>
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
            <sphere radius="0.015" />
         </geometry>
         <material name="black">
             <color rgba="100 0 0 0.95"/>
         </material>
     </visual>
  </link>
 
  <gazebo 
    reference="camera_link">
    <material>Gazebo/Black</material>
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
  </gazebo>   

  <joint name="camera_joint"
     type="fixed">
     <origin xyz="0.065 0 1.22" rpy="0 0.5 0"/>
     <parent link="base_link"/>
     <child link="camera_link"/>
  </joint>
 
  <gazebo reference="camera_link">
    <sensor name="Kinect" type="depth">
      <update_rate>20</update_rate> 
      <camera>
        <horizontal_fov>1.047198</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.05</near>
          <far>3</far>
        </clip>
      </camera>
      <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
        <baseline>0.2</baseline>
        <alwaysOn>true</alwaysOn>
        <updateRate>1.0</updateRate>
        <cameraName>Kinect_ir</cameraName>
        <imageTopicName>/Kinect/color/image_raw</imageTopicName>
        <cameraInfoTopicName>/Kinect/color/camera_info</cameraInfoTopicName>
        <depthImageTopicName>/Kinect/depth/image_raw</depthImageTopicName>
        <depthImageInfoTopicName>/Kinect/depth/camera_info</depthImageInfoTopicName>
        <pointCloudTopicName>/Kinect/depth/points</pointCloudTopicName>
        <frameName>camera_link</frameName>
        <pointCloudCutoff>0.5</pointCloudCutoff>
        <pointCloudCutoffMax>5.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>

2、在Gazebo中的模型状态

3、RVIZ显示，二维图像位姿正确，点云位姿错误

二、解决办法

1、解决办法：在原.urdf Kinect代码的基础上，再加一个空连杆（camera_link_fake）和用于调整点云图像位姿的调整关节（camera_joint_fake），然后将Kinect的<frameName>改为空连杆camera_link_fake。

  <!-- 深度相机Kinect,用小黑球替代 -->
  <link name="camera_link">
     <visual>
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
            <sphere radius="0.015" />
         </geometry>
         <material name="black">
             <color rgba="100 0 0 0.95"/>
         </material>
     </visual>
  </link>
  <gazebo 
    reference="camera_link">
    <material>Gazebo/Black</material>
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
  </gazebo>   
  <joint name="camera_joint"
     type="fixed">
     <origin xyz="0.065 0 1.22" rpy="0 0.5 0"/>
     <parent link="base_link"/>
     <child link="camera_link"/>
  </joint>

  <!-- 在RVIZ中显示点云位姿错误，此处加入一个空连杆和空关节，用于调整点云tf位姿 -->
  <link name="camera_link_fake"></link>
  <joint name="camera_joint_fake" type="fixed">
    <origin xyz="0 0 0" rpy="-1.5708 0 -1.5708 "/>
    <parent link="camera_link"/>
    <child link="camera_link_fake"/>
  </joint>

  <!-- 此时Kinect的framename一定要改成空连杆camera_link_fake -->
  <gazebo reference="camera_link">
    <sensor name="Kinect" type="depth">
      <update_rate>20</update_rate> 
      <camera>
        <horizontal_fov>1.047198</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.05</near>
          <far>3</far>
        </clip>
      </camera>
      <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
        <baseline>0.2</baseline>
        <alwaysOn>true</alwaysOn>
        <updateRate>1.0</updateRate>
        <cameraName>Kinect_ir</cameraName>
        <imageTopicName>/Kinect/color/image_raw</imageTopicName>
        <cameraInfoTopicName>/Kinect/color/camera_info</cameraInfoTopicName>
        <depthImageTopicName>/Kinect/depth/image_raw</depthImageTopicName>
        <depthImageInfoTopicName>/Kinect/depth/camera_info</depthImageInfoTopicName>
        <pointCloudTopicName>/Kinect/depth/points</pointCloudTopicName>
        <frameName>camera_link_fake</frameName>
        <pointCloudCutoff>0.5</pointCloudCutoff>
        <pointCloudCutoffMax>5.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>

2、再次运行Gazebo和RVIZ，点云显示正确。

3、bug解释：Gazebo的frame位姿tf仅仅提供给图像image这个话题，没有提供给点云话题。

4、感谢ROS社区，国内很难找到好的中文资源。参考链接。