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

  <xacro:macro name="diffbot" 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.26" />
  <xacro:property name="base_length" value="0.34" />
  <xacro:property name="base_height" value="0.1" />
  <xacro:property name="wheel_mass" value="0.3" /> <!-- arbitrary value for wheel mass -->
  <xacro:property name="wheel_len" value="0.020" />
  <xacro:property name="wheel_radius" value="0.0625" />
  <xacro:property name="caster_wheel_mass" value="0.1" /> <!-- arbitrary value for caster wheel mass -->
  <xacro:property name="caster_wheel_radius" value="0.04" />
  <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 0" rpy="0 0 0"/>
      <geometry>
        <box size="${base_width} ${base_length} ${base_height}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 ${base_height/2}" rpy="0 0 0"/>
      <geometry>
        <box size="${base_width} ${base_length} ${base_height}"/>
      </geometry>
      <material name="orange"/>
    </visual>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${base_mass}"/>
      <inertia
        ixx="${base_mass / 12.0 * (base_length*base_length + base_height*base_height)}" ixy="0.0" ixz="0.0"
        iyy="${base_mass / 12.0 * (base_height*base_height + base_width*base_width)}" iyz="0.0"
        izz="${base_mass / 12.0 * (base_width*base_width + base_length*base_length)}"/>
      </inertial>
    </link>

    <joint name="${prefix}left_wheel_joint" type="continuous">
      <parent link="${prefix}base_link"/>
      <child link="${prefix}left_wheel"/>
      <origin xyz="0 -0.07 ${z_offset}" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
      <dynamics damping="0.2"/>
    </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>

      <inertial>
        <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
        <mass value="${wheel_mass}"/>
        <inertia
          ixx="${wheel_mass / 12.0 * (3*wheel_radius + wheel_len*wheel_len)}" ixy="0.0" ixz="0.0"
          iyy="${wheel_mass / 12.0 * wheel_radius*wheel_radius}" iyz="0.0"
          izz="${wheel_mass / 12.0 * wheel_radius*wheel_radius}"/>
        </inertial>
      </link>

    <joint name="${prefix}right_wheel_joint" type="continuous">
      <parent link="${prefix}base_link"/>
      <child link="${prefix}right_wheel"/>
      <origin xyz="0 0.07 ${z_offset}" rpy="0 0 0"/>
      <axis xyz="0 -1 0"/>
      <dynamics damping="0.2"/>
    </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>

      <inertial>
        <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
        <mass value="${wheel_mass}"/>
        <inertia
          ixx="${wheel_mass / 12.0 * (3*wheel_radius + wheel_len*wheel_len)}" ixy="0.0" ixz="0.0"
          iyy="${wheel_mass / 12.0 * wheel_radius*wheel_radius}" iyz="0.0"
          izz="${wheel_mass / 12.0 * wheel_radius*wheel_radius}"/>
        </inertial>
      </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 ${z_offset}" 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="white"/>
      </visual>

      <inertial>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <mass value="${caster_wheel_mass}"/>
        <inertia
          ixx="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}" ixy="0.0" ixz="0.0"
          iyy="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}" iyz="0.0"
          izz="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}"/>
        </inertial>
      </link>

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

    <!-- caster rear wheel Link -->
    <link name="${prefix}caster_rear_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="white"/>
      </visual>

      <inertial>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <mass value="${caster_wheel_mass}"/>
        <inertia
          ixx="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}" ixy="0.0" ixz="0.0"
          iyy="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}" iyz="0.0"
          izz="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}"/>
        </inertial>
      </link>

      <!-- Laser Link -->
      <link name="${prefix}laser_link">
        <visual>
          <origin xyz="0 0 ${base_height}" rpy="0 0 0"/>
          <geometry>
            <!-- Define the geometry of the laser link, e.g., a cylinder -->
            <cylinder length="0.05" radius="0.02"/>
          </geometry>
        </visual>
      </link>

      <!-- Joint between Base Link and Laser Link -->
      <joint name="${prefix}laser_joint" type="fixed">
        <parent link="${prefix}base_link"/>
        <child link="${prefix}laser_link"/>
        <origin xyz="0 ${base_length/2} ${z_offset}" rpy="0 0 0"/>
      </joint>

      <!-- Camera Link -->
      <link name="${prefix}camera_link">
        <visual>
          <origin xyz="0 0 ${base_height}" rpy="0 0 0"/>
          <geometry>
            <!-- Define the geometry of the camera link, e.g., a box -->
            <box size="0.1 0.1 0.05"/>
          </geometry>
        </visual>
      </link>

      <!-- Joint between Base Link and Camera Link -->
      <joint name="${prefix}camera_joint" type="fixed">
        <parent link="${prefix}base_link"/>
        <child link="${prefix}camera_link"/>
        <origin xyz="0 ${base_length/2} ${z_offset + base_height}" rpy="0 0 0"/>
      </joint>



  </xacro:macro>

</robot>
