<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="iisy_ros2_control" params="name initial_positions_file">
        <xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)['initial_positions']}"/>

        <gazebo>
            <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
                <parameters>$(find iisy_config)/config/ros2_controllers.yaml</parameters>
            </plugin>
        </gazebo>

        <ros2_control name="IgnitionSystem" type="system">
            <hardware>
                <!-- By default, set up controllers for simulation. This won't work on real hardware -->
                <plugin>ign_ros2_control/IgnitionSystem</plugin>
            </hardware>
            <joint name="base_rot">
                <command_interface name="position"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['base_rot']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="base_link1_joint">
                <command_interface name="position"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['base_link1_joint']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="link1_link2_joint">
                <command_interface name="position"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['link1_link2_joint']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="link2_rot">
                <command_interface name="position"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['link2_rot']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="link2_link3_joint">
                <command_interface name="position"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['link2_link3_joint']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="link3_rot">
                <command_interface name="position"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['link3_rot']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>

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