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

  <xacro:property name="wheel_axis" value="0 1 0"/>
  <xacro:property name="wheel_damping" value="0.1"/>

  <!-- Macro: wheel with Gazebo plugin -->
  <xacro:macro name="wheel" params="name mesh origin_xyz origin_rpy mass ixx ixy ixz iyy iyz izz">
    <link name="${name}_link">
      <inertial>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <mass value="${mass}" />
        <inertia ixx="${ixx}" ixy="${ixy}" ixz="${ixz}" iyy="${iyy}" iyz="${iyz}" izz="${izz}" />
      </inertial>
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="${mesh}" />
        </geometry>
        <material name=""><color rgba="1 1 1 1"/></material>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="${mesh}" />
        </geometry>
      </collision>
    </link>
    <joint name="${name}_joint" type="continuous">
      <origin xyz="${origin_xyz}" rpy="${origin_rpy}"/>
      <parent link="base_link"/>
      <child link="${name}_link"/>
      <axis xyz="${wheel_axis}"/>
      <dynamics damping="${wheel_damping}"/>
    </joint>

    <transmission name="${name}_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <actuator name="${name}_motor">
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
      <joint name="${name}_joint">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      </joint>
    </transmission>

    <gazebo reference="${name}_link">
      <mu1>0.8</mu1>
      <mu2>0.8</mu2>
      <kp>100000.0</kp>
      <kd>1.0</kd>
      <material>Gazebo/Grey</material>
    </gazebo>
  </xacro:macro>

  <!-- Base links -->
  <link name="base_footprint"/>

  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0 0 0.020" rpy="0 0 0"/>
  </joint>

  <link name="base_link">
    <inertial>
      <origin xyz="-0.0076254 -0.00023134 0.06693" rpy="0 0 0" />
      <mass value="19.236" />
      <inertia ixx="0.14436" ixy="0.0012037" ixz="0.0019458"
               iyy="0.24191" iyz="0.0044629" izz="0.33755"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://agv_pro_description/meshes/base_link.stl"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://agv_pro_description/meshes/base_link.stl"/>
      </geometry>
    </collision>
  </link>

  <!-- Gazebo plugin for ros2_control -->
  <gazebo>
    <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so"/>
  </gazebo>

  <!-- Wheels -->
  <xacro:wheel name="right_rear_wheel"
               mesh="package://agv_pro_description/meshes/wheel_rb_link.stl"
               origin_xyz="-0.1718 -0.1799 0.05188" origin_rpy="0 0 0"
               mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07"
               iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>

  <xacro:wheel name="right_front_wheel"
               mesh="package://agv_pro_description/meshes/wheel_rf_link.stl"
               origin_xyz="-0.17181 0.1799 0.051884" origin_rpy="0 0 0"
               mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07"
               iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>

  <xacro:wheel name="left_front_wheel"
               mesh="package://agv_pro_description/meshes/wheel_lf_link.stl"
               origin_xyz="0.17128 0.1799 0.052" origin_rpy="0 0 0"
               mass="0.3015" ixx="0.00052475" ixy="-2.2533E-07" ixz="-4.1904E-07"
               iyy="0.00099948" iyz="7.5332E-08" izz="0.00052362"/>

  <xacro:wheel name="left_rear_wheel"
               mesh="package://agv_pro_description/meshes/wheel_lb_link.stl"
               origin_xyz="0.17128 -0.1799 0.052" origin_rpy="0 0 0"
               mass="0.29613" ixx="0.00051227" ixy="-2.0628E-07" ixz="-4.9074E-07"
               iyy="0.0009752" iyz="1.173E-07" izz="0.00051131"/>

  <!-- Lidar -->
  <link name="laser_link">
    <inertial>
      <origin xyz="-0.0035142 -2.8248E-05 0.0010013" rpy="0 0 0"/>
      <mass value="0.049095"/>
      <inertia ixx="2.0572E-05" ixy="8.5013E-08" ixz="2.0871E-07"
               iyy="2.0483E-05" iyz="-4.2154E-09" izz="3.4612E-05"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://agv_pro_description/meshes/laser_link.stl"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://agv_pro_description/meshes/laser_link.stl"/>
      </geometry>
    </collision>
  </link>

  <joint name="lidar_joint" type="fixed">
    <origin xyz="0.17891 0 0.20928" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="laser_link"/>
  </joint>

</robot>
