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

  <xacro:property name="ir_depth_rgb_offset_y" value="0.01"/> <!-- FIXME: what is this offset? -->

  <!-- Kinect2 and mount assembly -->
  <xacro:macro name="kinect2_v0" params="name parent *origin">
    <joint name="${name}_kinect2_joint" type="fixed">
      <xacro:insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_kinect2_link"/>
    </joint>
    <link name="${name}_kinect2_link">
      <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.102 -0.148 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://pr2_description/meshes/sensors/kinect2_v0/kinect2_assembly.dae" scale="0.001 0.001 0.001" />
        </geometry>
        <material name="gray">
          <color rgba="0.5 0.5 0.5 1"/>
        </material>
      </visual>
      <collision>
        <origin xyz="-0.042 0 0.1" rpy="0 0 0"/>
        <geometry>
          <box size="0.12 0.3 0.2"/>
        </geometry>
      </collision>
    </link>

    <!-- kinect2 ir sensor physical attachment -->
    <joint name="${name}_kinect2_ir_joint" type="fixed">
      <origin xyz="-0.032267 -0.0475 0.155" rpy="0 0 0"/>
      <parent link="${name}_kinect2_link"/>
      <child link="${name}_kinect2_ir_link"/>
    </joint>
    <link name="${name}_kinect2_ir_link">
      <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>
    </link>
    <!-- kinect2 ir sensor frame -->
    <joint name="${name}_kinect2_ir_optical_frame_joint" type="fixed">
      <origin xyz="${cal_kinect_x}                  ${cal_kinect_y+ir_depth_rgb_offset_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}_kinect2_ir_link"/>
      <child link="${name}_kinect2_ir_optical_frame"/>
    </joint>

    <link name="${name}_kinect2_ir_optical_frame"/>
    <!-- gazebo extensions -->
    <xacro:kinect2_ir_gazebo_v0 link_name="${name}_kinect2_ir_link" frame_name="${name}_kinect2_ir_optical_frame" camera_name="head_mount_kinect2" />

    <!-- kinect2 rgb sensor physical attachment -->
    <joint name="${name}_kinect2_rgb_joint" type="fixed">
      <origin xyz="0 -0.05175 0" rpy="0 0 0"/>
      <parent link="${name}_kinect2_ir_link"/>
      <child link="${name}_kinect2_rgb_link"/>
    </joint>

    <link name="${name}_kinect2_rgb_link" >
      <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>
    </link>

    <!-- kinect2 rgb sensor frame -->
    <joint name="${name}_kinect2_rgb_optical_frame_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}_kinect2_rgb_link"/>
      <child link="${name}_kinect2_rgb_optical_frame"/>
    </joint>

    <link name="${name}_kinect2_rgb_optical_frame"/>
    <!-- gazebo extensions -->
    <xacro:kinect2_rgb_gazebo_v0 link_name="${name}_kinect2_rgb_link" frame_name="${name}_kinect2_rgb_optical_frame" camera_name="head_mount_kinect2" />

  </xacro:macro>

</robot>
