action_servers_MPC_BC_operational_pick_experiment.launch

<?xml version="1.0"?>

<launch>
    <!-- set global arguments -->
    <arg name="robot_name" default="chonk"/>
    <param name="robot_description_wholebody" command="$(find xacro)/xacro --inorder '$(find chonk_pushing)/urdf/chonk_pushing_wholebody.urdf'"/>

    <!-- Spawn position controller -->
    <node
        name="trajectory_controller"
        pkg="controller_manager"
        type="spawner"
        ns="/$(arg robot_name)"
        args="/$(arg robot_name)/trajectory_controller"
        respawn="false"
        output="screen"
    />


    
    <!-- Launch a mux to send commands to the robot only from one source/controller -->
    <node
        name="mux_joint_position"
        pkg="topic_tools"
        type="mux"
        ns="/$(arg robot_name)"
        args="trajectory_controller/command
            streaming_controller/command
            DefaultPositionController/command
            ActionCmdPosePositionController/command
            ActionCmdConfigPositionController/command
            ActionTeleop2DPositionController/command
            mux:=mux_joint_position"
    />
    <!-- Action Service Node for commanding end-effector poses -->
    <node
        name="action_server_cmd_config"
        pkg="chonk_pushing"
        ns="/$(arg robot_name)"
        type="action_server_cmd_config.py"
        output="screen"
        >
        <param name="cmd_topic_name" type="string" value="/$(arg robot_name)/ActionCmdConfigPositionController/command"/>
        <remap from="/joint_states" to="/$(arg robot_name)/joint_states"/>
        <remap from="/mux_selected" to="/$(arg robot_name)/mux_joint_position/selected"/>
    </node>
    <!-- Action Service Node for planning joint positions and build MPC to track the planning results -->
    <node
        name="action_server_cmd_pose_MPC_BC_operational"
        pkg="chonk_pushing"
        ns="/$(arg robot_name)"
        type="action_server_cmd_pose_MPC_BC_operational_pick_experiment.py"
        output="screen"
        >
        <param name="link_donkey" type="string" value="link_donkey"/>
<!--        <param name="link_ee_right" type="string" value="RARM_END_EFFECTOR_finger"/>
        <param name="link_ee_left" type="string" value="LARM_END_EFFECTOR_finger"/>-->
        <param name="link_ee_right" type="string" value="RARM_END_EFFECTOR_grasp"/>
        <param name="link_ee_left" type="string" value="LARM_END_EFFECTOR_grasp"/>
<!--        <param name="link_sensor_right" type="string" value="RARM_changer_link_base"/>
        <param name="link_sensor_left" type="string" value="LARM_changer_link_base"/>-->
        <param name="link_head" type="string" value="CAMERA_HEAD_HEADING_Link"/>
        <param name="link_gaze" type="string" value="gaze_ref"/>
        <param name="cmd_topic_name" type="string" value="/$(arg robot_name)/ActionCmdPosePositionController/command"/>
        <remap from="/joint_states" to="/$(arg robot_name)/joint_states"/>
        <remap from="/mux_selected" to="/$(arg robot_name)/mux_joint_position/selected"/>
    </node>

</launch>