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

    <xacro:macro name="robotiq_gripper_ros2_control" params="
        name
        prefix
        use_fake_hardware
        fake_sensor_commands
        robot_ip
        robot_port
        min_position
        max_position
        max_angle
        min_speed
        max_speed
        min_force
        max_force">

        <ros2_control name="${name}" type="system">
            <!-- Plugins -->
            <hardware>
                 <xacro:if value="${use_fake_hardware}">
                    <plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
                    <param name="use_fake_hardware">${use_fake_hardware}</param>
                    <param name="mock_sensor_commands">${fake_sensor_commands}</param>
                    <param name="state_following_offset">0.0</param>
                </xacro:if>
                <xacro:unless value="${use_fake_hardware}">
                    <plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
                    <param name="gripper_closed_position">0.695</param>
                    <param name="robot_ip">${robot_ip}</param>
                    <param name="robot_port">${robot_port}</param>
                    <param name="use_fake_hardware">${use_fake_hardware}</param>
                </xacro:unless>
                <param name="gripper_speed_multiplier">1.0</param>
                <param name="gripper_force_multiplier">0.5</param>
                <param name="min_position">${min_position}</param>
                <param name="max_position">${max_position}</param>
                <param name="max_angle">${max_angle}</param>
                <param name="min_speed">${min_speed}</param>
                <param name="max_speed">${max_speed}</param>
                <param name="min_force">${min_force}</param>
                <param name="max_force">${max_force}</param>
            </hardware>

            <!-- Joint interfaces -->
            <!-- With Hardware, they handle mimic joints, so we only need this command interface activated -->
            <joint name="${prefix}gripper_gap">
                <command_interface name="position" />
                <state_interface name="position">
                    <param name="initial_value">0.0</param>
                </state_interface>
                <state_interface name="velocity"/>
            </joint>
            <joint name="${prefix}finger_joint">
                <state_interface name="position">
                    <param name="initial_value">0.695</param>
                </state_interface>
            </joint>
            <!-- When simulating we need to include the rest of the gripper joints -->
            <!-- <xacro:if value="${use_fake_hardware}"> -->
            <xacro:if value="false">
                <joint name="${prefix}right_outer_knuckle_joint">
                    <param name="mimic">${prefix}finger_joint</param>
                    <param name="multiplier">-1</param>
                    <command_interface name="position"/>
                    <state_interface name="position"/>
                    <state_interface name="velocity"/>
                </joint>
                <joint name="${prefix}left_outer_finger_joint">
                    <param name="mimic">${prefix}finger_joint</param>
                    <param name="multiplier">1</param>
                    <command_interface name="position"/>
                    <state_interface name="position"/>
                    <state_interface name="velocity"/>
                </joint>
                <joint name="${prefix}right_outer_finger_joint">
                    <param name="mimic">${prefix}finger_joint</param>
                    <param name="multiplier">-1</param>
                    <command_interface name="position"/>
                    <state_interface name="position"/>
                    <state_interface name="velocity"/>
                </joint>
                <joint name="${prefix}left_inner_knuckle_joint">
                    <param name="mimic">${prefix}finger_joint</param>
                    <param name="multiplier">-1</param>
                    <command_interface name="position"/>
                    <state_interface name="position"/>
                    <state_interface name="velocity"/>
                </joint>
                <joint name="${prefix}right_inner_knuckle_joint">
                    <param name="mimic">${prefix}finger_joint</param>
                    <param name="multiplier">-1</param>
                    <command_interface name="position"/>
                    <state_interface name="position"/>
                    <state_interface name="velocity"/>
                </joint>
                <joint name="${prefix}left_inner_finger_joint">
                    <param name="mimic">${prefix}finger_joint</param>
                    <param name="multiplier">1</param>
                    <command_interface name="position"/>
                    <state_interface name="position"/>
                    <state_interface name="velocity"/>
                </joint>
                <joint name="${prefix}right_inner_finger_joint">
                    <param name="mimic">${prefix}finger_joint</param>
                    <param name="multiplier">1</param>
                    <command_interface name="position"/>
                    <state_interface name="position"/>
                    <state_interface name="velocity"/>
                </joint>
            </xacro:if>

            <!-- Only add this with fake hardware mode -->
            <gpio name="reactivate_gripper">
                <command_interface name="reactivate_gripper_cmd" />
                <command_interface name="reactivate_gripper_response" />
            </gpio>

        </ros2_control>
    </xacro:macro>

</robot>
