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

  <!--
    Single ros2_control block, shared by the real robot and the Gazebo sim.
    Only the <hardware> plugin differs:
      sim=false -> agv_pro_hardware/AgvProHardwareInterface  (real serial base)
      sim=true  -> gz_ros2_control/GazeboSimSystem           (Gazebo Harmonic)

    Joint names match the wheel macro:
      <prefix>{front_left,front_right,rear_left,rear_right}_wheel_joint
    Each wheel sits at its true corner, so the mapping is 1:1 (no swaps).
  -->
  <xacro:macro name="agv_pro_ros2_control" params="sim prefix port_name">
    <ros2_control name="AgvProSystem" type="system">
      <hardware>
        <xacro:if value="${sim}">
          <plugin>gz_ros2_control/GazeboSimSystem</plugin>
        </xacro:if>
        <xacro:unless value="${sim}">
          <plugin>agv_pro_hardware/AgvProHardwareInterface</plugin>
          <param name="port_name">${port_name}</param>
          <param name="wheel_radius">0.072</param>
          <param name="lx">0.172</param>
          <param name="ly">0.180</param>
          <param name="front_left_joint">${prefix}front_left_wheel_joint</param>
          <param name="front_right_joint">${prefix}front_right_wheel_joint</param>
          <param name="rear_left_joint">${prefix}rear_left_wheel_joint</param>
          <param name="rear_right_joint">${prefix}rear_right_wheel_joint</param>
        </xacro:unless>
      </hardware>

      <joint name="${prefix}front_left_wheel_joint">
        <command_interface name="velocity"/>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
      </joint>
      <joint name="${prefix}front_right_wheel_joint">
        <command_interface name="velocity"/>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
      </joint>
      <joint name="${prefix}rear_left_wheel_joint">
        <command_interface name="velocity"/>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
      </joint>
      <joint name="${prefix}rear_right_wheel_joint">
        <command_interface name="velocity"/>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
      </joint>
    </ros2_control>
  </xacro:macro>
</robot>
