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

        <ros2_control name="${name}" type="system">
            <hardware>
                <!-- By default, set up controllers for simulation. This won't work on real hardware -->
                <plugin>mock_components/GenericSystem</plugin>
            </hardware>
            <joint name="left_rev1">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['left_rev1']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="left_rev2">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['left_rev2']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="left_rev3">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['left_rev3']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="left_rev4">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['left_rev4']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="left_rev5">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['left_rev5']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="left_rev6">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['left_rev6']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="left_rev7">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['left_rev7']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="right_rev1">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['right_rev1']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="right_rev2">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['right_rev2']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="right_rev3">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['right_rev3']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="right_rev4">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['right_rev4']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="right_rev5">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['right_rev5']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="right_rev6">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['right_rev6']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="right_rev7">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['right_rev7']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="left_left_pris1">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['left_left_pris1']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="right_left_pris1">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['right_left_pris1']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>

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