<?xml version="1.0"?>
<!--
  Hexagon stacks
 -->
<robot name="stack_hexagons" xmlns:xacro="http://ros.org/wiki/xacro">
  <include filename="$(find turtlebot_description)/urdf/common_properties.urdf.xacro"/>
  
  <!-- Xacro properties -->
  <xacro:property name="M_SCALE" value="0.001"/>
  
  <!-- Xacro macros -->
  <!-- Pole macros -->
  <xacro:macro name="stack_bottom_pole" params="parent number x_loc y_loc z_loc">
    <joint name="pole_bottom_${number}_joint" type="fixed">
      <origin xyz="${x_loc} ${y_loc} ${z_loc}" rpy="0 0 0"/>
      <parent link="${parent}"/>
      <child link="pole_bottom_${number}_link"/>
    </joint>
    <link name="pole_bottom_${number}_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder length="0.0492" radius=".006"/>
        </geometry>
      </collision>
      <inertial>
        <mass value="0.001"/>
        <origin xyz="0 0 0" rpy="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="stack_middle_pole" params="parent number x_loc y_loc z_loc">  
    <joint name="pole_middle_${number}_joint" type="fixed">
      <origin xyz="${x_loc} ${y_loc} ${z_loc}" rpy="0 0 0" />
      <parent link="${parent}"/>
      <child link="pole_middle_${number}_link"/>
    </joint>
    <link name="pole_middle_${number}_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 ${M_PI} 0"/>
        <geometry>
          <mesh filename="package://turtlebot_description/meshes/stacks/hexagons/pole_middle.dae"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder length="0.0608" radius=".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="stack_top_pole" params="parent number x_loc y_loc z_loc">  
    <joint name="pole_top_${number}_joint" type="fixed">
      <origin xyz="${x_loc} ${y_loc} ${z_loc}" rpy="0 0 0"/>
      <parent link="${parent}"/>
      <child link="pole_top_${number}_link"/>
    </joint>
    <link name="pole_top_${number}_link">
      <visual>
        <origin xyz=" 0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://turtlebot_description/meshes/stacks/hexagons/pole_top.dae"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder length="0.2032" radius=".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="stack_3d_sensor_pole" params="parent number x_loc y_loc z_loc">
    <joint name="pole_kinect_${number}_joint" type="fixed">
      <origin xyz="${x_loc} ${y_loc} ${z_loc}" rpy="0 0 0"/>
      <parent link="${parent}"/>
      <child link="pole_kinect_${number}_link"/>
    </joint>
    <link name="pole_kinect_${number}_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
<!--         <origin xyz="0 0 0" rpy="${-M_PI/2} ${M_PI} 0"/> -->
        <geometry>
          <mesh filename="package://turtlebot_description/meshes/stacks/hexagons/pole_kinect.dae"/>
        </geometry>
      </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>
  
  <!-- 
    Stack macro - all the components relative transforms are made with respect 
    to the parent link (usually base_link). They could be made relative to each
    other, but there is no necessary reason to do so.
  -->
  <xacro:macro name="stack_hexagons" params="parent">
    <stack_bottom_pole parent="${parent}" number="0" x_loc= "0.120" y_loc= "0.082" z_loc="0.1028"/>
    <stack_bottom_pole parent="${parent}" number="1" x_loc= "0.055" y_loc= "0.120" z_loc="0.1028"/>
    <stack_bottom_pole parent="${parent}" number="2" x_loc="-0.055" y_loc= "0.120" z_loc="0.1028"/>
    <stack_bottom_pole parent="${parent}" number="3" x_loc= "0.120" y_loc="-0.082" z_loc="0.1028"/>
    <stack_bottom_pole parent="${parent}" number="4" x_loc= "0.055" y_loc="-0.120" z_loc="0.1028"/>
    <stack_bottom_pole parent="${parent}" number="5" x_loc="-0.055" y_loc="-0.120" z_loc="0.1028"/>
    
    <joint name="plate_bottom_joint" type="fixed">
      <origin xyz="0.02364 0.0 0.1306" rpy="0 0 0"/>
      <parent link="${parent}"/>
      <child link="plate_bottom_link"/>
    </joint>  
    <link name="plate_bottom_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://turtlebot_description/meshes/stacks/hexagons/plate_bottom.dae"/>
        </geometry>
      </visual>  
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <box size="0.26727 0.340 0.006"/>
        </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>
    
    <stack_middle_pole parent="${parent}" number="0" x_loc= "0.0381" y_loc= "0.1505" z_loc="0.1640"/>
    <stack_middle_pole parent="${parent}" number="1" x_loc= "0.0381" y_loc="-0.1505" z_loc="0.1640"/>
    <stack_middle_pole parent="${parent}" number="2" x_loc="-0.0381" y_loc= "0.1505" z_loc="0.1640"/>
    <stack_middle_pole parent="${parent}" number="3" x_loc="-0.0381" y_loc="-0.1505" z_loc="0.1640"/>
    
    <joint name="plate_middle_joint" type="fixed">
      <origin xyz="-0.01364 0.0 0.1874" rpy="0 0 0"/>
      <parent link="${parent}"/>
      <child link="plate_middle_link"/>
    </joint>  
    <link name="plate_middle_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://turtlebot_description/meshes/stacks/hexagons/plate_middle.dae"/>
        </geometry>
      </visual>  
      <collision>
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
        <geometry>
          <box size="0.28727 0.340 0.006"/>
        </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>
    
    <stack_top_pole parent="${parent}" number="0" x_loc= "0.0381" y_loc= "0.1505" z_loc="0.2920"/>
    <stack_top_pole parent="${parent}" number="1" x_loc= "0.0381" y_loc="-0.1505" z_loc="0.2920"/>
    <stack_top_pole parent="${parent}" number="2" x_loc="-0.0381" y_loc= "0.1505" z_loc="0.2920"/>
    <stack_top_pole parent="${parent}" number="3" x_loc="-0.0381" y_loc="-0.1505" z_loc="0.2920"/>
    <stack_3d_sensor_pole parent="${parent}" number="0" x_loc="-0.1024" y_loc= "0.098" z_loc="0.2372"/>
    <stack_3d_sensor_pole parent="${parent}" number="1" x_loc="-0.1024" y_loc="-0.098" z_loc="0.2372"/>
    
    <joint name="plate_top_joint" type="fixed">
      <origin xyz="-0.01364 0.0  0.3966" rpy="0 0 0"/>
      <parent link="${parent}"/>
      <child link="plate_top_link"/>
    </joint>  
    <link name="plate_top_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://turtlebot_description/meshes/stacks/hexagons/plate_top.dae"/>
        </geometry>
      </visual>  
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <box size="0.28727 0.340 0.006"/>
        </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>
  </xacro:macro>
</robot>
