<?xml version="1.0"?>

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


  <xacro:property name="PI" value="3.1415926535897931"/>

  <xacro:property name="chassisHeight" value="0.1"/>
  <xacro:property name="chassisLength" value="0.4"/>
  <xacro:property name="chassisWidth" value="0.2"/>
  <xacro:property name="chassisMass" value="1"/>

  <xacro:property name="casterRadius" value="0.05"/>
  <xacro:property name="casterMass" value="0.5"/>

  <xacro:property name="wheelWidth" value="0.05"/>
  <xacro:property name="wheelRadius" value="0.1"/>
  <xacro:property name="wheelPos" value="0.2"/>
  <xacro:property name="wheelMass" value="0.2"/>

  <xacro:property name="cameraSize" value="0.05"/>
  <xacro:property name="cameraMass" value="0.01"/>

  <xacro:include filename="$(find line_follower_description)/urdf/line_follower.gazebo" />
  <xacro:include filename="$(find line_follower_description)/urdf/materials.xacro" />
  <xacro:include filename="$(find line_follower_description)/urdf/macros.xacro" />

  <link name="footprint" />

  <joint name="base_joint" type="fixed">
    <parent link="footprint"/>
    <child link="chassis"/>
  </joint>

  <link name="chassis">
    <collision>
      <origin xyz="0 0 ${wheelRadius}" rpy="0 0 0"/>
      <geometry>
	<box size="${chassisLength} ${chassisWidth} ${chassisHeight}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 ${wheelRadius}" rpy="0 0 0"/>
      <geometry>
	<box size="${chassisLength} ${chassisWidth} ${chassisHeight}"/>
      </geometry>
      <material name="orange"/>
    </visual>

    <inertial>
      <origin xyz="0 0 ${wheelRadius}" rpy="0 0 0"/>
      <mass value="${chassisMass}"/>
      <box_inertia m="${chassisMass}" x="${chassisLength}" y="${chassisWidth}" z="${chassisHeight}"/>
    </inertial>
  </link>

  <joint name="fixed" type="fixed">
    <parent link="chassis"/>
    <child link="caster_wheel"/>
  </joint>

  <link name="caster_wheel">

    <collision>
      <origin xyz="${casterRadius-chassisLength/2} 0 ${casterRadius-chassisHeight+wheelRadius}" rpy="0 0 0"/>
      <geometry>
	<sphere radius="${casterRadius}"/>
      </geometry>
    </collision>
  
    <visual> 
      <origin xyz="${casterRadius-chassisLength/2} 0 ${casterRadius-chassisHeight+wheelRadius}" rpy="0 0 0"/>
      <geometry>
	<sphere radius="${casterRadius}"/>
      </geometry>
    </visual>

    <inertial>
      <origin xyz="${casterRadius-chassisLength/2} 0 ${casterRadius-chassisHeight+wheelRadius}" rpy="0 0 0"/>
      <mass value="${casterMass}"/>
      <sphere_inertia m="${casterMass}" r="${casterRadius}"/>
    </inertial>
  </link>


  <wheel lr="left" tY="-1"/>
  <wheel lr="right" tY="1"/>

  <joint name="camera_joint" type="fixed">
    <origin xyz="${chassisLength/2} 0 ${cameraSize+wheelRadius+cameraSize/2}" rpy="0 0.60 0"/>
    <parent link="chassis"/>
    <child link="camera"/>
  </joint>

  <link name="camera">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
	<box size="${cameraSize} ${cameraSize} ${cameraSize}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
	<box size="${cameraSize} ${cameraSize} ${cameraSize}"/>
      </geometry>
      <material name="blue"/>
    </visual>

    <inertial>
      <mass value="${cameraMass}" />
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <box_inertia m="${cameraMass}" x="${cameraSize}" y="${cameraSize}" z="${cameraSize}" />
    </inertial>
  </link>

</robot>
