<?xml version="1.0"?>
<root xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
      xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
      xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
      xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:include filename="$(find xacro)/test/robots/pr2/sensors/kinect_prosilica_camera.gazebo.xacro" />

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

  <!-- kinect and prosilica combo -->
  <xacro:macro name="kinect_prosilica_camera_swept_back_v0" params="name parent *origin">
    <!-- stl contains structure + kinect + prosilica combo -->
    <joint name="${name}_joint" type="fixed">
      <!-- <limit lower="0.1" upper="0.1" effort="10000" velocity="100.0"/> -->
      <!-- <insert_block name="origin" /> -->
      <origin xyz="-0.138 0 0.091" rpy="0 0 0"/>
      <parent link="${parent}"/>
      <child link="${name}_link"/>
    </joint>
    <link name="${name}_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.0 0.0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://pr2_description/meshes/sensors/kinect_prosilica_v0/115x100_swept_back--coarse.STL" 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.0 0.0 0" rpy="0 0 0"/>
        <geometry>
          <sphere radius="0.0005"/>
        </geometry>
      </collision>
    </link>

    <!-- kinect ir sensor physical attachment -->
    <joint name="${name}_kinect_ir_joint" type="fixed">
      <origin xyz="-0.032267 0.0125 0.136453" rpy="0 0 0"/>
      <parent link="${name}_link"/>
      <child link="${name}_kinect_ir_link"/>
    </joint>
    <link name="${name}_kinect_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>
      <visual>
        <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
        <geometry>
          <sphere radius="0.0005"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
        <geometry>
          <sphere radius="0.0005"/>
        </geometry>
      </collision>
    </link>
    <!-- kinect ir sensor frame -->
    <joint name="${name}_kinect_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}_kinect_ir_link"/>
      <child link="${name}_kinect_ir_optical_frame"/>
    </joint>

    <link name="${name}_kinect_ir_optical_frame"/>
    <!-- gazebo extensions -->
    <xacro:kinect_ir_gazebo_v0 link_name="${name}_kinect_ir_link" frame_name="${name}_kinect_ir_optical_frame" camera_name="head_mount_kinect" />




    <!-- kinect rgb sensor physical attachment -->
    <joint name="${name}_kinect_rgb_joint" type="fixed">
      <origin xyz="0 -0.03 0" rpy="0 0 0"/>
      <parent link="${name}_kinect_ir_link"/>
      <child link="${name}_kinect_rgb_link"/>
    </joint>

    <link name="${name}_kinect_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>
      <visual>
        <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
        <geometry>
          <sphere radius="0.0005"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
        <geometry>
          <sphere radius="0.0005"/>
        </geometry>
      </collision>
    </link>

    <!-- kinect rgb sensor frame -->
    <joint name="${name}_kinect_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}_kinect_rgb_link"/>
      <child link="${name}_kinect_rgb_optical_frame"/>
    </joint>

    <link name="${name}_kinect_rgb_optical_frame"/>
    <!-- gazebo extensions -->
    <xacro:kinect_rgb_gazebo_v0 link_name="${name}_kinect_rgb_link" frame_name="${name}_kinect_rgb_optical_frame" camera_name="head_mount_kinect" />





    <!-- prosilica sensor physical attachment -->
    <joint name="${name}_prosilica_joint" type="fixed">
      <origin xyz="-0.046457 0.0125 0.088921" rpy="0 0 0"/>
      <parent link="${name}_link"/>
      <child link="${name}_prosilica_link"/>
    </joint>
    <link name="${name}_prosilica_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.0 0.0 0" rpy="0 0 0"/>
        <geometry>
          <sphere radius="0.0005"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
        <geometry>
          <sphere radius="0.0005"/>
        </geometry>
      </collision>
    </link>
    <!-- prosilica sensor frame -->
    <joint name="${name}_prosilica_optical_frame_joint" type="fixed">
      <origin xyz="${cal_high_def_x}            ${cal_high_def_y}         ${cal_high_def_z}"
                rpy="${-M_PI/2+cal_high_def_roll} ${0.0+cal_high_def_pitch} ${-M_PI/2+cal_high_def_yaw}" />
      <parent link="${name}_prosilica_link"/>
      <child link="${name}_prosilica_optical_frame"/>
    </joint>

    <link name="${name}_prosilica_optical_frame"/>
    <!-- gazebo extensions -->
    <xacro:prosilica_cam_gazebo_v1 link_name="${name}_prosilica_link" camera_name="prosilica_camera" frame_name="${name}_prosilica_optical_frame" />

  </xacro:macro>


</root>
