<?xml version="1.0"?>
<robot name="sensor_asus_xtion_pro" 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" />
  
  <!-- Xacro properties -->
  <xacro:property name="M_SCALE" value="0.001"/>
  <xacro:property name="asus_xtion_pro_depth_rel_rgb_py" value="0.0270" />
  <xacro:property name="asus_xtion_pro_cam_rel_rgb_py"   value="-0.0220" />

  <!-- Parameterised in part by the values in turtlebot_properties.urdf.xacro -->
  <xacro:macro name="sensor_asus_xtion_pro" 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 ${asus_xtion_pro_cam_rel_rgb_py} 0" 
              rpy="0 0 0"/>
      <parent link="camera_rgb_frame"/>
      <child link="camera_link"/>
    </joint>
    <link name="camera_link">
      <visual>
        <origin xyz="-0.01 0 0" rpy="${-M_PI/2} -${M_PI} ${-M_PI/2}"/>
        <geometry>
          <mesh filename="package://turtlebot_description/meshes/sensors/asus_xtion_pro_live.dae" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
        <geometry>
        <box size="0.0730 .2760 0.0720"/>
      </geometry>
      </collision>
      <inertial>
        <mass value="0.01" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.001" ixy="0.0" ixz="0.0"
          iyy="0.001" iyz="0.0"
          izz="0.001" />
      </inertial>
    </link>

    <joint name="camera_depth_joint" type="fixed">
      <origin xyz="0 ${asus_xtion_pro_depth_rel_rgb_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.01" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.001" ixy="0.0" ixz="0.0"
          iyy="0.001" iyz="0.0"
          izz="0.001" />
      </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>

    <!-- RGBD sensor for simulation -->
<!--     <turtlebot_sim_xtion/> not working currently -->
  </xacro:macro>
</robot>
