<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="arm_slider_ros2_control" params="device_name mesh_path">
        <xacro:property name="initial_positions" value="${load_yaml(mesh_path + '/devices/arm_slider/config/initial_positions.yaml')['initial_positions']}"/>

        <ros2_control name="${device_name}arm_slider" 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="${device_name}arm_base_joint">
                <command_interface name="position"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['arm_base_joint']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="${device_name}arm_link_1_joint">
                <command_interface name="position"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['arm_link_1_joint']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="${device_name}arm_link_2_joint">
                <command_interface name="position"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['arm_link_2_joint']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="${device_name}arm_link_3_joint">
                <command_interface name="position"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['arm_link_3_joint']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="${device_name}gripper_base_joint">
                <command_interface name="position"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['gripper_base_joint']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="${device_name}gripper_right_joint">
                <command_interface name="position"/>
                <state_interface name="position">
                  <param name="initial_value">${initial_positions['gripper_right_joint']}</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>

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