<?xml version="1.0"?>

<robot name="turtlebot" xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:include filename="$(find rbx1_description)/urdf/turtlebot_hardware.xacro"/>

  <xacro:include filename="$(find rbx1_description)/urdf/gazebo.urdf.xacro"/>

  <material name="Green">
    <color rgba="0.0 0.8 0.0 1.0"/>
  </material>
  
  <material name="OffWhite">
    <color rgba="0.8 0.8 0.8 0.9"/>
  </material>

  <property name="M_PI" value="3.14159"/>
  <property name="SCALE" value="0.0254"/>
  <property name="MESH_SCALE" value="100"/>
  <property name="base_x" value="0.33" />
  <property name="base_y" value="0.33" />

  <macro name="turtlebot">
  <!-- base_footprint is a fictitious link(frame) that is on the ground right below base_link origin, navigation stack depends on this frame -->

  <link name="base_footprint">
    <inertial>
      <mass value="0.0001" />
      <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>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="0.001 0.001 0.001" />
      </geometry>
      <material name="Green" />
    </visual>

    <collision>
      <origin xyz="0 0 0.017" rpy="0 0 0" />
      <geometry>
       <box size="0.001 0.001 0.001" />
      </geometry>
    </collision>
  </link>

  <link name="base_link">
    <inertial>
      <mass value="1" />
      <origin xyz="0 0 0.0308" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0" izz="1.0" />
    </inertial>

    <visual>
      <origin xyz=" 0 0 0.0308" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://rbx1_description/meshes/create_body.dae" scale="${MESH_SCALE} ${MESH_SCALE} ${MESH_SCALE}"/>
      </geometry>
    </visual>

    <collision>
      <origin xyz="0.0 0.0 0.0308" rpy="0 0 0" />
      <geometry>
        <cylinder length="0.0611632" radius="0.16495"/>
      </geometry>
    </collision>
  </link>

  <link name="wall_sensor_link">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0" izz="1.0" />
    </inertial>
  </link>


  <link name="left_cliff_sensor_link">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0" izz="1.0" />
    </inertial>
  </link>

  <link name="right_cliff_sensor_link">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0" izz="1.0" />
    </inertial>
  </link>

  <link name="leftfront_cliff_sensor_link">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0" izz="1.0" />
    </inertial>
  </link>

  <link name="rightfront_cliff_sensor_link">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0" izz="1.0" />
    </inertial>
  </link>

  <joint name="base_footprint_joint" type="fixed">
    <origin xyz="0 0 0.017" rpy="0 0 0" />        
    <parent link="base_footprint"/>
    <child link="base_link" />
  </joint>

  <joint name="base_wall_sensor_joint" type="fixed">
    <origin xyz="0.09 -0.120 0.042" rpy="0 0 -1.0" />        
    <parent link="base_link"/>
    <child link="wall_sensor_link" />
  </joint>

  <joint name="base_left_cliff_sensor_joint" type="fixed">
    <origin xyz="0.07 0.14 0.01" rpy="0 1.57079 0" />        
    <parent link="base_link"/>
    <child link="left_cliff_sensor_link" />
  </joint>

  <joint name="base_right_cliff_sensor_joint" type="fixed">
    <origin xyz="0.07 -0.14 0.01" rpy="0 1.57079 0" />        
    <parent link="base_link"/>
    <child link="right_cliff_sensor_link" />
  </joint>

  <joint name="base_leftfront_cliff_sensor_joint" type="fixed">
    <origin xyz="0.15 0.04 0.01" rpy="0 1.57079 0" />        
    <parent link="base_link"/>
    <child link="leftfront_cliff_sensor_link" />
  </joint>

  <joint name="base_rightfront_cliff_sensor_joint" type="fixed">
    <origin xyz="0.15 -0.04 0.01" rpy="0 1.57079 0" />        
    <parent link="base_link"/>
    <child link="rightfront_cliff_sensor_link" />
  </joint>

  <link name="base_l_wheel_link">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.01" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0" izz="1.0" />
    </inertial>

    <visual>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <cylinder radius="0.033" length = "0.023"/>
      </geometry>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <cylinder radius="0.033" length = "0.023"/>
      </geometry>
    </collision>
  </link>

  <joint name="base_l_wheel_joint" type="continuous">
    <origin xyz="0 0.13 0.015" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="base_l_wheel_link"/>
    <axis xyz="0 1 0"/>
  </joint>

  <link name="base_r_wheel_link">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.01" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0" izz="1.0" />
    </inertial>

    <visual>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <cylinder radius="0.033" length = "0.023"/>
      </geometry>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <cylinder radius="0.033" length = "0.023"/>
      </geometry>
    </collision>
  </link>

  <joint name="base_r_wheel_joint" type="continuous">
    <origin xyz="0 -0.13 0.015" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="base_r_wheel_link"/>
    <axis xyz="0 1 0"/>
  </joint>

  <link name="rear_wheel_link">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.001" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0" izz="1.0" />
    </inertial>

    <visual>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707"/>
      <geometry>
        <sphere radius="0.018" />
      </geometry>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <sphere radius="0.018" />
      </geometry>
    </collision>
  </link>
  
  <joint name="rear_castor_joint" type="fixed">
    <origin xyz="-0.13 0 0.0" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="rear_wheel_link"/>
    <axis xyz="0 1 0"/>
  </joint>

  <link name="front_wheel_link">
    <inertial>
      <origin xyz="0 0 0" />
      <mass value="0.01" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0" izz="1.0" />
    </inertial>

    <visual>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707"/>
      <geometry>
        <sphere radius="0.018" />
      </geometry>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <sphere radius="0.018" />
      </geometry>
    </collision>
  </link>

  <joint name="front_castor_joint" type="fixed">
    <origin xyz="0.13 0 0.0" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="front_wheel_link"/>
    <axis xyz="0 1 0"/>
  </joint>

  <turtlebot_spacer parent="base_link" number="0" x_loc="-0.00254" y_loc="0.1114679" z_loc="0.062992"/>
  <turtlebot_spacer parent="base_link" number="1" x_loc="-0.00254" y_loc="-0.1114679" z_loc="0.062992"/>
  <turtlebot_spacer parent="base_link" number="2" x_loc="-0.07239" y_loc="-0.1114679" z_loc="0.062992"/>
  <turtlebot_spacer parent="base_link" number="3" x_loc="-0.07239" y_loc="0.1114679" z_loc="0.062992"/>

  <joint name="gyro_joint" type="fixed">
    <origin xyz="0 0 0.04" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="gyro_link" />
  </joint>

  <link name="gyro_link">
    <visual>
      <geometry>
        <box size="0.02 0.035 0.002" />
      </geometry>
      <material name="Green" />
    </visual>
    <inertial>
      <mass value="0.000001" />
      <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="laser_joint" type="fixed">
    <origin xyz="-0.065 0 0.075" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="laser" />
  </joint>

  <link name="laser">
    <visual>
      <geometry>
        <box size="0.02 0.035 0.002" />
      </geometry>
      <material name="Green" />
    </visual>
    <inertial>
      <mass value="0.000001" />
      <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="plate_0_joint" type="fixed">
    <origin xyz="-0.04334 0  0.06775704" rpy="0 0 0" />
    <parent link="base_link"/>
    <child link="plate_0_link" />
  </joint>


  <link name="plate_0_link">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0"
        izz="1.0" />
    </inertial>

    <visual>
      <origin xyz=" 0 0 0 " rpy="0 0 0" />
      <geometry>
        <mesh filename="package://rbx1_description/meshes/plate_0_logo.dae" scale="${MESH_SCALE} ${MESH_SCALE} ${MESH_SCALE}"/>
      </geometry>
    </visual>

    <collision>
      <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      <geometry>
        <box size="0.233502 0.314845 0.006401"/>
      </geometry>
    </collision>
  </link>

  <turtlebot_standoff_2in parent="base_link" number="0" x_loc="0.0676402" y_loc="0.1314196" z_loc="0.0709803"/>
  <turtlebot_standoff_2in parent="base_link" number="1" x_loc="0.0676402" y_loc="-0.1314196" z_loc="0.0709803"/>
  <turtlebot_standoff_2in parent="base_link" number="2" x_loc="-0.052832" y_loc="-0.1314196" z_loc="0.0709803"/>
  <turtlebot_standoff_2in parent="base_link" number="3" x_loc="-0.052832" y_loc="0.1314196" z_loc="0.0709803"/>

  <joint name="plate_1_joint" type="fixed">
    <origin xyz="0.04068 0 0.05715" rpy="0 0 0" />
    <parent link="plate_0_link"/>
    <child link="plate_1_link" />
  </joint>

  <link name="plate_1_link">
    <inertial>
      <mass value="0.1" />
      <origin xyz="0 0 0" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0"
        izz="1.0" />
    </inertial>

    <visual>
      <origin xyz=" 0 0 0 " rpy="0 0 0" />
      <geometry>
        <mesh filename="package://rbx1_description/meshes/plate_1_logo.dae" scale="${MESH_SCALE} ${MESH_SCALE} ${MESH_SCALE}"/>
      </geometry>
    </visual>

    <collision>
      <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      <geometry>
        <box size="0.314856 0.314856 0.006401"/>
      </geometry>
    </collision>
  </link>

  <turtlebot_standoff_2in parent="standoff_2in_0_link" number="4" x_loc="0" y_loc="0" z_loc="0.05715"/>
  <turtlebot_standoff_2in parent="standoff_2in_1_link" number="5" x_loc="0" y_loc="0" z_loc="0.05715"/>
  <turtlebot_standoff_2in parent="standoff_2in_2_link" number="6" x_loc="0" y_loc="0" z_loc="0.05715"/>
  <turtlebot_standoff_2in parent="standoff_2in_3_link" number="7" x_loc="0" y_loc="0" z_loc="0.05715"/>

  <joint name="plate_2_joint" type="fixed">
    <origin xyz="0 0 0.0572008" rpy="0 0 0" />
    <parent link="plate_1_link"/>
    <child link="plate_2_link" />
  </joint>

  <link name="plate_2_link">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0"
        izz="1.0" />
    </inertial>

    <visual>
      <origin xyz=" 0 0 0 " rpy="0 0 0" />
      <geometry>
        <mesh filename="package://rbx1_description/meshes/plate_1_logo.dae" scale="${MESH_SCALE} ${MESH_SCALE} ${MESH_SCALE}"/>
      </geometry>
    </visual>

    <collision>
      <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      <geometry>
        <box size="0.314856 0.314856 0.006401"/>
      </geometry>
    </collision>
  </link>
  
  <!--

  <joint name="base_kinect_joint" type="fixed">
    <origin xyz="${turtlebot_calib_cam_x} ${turtlebot_calib_cam_y} ${turtlebot_calib_cam_z}" 
            rpy="${turtlebot_calib_cam_rr} ${turtlebot_calib_cam_rp} ${turtlebot_calib_cam_ry}" />
    <parent link="${turtlebot_kinect_frame_name}" />
    <child link="kinect_link" />
  </joint>

  <link name="kinect_link">
    <inertial>
      <mass value="0.00001" />
      <origin xyz="0 0 0" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0"
        izz="1.0" />
    </inertial>

    <visual>
      <origin xyz=" 0 0 0 " rpy="0 0 0" />
      <geometry>
        <mesh filename="package://rbx1_description/meshes/kinect.dae"/>
      </geometry>
    </visual>

    <collision>
      <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      <geometry>
        <box size="0.0730 .2760 0.0720"/>
      </geometry>
    </collision>

  </link>

  <joint name="kinect_depth_joint" type="fixed">
    <origin xyz="0 0.018 0" rpy="0 0 0" />
    <parent link="kinect_link" />
    <child link="kinect_depth_frame" />
  </joint>

  <link name="kinect_depth_frame">
    <inertial>
      <mass value="0.000001" />
      <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="kinect_depth_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
    <parent link="kinect_depth_frame" />
    <child link="kinect_depth_optical_frame" />
  </joint>

  <link name="kinect_depth_optical_frame">
    <inertial>
      <mass value="0.000001" />
      <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="kinect_rgb_joint" type="fixed">
    <origin xyz="0 -0.005 0" rpy="0 0 0" />
    <parent link="kinect_link" />
    <child link="kinect_rgb_frame" />
  </joint>

  <link name="kinect_rgb_frame">
    <inertial>
      <mass value="0.000001" />
      <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="kinect_rgb_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
    <parent link="kinect_rgb_frame" />
    <child link="kinect_rgb_optical_frame" />
  </joint>

  <link name="kinect_rgb_optical_frame">
    <inertial>
      <mass value="0.000001" />
      <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:my_cylinder name="left_eye_link" l="0.01" r="0.01" xyz="0 0 0" rpy="0 1.57 0" mass="0.001" material="Gazebo/WhiteEmissive">
    <xacro:grey3 />
  </xacro:my_cylinder>
	
  <xacro:my_cylinder name="right_eye_link" l="0.01" r="0.01" xyz="0 0 00" rpy="0 1.57 0" mass="0.001" material="Gazebo/WhiteEmissive">
     <xacro:grey3 />
  </xacro:my_cylinder>

  <turtlebot_standoff_kinect parent="plate_2_link" number="0" x_loc="-0.1024382" y_loc="0.098" z_loc="0.0032004"/>
  <turtlebot_standoff_kinect parent="plate_2_link" number="1" x_loc="-0.1024382" y_loc="-0.098" z_loc="0.0032004"/>
  
