<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:macro name="xarm5_ros2_control" params="prefix
    velocity_control:='false'
    ros2_control_plugin:='uf_robot_hardware/UFRobotSystemHardware'
    hw_ns:='xarm' add_gripper:='false'
    robot_ip:='' report_type:='normal' baud_checkset:='true' default_gripper_baud:=2000000
    joint1_lower_limit:=${-2.0*pi}  joint1_upper_limit:=${2.0*pi}
    joint2_lower_limit:=${-2.059}  joint2_upper_limit:=${2.0944}
    joint3_lower_limit:=${-3.927}  joint3_upper_limit:=${0.19198}
    joint4_lower_limit:=${-1.69297}  joint4_upper_limit:=${pi}
    joint5_lower_limit:=${-2.0*pi}  joint5_upper_limit:=${2.0*pi}">
    <ros2_control name="${prefix}${ros2_control_plugin}" type="system">
      <hardware>
        <plugin>${ros2_control_plugin}</plugin>
        <xacro:if value="${ros2_control_plugin == 'uf_robot_hardware/UFRobotSystemHardware'}">
          <param name="hw_ns">${prefix}${hw_ns}</param>
          <param name="velocity_control">${velocity_control}</param>
          <param name="prefix">P${prefix}</param>
          <param name="robot_ip">R${robot_ip}</param>
          <param name="report_type">${report_type}</param>
          <param name="dof">5</param>
          <param name="baud_checkset">${baud_checkset}</param>
          <param name="default_gripper_baud">${default_gripper_baud}</param>
          <param name="robot_type">xarm</param>
          <param name="add_gripper">${add_gripper}</param>
        </xacro:if>
      </hardware>
      <joint name="${prefix}joint1">
        <command_interface name="position">
          <param name="min">${joint1_lower_limit}</param>
          <param name="max">${joint1_upper_limit}</param>
        </command_interface>
        <command_interface name="velocity">
          <param name="min">-3.14</param>
          <param name="max">3.14</param>
        </command_interface>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <!-- <state_interface name="effort"/> -->
      </joint>
      <joint name="${prefix}joint2">
        <command_interface name="position">
          <param name="min">${joint2_lower_limit}</param>
          <param name="max">${joint2_upper_limit}</param>
        </command_interface>
        <command_interface name="velocity">
          <param name="min">-3.14</param>
          <param name="max">3.14</param>
        </command_interface>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <!-- <state_interface name="effort"/> -->
      </joint>
      <joint name="${prefix}joint3">
        <command_interface name="position">
          <param name="min">${joint3_lower_limit}</param>
          <param name="max">${joint3_upper_limit}</param>
        </command_interface>
        <command_interface name="velocity">
          <param name="min">-3.14</param>
          <param name="max">3.14</param>
        </command_interface>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <!-- <state_interface name="effort"/> -->
      </joint>
      <joint name="${prefix}joint4">
        <command_interface name="position">
          <param name="min">${joint4_lower_limit}</param>
          <param name="max">${joint4_upper_limit}</param>
        </command_interface>
        <command_interface name="velocity">
          <param name="min">-3.14</param>
          <param name="max">3.14</param>
        </command_interface>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <!-- <state_interface name="effort"/> -->
      </joint>
      <joint name="${prefix}joint5">
        <command_interface name="position">
          <param name="min">${joint5_lower_limit}</param>
          <param name="max">${joint5_upper_limit}</param>
        </command_interface>
        <command_interface name="velocity">
          <param name="min">-3.14</param>
          <param name="max">3.14</param>
        </command_interface>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <!-- <state_interface name="effort"/> -->
      </joint>
    </ros2_control>
  </xacro:macro>
</robot>