<?xml version="1.0"?>
<robot name="turtlebot_hardware" xmlns:xacro="http://ros.org/wiki/xacro">
  <property name="M_PI" value="3.14159"/>

  <include filename="$(find turtlebot_description)/urdf/gazebo.urdf.xacro"/>
  <include filename="$(find turtlebot_description)/urdf/turtlebot_calibration.xacro" />

<xacro:macro name="turtlebot_kinect">

<!-- to make this more realistic later we need to add that the rotation axis is offset from the camera center -->

  <joint name="base_camera_joint" type="revolute"> 
    <origin xyz="${turtlebot_calib_cam_x} ${turtlebot_calib_cam_y} ${turtlebot_calib_cam_z}" 
            rpy="${turtlebot_calib_cam_rr} ${turtlebot_calib_cam_rp} ${turtlebot_calib_cam_ry}" />
    <axis xyz="0 1 0"/>
    <limit effort="1000.0" lower="-1.5708" upper="1.5708" velocity="0.5"/> 
    <dynamics damping="0.1"/>
    <parent link="base_link" />
    <child link="camera_link" />
  </joint>

<transmission type="pr2_mechanism_model/SimpleTransmission" name="kinect_tilt_transmission">
  <actuator name="kinect_tilt_motor" />
  <joint name="base_camera_joint" />
  <mechanicalReduction>10</mechanicalReduction>
</transmission>

  <link name="camera_link">
    <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>

    <visual>
      <origin xyz=" 0 0 0 " rpy="0 0 0" />
      <geometry>
        <mesh filename="package://turtlebot_description/meshes/kinect.dae"/>
      </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>

  </link>

  <joint name="camera_depth_joint" type="fixed">
    <origin xyz="0 0.018 0" rpy="0 0 0" />
    <parent link="camera_link" />
    <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>

  <joint name="camera_rgb_joint" type="fixed">
    <origin xyz="0 -0.005 0" rpy="0 0 0" />
    <parent link="camera_link" />
    <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>

  <!-- Kinect sensor for simulation -->
  <turtlebot_sim_kinect/>
</xacro:macro>

</robot>
