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

  <!-- Gazebo-specific properties -->
  <xacro:property name="wheel_damping" value="0.1"/>
  <xacro:property name="wheel_axis" value="0 1 0"/>

  <!-- Base footprint -->
  <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="file://$(find agv_pro_description)/meshes/base_link.stl" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="file://$(find agv_pro_description)/meshes/base_link.stl" />
      </geometry>
    </collision>
  </link>

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

  <gazebo reference="base_link">
    <material>Gazebo/White</material>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <kp>100000.0</kp>
    <kd>1.0</kd>
  </gazebo>

  <!-- Include wheel macros -->
  <xacro:include filename="$(find agv_pro_description)/urdf/parts/wheel_macro.xacro"/>
  <xacro:include filename="$(find agv_pro_description)/urdf/parts/gazebo_control_plugin.xacro"/>
  <!-- Add all four wheels using macro -->
  <xacro:wheel name="right_rear_wheel" mesh="file://$(find 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="file://$(find 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="file://$(find 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="file://$(find 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="file://$(find agv_pro_description)/meshes/laser_link.stl" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="file://$(find 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>

  <!-- ros2_control tag -->
  <ros2_control name="AGVHardware" type="system">
    <hardware>
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>

    <joint name="right_rear_wheel_joint">
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
    <joint name="right_front_wheel_joint">
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
    <joint name="left_front_wheel_joint">
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
    <joint name="left_rear_wheel_joint">
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
  </ros2_control>

</robot>