-->

  <turtlebot_standoff_3in parent="standoff_2in_4_link" number="0" x_loc="0" y_loc="0" z_loc="0.05715"/>
  <turtlebot_standoff_3in parent="standoff_2in_5_link" number="1" x_loc="0" y_loc="0" z_loc="0.05715"/>
  <turtlebot_standoff_3in parent="standoff_2in_6_link" number="2" x_loc="0" y_loc="0" z_loc="0.05715"/>
  <turtlebot_standoff_3in parent="standoff_2in_7_link" number="3" x_loc="0" y_loc="0" z_loc="0.05715"/>

  <joint name="plate_3_joint" type="fixed">
    <origin xyz="-0.01316 0 0.08" rpy="0 0 0" />
    <parent link="plate_2_link"/>
    <child link="plate_3_link" />
  </joint>

  <link name="plate_3_link">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0"
        izz="1.0" />
    </inertial>

    <visual>
      <origin xyz=" 0 0 0 " rpy="0 0 0" />
      <geometry>
        <mesh filename="package://rbx1_description/meshes/plate_2_logo.dae" scale="${MESH_SCALE} ${MESH_SCALE} ${MESH_SCALE}"/>
      </geometry>
    </visual>

    <collision>
      <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      <geometry>
        <box size="0.288 0.315 0.006401"/>
      </geometry>
    </collision>
  </link>
  </macro>
</robot>
