<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:macro name="aubo_ros2_control" params="hardware robot_ip:=^">
    <ros2_control name="aubo_hardware_interface" type="system">
      <hardware>
        <xacro:if value="${hardware=='gazebo'}">
          <plugin>gz_ros2_control/GazeboSimSystem</plugin>
        </xacro:if>
        <xacro:if value="${hardware=='mock'}">
          <plugin>mock_components/GenericSystem</plugin>
        </xacro:if>
        <xacro:if value="${hardware=='real'}">
          <plugin>aubo_driver/AuboHardwareInterface</plugin>
          <param name="robot_ip">${robot_ip}</param>
        </xacro:if>
      </hardware>

      <joint name="shoulder_joint">
        <command_interface name="position" />
        <command_interface name="velocity" />

        <state_interface name="position">
          <param name="initial_value">0.0</param>
        </state_interface>
        <state_interface name="velocity" />
      </joint>


      <joint name="upperArm_joint">
        <command_interface name="position" />
        <command_interface name="velocity" />

        <xacro:if value="${hardware=='gazebo'}">
          <state_interface name="position">
            <param name="initial_value">0.5</param>
          </state_interface>
        </xacro:if>
        <xacro:if value="${hardware=='real' or hardware=='mock'}">
          <state_interface name="position" />
        </xacro:if>
        <state_interface name="velocity" />
      </joint>


      <joint name="foreArm_joint">
        <command_interface name="position" />
        <command_interface name="velocity" />

        <xacro:if value="${hardware=='gazebo'}">
          <state_interface name="position">
            <param name="initial_value">1.75</param>
          </state_interface>
        </xacro:if>
        <xacro:if value="${hardware=='real' or hardware=='mock'}">
          <state_interface name="position" />
        </xacro:if>
        <state_interface name="velocity" />
      </joint>

      <joint name="wrist1_joint">
        <command_interface name="position" />
        <command_interface name="velocity" />

        <xacro:if value="${hardware=='gazebo'}">
          <state_interface name="position">
            <param name="initial_value">0.0</param>
          </state_interface>
        </xacro:if>
        <xacro:if value="${hardware=='real' or hardware=='mock'}">
          <state_interface name="position" />
        </xacro:if>
        <state_interface name="velocity" />
      </joint>

      <joint name="wrist2_joint">
        <command_interface name="position" />
        <command_interface name="velocity" />

        <xacro:if value="${hardware=='gazebo'}">
          <state_interface name="position">
            <param name="initial_value">1.57</param>
          </state_interface>
        </xacro:if>
        <xacro:if value="${hardware=='real' or hardware=='mock'}">
          <state_interface name="position" />
        </xacro:if>
        <state_interface name="velocity" />
      </joint>

      <joint name="wrist3_joint">
        <command_interface name="position" />
        <command_interface name="velocity" />

        <xacro:if value="${hardware=='gazebo'}">
          <state_interface name="position">
            <param name="initial_value">0.0</param>
          </state_interface>
        </xacro:if>
        <xacro:if value="${hardware=='real' or hardware=='mock'}">
          <state_interface name="position" />
        </xacro:if>
        <state_interface name="velocity" />
      </joint>

    </ros2_control>

  </xacro:macro>
</robot>
