<?xml version="1.0"?>
<!--
  Xtion Pro Live + Poles for the Turtlebot 2 stack
 -->
<robot name="xtion_turtlebot2" 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"/>

  <!-- Xacro properties -->
  <xacro:property name="M_SCALE" value="0.001"/>
  <xacro:property name="turtlebot2_xtion_calib_px" value="-0.095"/>
  <xacro:property name="turtlebot2_xtion_calib_py" value="0"/>
  <xacro:property name="turtlebot2_xtion_calib_pz" value="0.2680"/>
  <xacro:property name="turtlebot2_xtion_calib_or" value="0"/>
  <xacro:property name="turtlebot2_xtion_calib_op" value="0"/>
  <xacro:property name="turtlebot2_xtion_calib_oy" value="0"/>
  
  <!-- Xacro macros -->  
  <xacro:macro name="stack_xtion_pole" params="parent number x_loc y_loc z_loc">
    <joint name="pole_xtion_${number}_joint" type="fixed">
      <origin xyz="${x_loc} ${y_loc} ${z_loc}" rpy="0 0 0"/>
      <parent link="${parent}"/>
      <child link="pole_xtion_${number}_link"/>
    </joint>
    <link name="pole_xtion_${number}_link">
      <visual>
        <origin xyz="0 0 0" rpy="${-M_PI/2} 0 0"/>
        <geometry>
          <mesh filename="package://turtlebot_description/meshes/turtlebot2_stack/pole_kinect.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
        <xacro:insert_block name="material_light_grey"/>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder length="0.0936" radius="0.006"/>
        </geometry>
      </collision>
      <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>
  </xacro:macro>

  <xacro:macro name="xtion_turtlebot2" params="parent">
    <stack_xtion_pole parent="${parent}" number="0" x_loc="-0.1024" y_loc= "0.098" z_loc="0.259"/>
    <stack_xtion_pole parent="${parent}" number="1" x_loc="-0.1024" y_loc="-0.098" z_loc="0.259"/>
      
    <joint name="camera_joint" type="fixed">
      <origin xyz="${turtlebot2_xtion_calib_px} ${turtlebot2_xtion_calib_py} ${turtlebot2_xtion_calib_pz}" 
              rpy="${turtlebot2_xtion_calib_or} ${turtlebot2_xtion_calib_op} ${turtlebot2_xtion_calib_oy}"/>
      <parent link="${parent}"/>
      <child link="camera_link"/>
    </joint>  
    <link name="camera_link">
      <visual>
        <origin xyz="0 0 0" rpy="${M_PI/2} 0 ${M_PI/2}"/>
        <geometry>
          <!-- Thanks to IHeartRobotics for the Collada model! -->
          <mesh filename="package://turtlebot_description/meshes/asus_xtion_pro_live.dae" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
        <xacro:insert_block name="material_light_grey"/>
      </visual>  
      <collision>
        <origin xyz="-0.0315 0.0 -0.017" rpy="0 0 0"/>
        <geometry>
          <box size="0.07271 0.27794 0.073"/>
        </geometry>
      </collision>
      <inertial>
        <mass value="0.01"/>
        <origin xyz="0 0 0"/>
        <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                 iyy="0.01" iyz="0.0"
                 izz="0.01" />
      </inertial>  
    </link>
    
    <!-- The fixed joints & links below are usually published by static_transformers launched by the OpenNi launch 
         files. However, for Gazebo simulation we need them, so we add them here.
         (Hence, don't publish them additionally!) -->
	  <joint name="camera_depth_joint" type="fixed">
	    <origin xyz="0 0.0125 0" rpy="0 0 0" />
	    <parent link="xtion_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.0125 0" rpy="0 0 0" />
	    <parent link="xtion_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>
    
    <!-- Xtion sensor for simulation -->
	  <!-- gazebo_ros_wge100 plugin is in kt2_gazebo_plugins package - TODO: DEPRECATED!!-->
	  <gazebo reference="xtion_link" >
	    <sensor:camera name="xtion" >
	      <imageSize>640 480</imageSize>
	      <imageFormat>R8G8B8</imageFormat>
	      <hfov>62.8</hfov>
	      <nearClip>0.1</nearClip>
	      <farClip>10</farClip>
	      <updateRate>2.0</updateRate>
	      <controller:gazebo_ros_wge100 name="xtion_controller" plugin="libgazebo_ros_wge100.so">
	        <alwaysOn>true</alwaysOn>
	        <updateRate>5000.0</updateRate>
	        <cameraName>camera/rgb</cameraName>
	        <frameName>camera_rgb_optical_frame</frameName>
	        <hackBaseline>0.0</hackBaseline>
	        <CxPrime>320.5</CxPrime>
	        <Cx>320.5</Cx>
	        <Cy>240.5</Cy>
	        <baseline>0.09</baseline>
	        <!-- image_width / (2*tan(hfov_radian /2)) -->
	        <!-- 320 for wide and 772.55 for narrow stereo camera -->
	        <focal_length>525</focal_length>
	        <cameraNamespace>camera/rgb</cameraNamespace>
	        <depthData>true</depthData>
	        <interface:camera name="xtion_iface" />
	      </controller:gazebo_ros_wge100>
	    </sensor:camera>
	  </gazebo>
  </xacro:macro>
</robot>
