<?xml version="1.0"?>
<robot name="turtlebot_body" xmlns:xacro="http://ros.org/wiki/xacro">
  <property name="M_PI" value="3.14159"/>
  <property name="MESH_SCALE" value="100"/>

  <!-- Macro for TurtleBot body. Including Gazebo extensions, but does not include Kinect -->
  <xacro:include filename="$(find rbx1_description)/urdf/gazebo.urdf.xacro"/>

  <property name="base_x" value="0.33" />
  <property name="base_y" value="0.33" />

  <xacro:macro name="turtlebot_body">
    <material name="Green">
      <color rgba="0.0 0.8 0.0 1.0"/>
    </material>

  <!-- 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">
    <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="2" />
      <origin xyz="0 0 0.0" />
      <inertia ixx="0.01" ixy="0.0" ixz="0.0"
        iyy="0.01" iyz="0.0" izz="0.5" />
    </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.016495"/>
      </geometry>
    </collision>
  </link>

  <link name="wall_sensor_link">
    <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>


  <link name="left_cliff_sensor_link">
    <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>

  <link name="right_cliff_sensor_link">
    <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>

  <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="0.01" iyz="0.0" izz="0.01" />
    </inertial>
  </link>

  <link name="rightfront_cliff_sensor_link">
    <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="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="left_wheel_link">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.01" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
        iyy="0.001" iyz="0.0" izz="0.001" />
    </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="left_wheel_link"/>
    <axis xyz="0 1 0"/>
  </joint>

  <link name="right_wheel_link">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.01" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
        iyy="0.001" iyz="0.0" izz="0.001" />
    </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="right_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="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 1.5707 1.5707"/>
      <geometry>
        <sphere radius="0.015" />
      </geometry>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <sphere radius="0.015" />
      </geometry>
    </collision>
  </link>
  
  <!-- fixed because there's no transmission -->
  <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="0.001" ixy="0.0" ixz="0.0"
        iyy="0.001" iyz="0.0" izz="0.001" />
    </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>

  <!-- fixed because there's no transmission -->
  <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">
    <axis xyz="0 1 0" />
    <origin xyz="0 0 0.04" rpy="0 0 0" />
    <parent link="base_link"/>
    <child link="gyro_link"/>
  </joint>
  <link name="gyro_link">
    <inertial>
      <mass value="0.001" />
      <origin xyz="0 0 0" rpy="0 0 0" />
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="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.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="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="0.01" ixy="0.0" ixz="0.0"
        iyy="0.01" iyz="0.0"
        izz="0.01" />
    </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="0.01" ixy="0.0" ixz="0.0"
        iyy="0.01" iyz="0.0"
        izz="0.01" />
    </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="0.001" ixy="0.0" ixz="0.0"
        iyy="0.001" iyz="0.0"
        izz="0.001" />
    </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_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_8in parent="standoff_2in_4_link" number="0" x_loc="0" y_loc="0" z_loc="0.05715"/>
  <turtlebot_standoff_8in parent="standoff_2in_5_link" number="1" x_loc="0" y_loc="0" z_loc="0.05715"/>
  <turtlebot_standoff_8in parent="standoff_2in_6_link" number="2" x_loc="0" y_loc="0" z_loc="0.05715"/>
  <turtlebot_standoff_8in 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.2063496" 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="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>
        <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>
  
  <!-- Simulation sensors -->
  <turtlebot_sim_imu/>
  <turtlebot_sim_laser/>
  <turtlebot_sim_create/>
  <turtlebot_sim_wall_sensors/>
  </xacro:macro>

</robot>
