<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  
  <xacro:include filename="$(find pr2_description)/urdf/sensors/kinect_camera.gazebo.xacro" />

  <xacro:macro name="kinect_camera_v0" params="name parent *origin">
    <joint name="${name}_frame_joint" type="fixed">
      <xacro:insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_frame"/>
    </joint>
    <link name="${name}_frame">

      <inertial>
        <mass value="0.1" />
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <inertia ixx="0.01"  ixy="0.0"  ixz="0.0"
                 iyy="0.01"  iyz="0.0"
                 izz="0.01" />
      </inertial>

      <visual>
        <origin xyz="0.0 0.012 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://pr2_description/meshes/sensors/kinect_v0/kinect.dae" scale="0.001 0.001 0.001" />
        </geometry>
      </visual>

      <collision>
        <origin xyz="0.0 0.012 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://pr2_description/meshes/sensors/kinect_v0/kinect.dae" scale="0.001 0.001 0.001" />
        </geometry>
      </collision>

    </link>

    <joint name="${name}_optical_joint" type="fixed">
      <origin xyz="${cal_kinect_x}            ${cal_kinect_y}         ${cal_kinect_z}"
                rpy="${-M_PI/2+cal_kinect_roll} ${0.0+cal_kinect_pitch} ${-M_PI/2+cal_kinect_yaw}" />
      <parent link="${name}_frame"/>
      <child link="${name}_optical_frame"/>
    </joint>

    <link name="${name}_optical_frame"/>

    <!-- extensions -->
    <xacro:kinect_camera_gazebo_v0 name="${name}" />
  </xacro:macro>

</robot>