<?xml version="1.0"?>
<robot name="robot3" xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:property name="M_PI"         value="3.14159265" />
  <xacro:property name="base_length"  value="0.3" />
  <xacro:property name="base_width"   value="0.2" />
  <xacro:property name="base_height"  value="1.0" />
  <xacro:property name="wheel_radius" value="0.1" />
  <xacro:property name="wheel_length" value="0.05" />
  <xacro:property name="lrf_size"     value="0.1" />

  <link name="base_footprint">
    <internal>
      <origin xyz="0 0 -${wheel_radius*0.5}" />
    </internal>
  </link>

  <link name="base_link">
    <visual>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
    </collision>
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.500"/>
      <inertia ixx="0.0020833333" ixy="0" ixz="0" iyy="0.0070833333" iyz="0" izz="0.0083333333"/>
    </inertial>
  </link>

  <joint name="base_link_joint" type="fixed">
    <origin xyz="0 0 ${base_height*0.5+wheel_radius*0.5-0.1}"/>
    <parent link="base_footprint"/>
    <child link="base_link"/>
  </joint>


  <link name="right_wheel_link">
    <visual>
      <geometry>
        <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="black"/>
    </visual>

    <collision>
      <geometry>
        <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.500"/>
      <inertia ixx="0.0013541667" ixy="0" ixz="0" iyy="0.0013541667" iyz="0" izz="0.0025"/>
    </inertial>

  </link>

  <joint name="right_wheel_joint" type="continuous">
    <parent link="base_footprint"/>
    <child link="right_wheel_link"/>
    <origin rpy="${M_PI*0.5} 0 0" xyz="${base_length*0.5-0.1} -${base_width*0.5+wheel_length} 0"/>
    <axis xyz="0 0 -1"/>
  </joint>

  <link name="left_wheel_link">
    <visual>
      <geometry>
        <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="black"/>
    </visual>

    <collision>
      <geometry>
        <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.500"/>
      <inertia ixx="0.0013541667" ixy="0" ixz="0" iyy="0.0013541667" iyz="0" izz="0.0025"/>
    </inertial>

  </link>

  <joint name="left_wheel_joint" type="continuous">
    <parent link="base_footprint"/>
    <child link="left_wheel_link"/>
    <origin rpy="${M_PI*0.5} 0 0" xyz="${base_length*0.5-0.1} ${base_width*0.5+wheel_length} 0"/>
    <axis xyz="0 0 -1"/>
  </joint>

  <link name="caster_link">
    <visual>
      <geometry>
        <sphere radius="${wheel_radius*0.5}"/>
      </geometry>
    </visual>

    <collision>
      <geometry>
        <sphere radius="${wheel_radius*0.5}"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.500"/>
      <inertia ixx="0.00025" ixy="0" ixz="0" iyy="0.00025"  iyz="0" izz="0.00025"/>
    </inertial>

  </link>

  <joint name="caster_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="caster_link"/>
    <origin xyz="-${base_length*0.5-wheel_radius*0.5} 0  -${wheel_radius*0.5}"/>
  </joint>

  <!-- ===============  Transmission =============== -->
  <transmission name="right_wheel_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_wheel_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_wheel_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>15</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="left_wheel_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_wheel_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_wheel_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>15</mechanicalReduction>
    </actuator>
  </transmission>

  <!-- =============== Sensor =============== -->
  <!-- =============== lrf =============== -->
  <link name="lrf_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="${lrf_size} ${lrf_size} ${lrf_size}"/>
      </geometry>
      <material name="Blue" />
    </visual>
  </link>
  <joint name="lrf_joint" type="fixed">
    <origin xyz="${(base_length+lrf_size)*0.5} 0 ${base_height*0.5}" rpy="0 0 0" />
    <parent link="base_footprint"/>
    <child link="lrf_link" />
  </joint>

  <gazebo reference="lrf_link">
    <material>Gazebo/Blue</material>
    <turnGravityOff>false</turnGravityOff>
    <sensor type="ray" name="lrf_sensor">
      <pose>0.2 0 0 0 0 0</pose>
      <visualize>true</visualize>
      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>500</samples>
            <resolution>1</resolution>
            <min_angle>-${M_PI*3/4}</min_angle>
            <max_angle>${M_PI*3/4}</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.20</min>
          <max>131</max>
          <resolution>0.01</resolution>
        </range>
      </ray>
      <plugin name="gazebo_ros_lrf_controller" filename="libgazebo_ros_laser.so">
        <topicName>scan</topicName>
        <frameName>lrf_link</frameName>
        <min_range>0.9</min_range>
        <max_range>130</max_range>
      </plugin>
    </sensor>
  </gazebo>

  <!-- =============== Gazebo =============== -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>robot3</robotNamespace>
    </plugin>
  </gazebo>

  <gazebo reference="base_link">
    <selfCollide>true</selfCollide>
    <material>Gazebo/White</material>
    <mu1 value="0.05" />
    <mu2 value="0.05" />
  </gazebo>

  <gazebo reference="right_wheel_link">
    <selfCollide>true</selfCollide>
    <material>Gazebo/Green</material>
    <mu1 value="0.8" />
    <mu2 value="0.8" />
  </gazebo>

  <gazebo reference="left_wheel_link">
    <selfCollide>true</selfCollide>
    <material>Gazebo/Red</material>
    <mu1 value="0.8" />
    <mu2 value="0.8" />
  </gazebo>

  <gazebo reference="caster_link">
    <selfCollide>true</selfCollide>
    <material>Gazebo/Black</material>
    <!-- This caster is no frictional resistance. -->
    <mu1 value="0.0" />
    <mu2 value="0.0" />
  </gazebo>
</robot>
