<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:macro name="staubi" params="prefix">

  <!-- Constants for robot dimensions -->
  <xacro:property name="PI" value="3.1415926535897931"/>
  <xacro:property name="base_mass" value="0.3" /> <!-- arbitrary value for base mass -->
  <xacro:property name="base_width" value="0.345" />
  <xacro:property name="base_height" value="0.07" />
  <xacro:property name="floor_clearance" value="0.02" />
  <xacro:property name="wheel_mass" value="0.3" /> <!-- arbitrary value for wheel mass -->
  <xacro:property name="wheel_len" value="0.015" />
  <xacro:property name="wheel_radius" value="${0.071 / 2}" />
  <xacro:property name="wheel_distance" value="${0.222}" />
  <xacro:property name="caster_wheel_mass" value="0.1" /> <!-- arbitrary value for caster wheel mass -->
  <xacro:property name="caster_wheel_radius" value="0.030" />
  <xacro:property name="z_offset" value="-${base_height/2}" /> <!-- Space btw top of beam and the each joint -->

  <!-- Base Link -->
  <link name="${prefix}base_link">
    <collision>
      <origin xyz="0 0 ${floor_clearance+base_height/2}" rpy="0 0 0"/>
      <geometry>
          <cylinder length="${base_height}" radius="${base_width/2}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 ${floor_clearance+base_height/2}" rpy="0 0 0"/>
      <geometry>
          <cylinder length="${base_height}" radius="${base_width/2}"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <visual>
      <origin xyz="${base_width/2 - 0.02} 0 ${floor_clearance+base_height+0.005}" rpy="0 0 0"/>
      <geometry>
          <cylinder length="0.01" radius="0.015"/>
      </geometry>
      <material name="red"/>
    </visual>

    </link>

    <joint name="${prefix}left_wheel_joint" type="continuous">
      <parent link="${prefix}base_link"/>
      <child link="${prefix}left_wheel"/>
      <origin xyz="0 ${wheel_distance/2} ${wheel_radius}" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
      <dynamics damping="0.2"/>
      <limit effort="100" velocity="1.0"/>
    </joint>

    <!-- left wheel Link -->
    <link name="${prefix}left_wheel">
      <collision>
        <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
        <geometry>
          <cylinder length="${wheel_len}" radius="${wheel_radius}"/>
        </geometry>
      </collision>

      <visual>
        <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
        <geometry>
          <cylinder length="${wheel_len}" radius="${wheel_radius}"/>
        </geometry>
        <material name="black"/>
      </visual>

      </link>

    <joint name="${prefix}right_wheel_joint" type="continuous">
      <parent link="${prefix}base_link"/>
      <child link="${prefix}right_wheel"/>
      <origin xyz="0 -${wheel_distance/2} ${wheel_radius}" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
      <dynamics damping="0.2"/>
      <limit effort="100" velocity="1.0"/>
    </joint>

    <!-- right wheel Link -->
    <link name="${prefix}right_wheel">
      <collision>
        <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
        <geometry>
          <cylinder length="${wheel_len}" radius="${wheel_radius}"/>
        </geometry>
      </collision>

      <visual>
        <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
        <geometry>
          <cylinder length="${wheel_len}" radius="${wheel_radius}"/>
        </geometry>
        <material name="black"/>
      </visual>

      </link>

    <joint name="${prefix}caster_frontal_wheel_joint" type="fixed">
      <parent link="${prefix}base_link"/>
      <child link="${prefix}caster_frontal_wheel"/>
      <origin xyz="${base_width/2 - caster_wheel_radius} 0 ${caster_wheel_radius}" rpy="0 0 0"/>
    </joint>

    <!-- caster frontal wheel Link -->
    <link name="${prefix}caster_frontal_wheel">
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <sphere radius="${caster_wheel_radius}"/>
        </geometry>
      </collision>

      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <sphere radius="${caster_wheel_radius}"/>
        </geometry>
        <material name="grey"/>
      </visual>

      </link>

    <joint name="${prefix}laser_joint" type="fixed">
      <parent link="${prefix}base_link"/>
      <child link="${prefix}laser"/>
      <origin xyz="0.025 0.025 ${floor_clearance+base_height+0.01}" rpy="0 0 0.66939"/>
    </joint>

    <!-- laser Link -->
    <link name="${prefix}laser">
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder length="0.03" radius="0.035"/>
        </geometry>
      </collision>

      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder length="0.02" radius="0.035"/>
        </geometry>
        <material name="grey"/>
      </visual>
    </link>

  </xacro:macro>

</robot>
