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

  <xacro:macro name="panda_arm_ros2_control" params="ns robot_ip use_fake_hardware:=^|false fake_sensor_commands:=^|false">
    <ros2_control name="FrankaHardwareInterface" type="system">
      <hardware>
        <xacro:if value="${use_fake_hardware}">
          <plugin>fake_components/GenericSystem</plugin>
          <param name="fake_sensor_commands">${fake_sensor_commands}</param>
          <param name="state_following_offset">0.0</param>
        </xacro:if>
        <xacro:unless value="${use_fake_hardware}">
          <plugin>franka_hardware/FrankaHardwareInterface</plugin>
          <param name="robot_ip">${robot_ip}</param>
        </xacro:unless>
      </hardware>

      <xacro:macro name="configure_joint" params="joint_name initial_position">
        <joint name="${joint_name}">
          <param name="initial_position">${initial_position}</param>
          <command_interface name="effort"/>
          <state_interface name="position"/>
          <state_interface name="velocity"/>
          <state_interface name="effort"/>
        </joint>
      </xacro:macro>

      <xacro:configure_joint joint_name="${ns}_joint1" initial_position="0.0"/>
      <xacro:configure_joint joint_name="${ns}_joint2" initial_position="${-pi/4}"/>
      <xacro:configure_joint joint_name="${ns}_joint3" initial_position="0.0"/>
      <xacro:configure_joint joint_name="${ns}_joint4" initial_position="${-3*pi/4}"/>
      <xacro:configure_joint joint_name="${ns}_joint5" initial_position="0.0"/>
      <xacro:configure_joint joint_name="${ns}_joint6" initial_position="${pi/2}"/>
      <xacro:configure_joint joint_name="${ns}_joint7" initial_position="${pi/4}"/>

    </ros2_control>
  </xacro:macro>
</robot>