Integration of Gazebo kinect plugin with custom robot

edit

Hello,

I use ROS kinetic on a VMware virtual machine running Ubuntu 16.04 and Gazebo 7.16.0

I am having some issues while integrating the Gazebo kinect plugin with my custom robot simulation. I installed the kinect plugin properly and checked it with the Turtlebot Gazebo simulation:

roslaunch turtlebot_gazebo turtlebot_world.launch

The camera topics are published and I can view them using rviz.

In the following I use the turtlebot xacros as a reference for integrating the kinect with my custom robot simulation. First, I only spawn the kinect urdf along with the plugin to Gazebo. Here is the corresponding xacro:

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

  <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_common_library.urdf.xacro" />
  <xacro:property name="kinect_cam_py" value="-0.0125"/>
  <!--
  <joint name="camera_rgb_joint" type="fixed">
      <origin xyz="${cam_px} ${kinect_cam_py} ${cam_pz}" rpy="${cam_or} ${cam_op} ${cam_oy}"/>
      <parent link="${parent}"/>
      <child link="camera_rgb_frame" />
    </joint>
  -->
    <link name="camera_rgb_frame"/>
<joint name="camera_rgb_optical_joint" type="fixed">
  <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
  <parent link="camera_rgb_frame" />
  <child link="camera_rgb_optical_frame" />
</joint>
<link name="camera_rgb_optical_frame"/>

<joint name="camera_joint" type="fixed">
  <origin xyz="-0.031 ${-kinect_cam_py} -0.016" rpy="0 0 0"/>
  <parent link="camera_rgb_frame"/>
  <child link="camera_link"/>
</joint>  
  <link name="camera_link">
  <visual>
   <origin xyz="0 0 0" rpy="0 0 ${M_PI/2}"/>
    <geometry>
     <mesh filename="package://turtlebot_description/meshes/sensors/kinect.dae"/>
    </geometry>
  </visual>
  <collision>
    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
    <geometry>
      <box size="0.07271 0.27794 0.073"/>
    </geometry>
  </collision>
  <inertial>
    <mass value="0.564" />
    <origin xyz="0 0 0" />
    <inertia ixx="0.003881243" ixy="0.0" ixz="0.0"
             iyy="0.000498940" iyz="0.0"
             izz="0.003879257" />
  </inertial>
</link>

<!-- The fixed joints & links below are usually published by static_transformers launched by the OpenNi launch 
     files. However, for Gazebo simulation we need them, so we add them here.
     (Hence, don't publish them additionally!) -->
<joint name="camera_depth_joint" type="fixed">
  <origin xyz="0 ${2 * -kinect_cam_py} 0" rpy="0 0 0" />
  <parent link="camera_rgb_frame" />
  <child link="camera_depth_frame" />
</joint>
<link name="camera_depth_frame"/>

<joint name="camera_depth_optical_joint" type="fixed">
  <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
  <parent link="camera_depth_frame" />
  <child link="camera_depth_optical_frame" />
</joint>

<link name="camera_depth_optical_frame"/>

    <!-- camera -->
      <gazebo reference="camera_link">
    <sensor type="depth" name="camera1">
        <always_on>1</always_on>
        <visualize>true</visualize>             
        <camera>
            <horizontal_fov>1.047</horizontal_fov>  
            <image>
                <width>640</width>
                <height>480</height>
                <format>R8G8B8</format>
            </image>
            <depth_camera>

            </depth_camera>
            <clip>
                <near>0.1</near>
                <far>100</far>
            </clip>
        </camera>
             <plugin name="camera_controller" filename="libgazebo_ros_openni_kinect.so">
             <alwaysOn>true</alwaysOn>
                <updateRate>10.0</updateRate>
                <cameraName>camera</cameraName>
                <frameName>camera_link</frameName>                   
            <imageTopicName>rgb/image_raw</imageTopicName>
            <depthImageTopicName>depth/image_raw</depthImageTopicName>
            <pointCloudTopicName>depth/points</pointCloudTopicName>
            <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>              
            <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>            
            <pointCloudCutoff>0 ...