<?xml version="1.0"?>
<robot name="sensor_kinect" xmlns:xacro="http://ros.org/wiki/xacro"
                            xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" 
                            xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
                            xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor">
  <include filename="$(find turtlebot_description)/urdf/common_properties.urdf.xacro"/>
  <include filename="$(find turtlebot_description)/urdf/turtlebot_gazebo.urdf.xacro"/>
  <include filename="$(find turtlebot_description)/urdf/common_properties.urdf.xacro" />
  <include filename="$(find turtlebot_description)/urdf/turtlebot_properties.urdf.xacro" />
  
  <!-- Parameterised in part by the values in turtlebot_properties.urdf.xacro -->
  <xacro:macro name="sensor_kinect" params="parent">
    <joint name="camera_rgb_joint" type="fixed">
      <origin xyz="${cam_px} ${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">
      <inertial>
        <mass value="0.001" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
                 iyy="0.0001" iyz="0.0"
                 izz="0.0001" />
      </inertial>
    </link>
    <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">
      <inertial>
        <mass value="0.001" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
                 iyy="0.0001" iyz="0.0"
                 izz="0.0001" />
      </inertial>
    </link>

  <joint name="camera_joint" type="fixed">
    <origin xyz="-0.031 ${-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.0315 0.0 -0.017" rpy="0 0 0"/>
	    <geometry>
	      <box size="0.07271 0.27794 0.073"/>
	    </geometry>
	  </collision>
    <inertial>
      <mass value="0.001" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
               iyy="0.0001" iyz="0.0"
               izz="0.0001" />
    </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 * -cam_py} 0" rpy="0 0 0" />
	  <parent link="camera_rgb_frame" />
	  <child link="camera_depth_frame" />
	</joint>
	<link name="camera_depth_frame">
    <inertial>
      <mass value="0.001" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
               iyy="0.0001" iyz="0.0"
               izz="0.0001" />
    </inertial>
	</link>
	<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">
    <inertial>
      <mass value="0.001" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
               iyy="0.0001" iyz="0.0"
               izz="0.0001" />
    </inertial>
	</link>
	
	<!-- Kinect sensor for simulation -->
<!-- 	<turtlebot_sim_kinect/> not working currently -->
  </xacro:macro>
</robot>
