src/custom-ur-descriptions/cms_moveit_config/launch/robot_bringup.launch.py

"""Unified MoveIt launch file for UR5e with parameterized gripper support.

Usage:
    ros2 launch cms_moveit_config robot_bringup.launch.py gripper:=epick
    ros2 launch cms_moveit_config robot_bringup.launch.py gripper:=hande use_mock_hardware:=true

Supported grippers: none, epick, hande, 2fg7, pipettor

This replaces 5 separate MoveIt config packages with a single parameterized package.
Uses OpaqueFunction pattern (same as UR driver upstream) to resolve gripper arg
before constructing MoveItConfigsBuilder calls.

NOTE: robot_state_publisher is NOT started here — the UR driver's ur_control.launch.py
starts its own RSP unconditionally. Starting a second one causes duplicate /tf
publications with subtly different kinematic models (#51).
"""

from moveit_configs_utils import MoveItConfigsBuilder
from launch.substitutions import FindExecutable, LaunchConfiguration
from launch_ros.actions import Node
from launch.actions import (
    DeclareLaunchArgument,
    ExecuteProcess,
    IncludeLaunchDescription,
    OpaqueFunction,
    SetEnvironmentVariable,
    TimerAction,
)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch import LaunchDescription
from launch_param_builder import ParameterBuilder
from ament_index_python.packages import get_package_share_path


# ─── Per-gripper configuration ──────────────────────────────────────────────
# Per-gripper launch values come from the active beamline YAML
# (grippers.<name>, via config_loader.gripper_launch_config) — this file is a
# generic interpreter with no hardcoded gripper dict (#77). A new beamline with
# existing gripper types sets only YAML; only brand-new gripper hardware adds a
# structural branch (the gripper-specific spawner/driver Nodes below).
#
# ros2_control controllers are NOT per-gripper: every gripper loads the SAME
# shared base (ur_base_controllers.yaml = full UR set), with gripper-specific
# controllers added by the spawner Nodes via --param-file overlay (#86). So the
# base file stays a launch constant here, not a YAML field.
from beambot.config_loader import gripper_launch_config

_BASE_CONTROLLERS = "ur_base_controllers.yaml"

SUPPORTED_GRIPPERS = ["none", "epick", "hande", "2fg7", "pipettor"]


def launch_setup(context, *args, **kwargs):
    """Resolve gripper arg and build all launch actions.

    This runs inside OpaqueFunction so LaunchConfiguration values are available
    as plain strings — required because MoveItConfigsBuilder.load_yaml() resolves
    eagerly and needs string paths, not LaunchConfiguration substitutions.
    """
    gripper = LaunchConfiguration("gripper").perform(context)
    robot_ip = LaunchConfiguration("robot_ip").perform(context)
    ur_type = LaunchConfiguration("ur_type").perform(context)
    use_mock_hardware = LaunchConfiguration("use_mock_hardware").perform(context)
    tf_prefix = LaunchConfiguration("tf_prefix").perform(context)

    if gripper not in SUPPORTED_GRIPPERS:
        raise ValueError(
            f"Unknown gripper '{gripper}'. Supported: {SUPPORTED_GRIPPERS}"
        )

    # Per-gripper launch values from the active beamline YAML (#77).
    config = gripper_launch_config(gripper)
    pkg_share = get_package_share_path("cms_moveit_config")
    desc_share = get_package_share_path("cms_robot_description")

    # ── Xacro args ──────────────────────────────────────────────────────
    xacro_args = {
        "name": ur_type,
        "ur_type": ur_type,
        "tf_prefix": tf_prefix,
        "robot_ip": robot_ip,
        # Use the per-robot calibrated kinematics so move_group's planning model
        # matches the UR driver's robot_state_publisher (which gets the same file
        # via kinematics_params_file below). Without this the wrapper xacro falls
        # back to nominal default_kinematics.yaml and every IK/Cartesian goal lands
        # ~2.5mm off the commanded pose (calibration delta).
        "kinematics_params": str(desc_share / "config" / "ur5e_calibration.yaml"),
    }

    # Gripper-specific xacro args
    if gripper == "epick":
        xacro_args["cup_profile"] = LaunchConfiguration("cup_profile").perform(context)
    elif gripper == "hande":
        xacro_args["socat_ip_address"] = robot_ip

    # ── UR driver ───────────────────────────────────────────────────────
    ur_launch_args = {
        "ur_type": ur_type,
        "robot_ip": robot_ip,
        "tf_prefix": tf_prefix,
        "use_mock_hardware": use_mock_hardware,
        "launch_rviz": "false",
        "description_package": "ur_description",
        "description_file": str(desc_share / "urdf" / config["urdf_xacro"]),
        "controllers_file": str(pkg_share / "config" / _BASE_CONTROLLERS),
        "kinematics_params_file": str(desc_share / "config" / "ur5e_calibration.yaml"),
        "use_tool_communication": (
            "false" if use_mock_hardware == "true"
            else config["use_tool_communication"]
        ),
        "tool_voltage": config["tool_voltage"],
        # Jazzy: hardware loads async, so spawners need longer timeout to avoid
        # retry cycles while controller_manager is busy initializing.
        "controller_spawner_timeout": "30",
    }
    # RS485 params (2fg7 needs 1Mbps/Even)
    ur_launch_args.update(config["tool_comm_params"])

    ur_control_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            str(get_package_share_path("ur_robot_driver") / "launch" / "ur_control.launch.py")
        ),
        launch_arguments=ur_launch_args.items(),
    )

    # ── MoveIt config ───────────────────────────────────────────────────
    # joint_limits and trajectory_execution use absolute paths because
    # MoveItConfigsBuilder's default inference looks for config/joint_limits.yaml
    # at the package root, but our per-gripper files are in config/<gripper>/.
    moveit_config = (
        MoveItConfigsBuilder("ur_moveit", package_name="cms_moveit_config")
        .robot_description(
            file_path=str(desc_share / "urdf" / config["urdf_xacro"]),
            mappings=xacro_args,
        )
        .robot_description_semantic(
            file_path=str(pkg_share / "srdf" / "ur.srdf.xacro"),
            mappings={"gripper": gripper},
        )
        .joint_limits(
            file_path=str(pkg_share / "config" / gripper / "joint_limits.yaml"),
        )
        .trajectory_execution(
            file_path=str(pkg_share / "config" / config["moveit_controllers"]),
        )
        .robot_description_kinematics(file_path="config/kinematics.yaml")
        .planning_scene_monitor(
            publish_robot_description=False,
            publish_robot_description_semantic=True,
        )
        .planning_pipelines(
            pipelines=["ompl", "pilz_industrial_motion_planner", "stomp"]
        )
        .to_moveit_configs()
    )

    move_group_capabilities = {
        "capabilities": "move_group/ExecuteTaskSolutionCapability"
    }

    run_move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[
            moveit_config.robot_description_kinematics,
            moveit_config.to_dict(),
            move_group_capabilities,
            # Wait for joint_states before declaring ready (controllers load async in Jazzy)
            {"planning_scene_monitor_options.wait_for_initial_state_timeout": 30.0},
        ],
    )

    # ── RViz ────────────────────────────────────────────────────────────
    rviz_config = str(pkg_share / "rviz" / "view_robot_mtc.rviz")

    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        output="log",
        arguments=["-d", rviz_config],
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
            moveit_config.planning_pipelines,
            moveit_config.joint_limits,
        ],
        # The orchestrator imports cv2 (YOLO warm-up) before launching this
        # subprocess. opencv-python's config-3.py forcibly sets
        # QT_QPA_PLATFORM_PLUGIN_PATH to its bundled Qt5 plugins, which RViz2
        # (built against system Qt6) then loads and aborts on (ABI mismatch).
        # Override it back to the system Qt6 platforms dir for RViz only.
        additional_env={
            "QT_QPA_PLATFORM_PLUGIN_PATH": "/usr/lib/x86_64-linux-gnu/qt6/plugins/platforms",
        },
    )

    # Payload is set by the orchestrator's lifecycle manager after launch
    # (readiness-gated via the set_payload service), not here.

    # ── Build launch actions list ───────────────────────────────────────
    actions = [ur_control_launch]
    if gripper == "epick":
        # The UR driver does not forward cup_profile to Xacro.
        actions.insert(0, SetEnvironmentVariable(
            "BEAMBOT_EPICK_CUP_PROFILE", xacro_args["cup_profile"],
        ))

    # Common nodes
    actions.append(run_move_group_node)
    actions.append(rviz_node)

    # Optional 8BitDo/Xbox-compatible Cartesian teleoperation. SDL supplies a
    # stable gamepad mapping; MoveIt Servo supplies limits and collision stops.
    joystick_enabled = IfCondition(LaunchConfiguration("enable_joystick"))
    joystick_config = str(pkg_share / "config" / "joystick_teleop.yaml")
    servo_params = {
        "moveit_servo": ParameterBuilder("cms_moveit_config")
        .yaml("config/servo.yaml")
        .to_dict()
    }
    actions.extend([
        Node(
            package="joy",
            executable="game_controller_node",
            name="game_controller_node",
            parameters=[{
                "device_id": LaunchConfiguration("joystick_device_id"),
                "deadzone": 0.12,
                "autorepeat_rate": 50.0,
            }],
            condition=joystick_enabled,
        ),
        Node(
            package="teleop_twist_joy",
            executable="teleop_node",
            name="translation_teleop",
            parameters=[joystick_config],
            remappings=[("/cmd_vel", "/joystick/servo_node/delta_twist_cmds")],
            condition=joystick_enabled,
        ),
        Node(
            package="teleop_twist_joy",
            executable="teleop_node",
            name="rotation_teleop",
            parameters=[joystick_config],
            remappings=[("/cmd_vel", "/joystick/servo_node/delta_twist_cmds")],
            condition=joystick_enabled,
        ),
        Node(
            package="cms_moveit_config",
            executable="cms_servo_node",
            name="servo_node",
            namespace="joystick",
            parameters=[
                servo_params,
                # AccelerationLimitedPlugin requires these unnamespaced params.
                {"update_period": 0.01},
                {"planning_group_name": "ur_arm"},
                moveit_config.robot_description,
                moveit_config.robot_description_semantic,
                moveit_config.robot_description_kinematics,
                moveit_config.joint_limits,
            ],
            output="screen",
            condition=joystick_enabled,
        ),
        # Select Cartesian twist input after Servo has initialized.
        TimerAction(
            period=12.0,
            actions=[
                ExecuteProcess(
                    cmd=[
                        FindExecutable(name="ros2"),
                        "service", "call",
                        "/joystick/servo_node/switch_command_type",
                        "moveit_msgs/srv/ServoCommandType",
                        "{command_type: 1}",
                    ],
                    output="screen",
                )
            ],
            condition=joystick_enabled,
        ),
    ])

    # ── Gripper-specific nodes ──────────────────────────────────────────
    # Gripper controllers are NOT in the shared base controllers file; each
    # spawner carries its controller's type+params via --param-file overlay
    # (config/<gripper>_controllers.yaml). The spawner reads the overlay with
    # plain yaml.safe_load — no $(var ...) expansion — so overlays use literal
    # values. See #86.
    if gripper == "epick":
        # One spawner for both ePick controllers (native mode takes multiple
        # names) — shares a single load/switch and removes any ordering ambiguity.
        actions.append(Node(
            package="controller_manager",
            executable="spawner",
            arguments=["epick_gripper_action_controller", "epick_status_publisher_controller",
                       "-c", "/controller_manager",
                       "--param-file", str(pkg_share / "config" / "epick_controllers.yaml")],
        ))
    elif gripper == "hande":
        actions.append(Node(
            package="controller_manager",
            executable="spawner",
            arguments=["gripper_action_controller", "-c", "/controller_manager",
                       "--param-file", str(pkg_share / "config" / "hande_controllers.yaml")],
        ))
    elif gripper == "2fg7":
        actions.append(TimerAction(
            period=3.0,  # Wait for tool_communication to create /tmp/ttyUR
            actions=[
                Node(
                    package="onrobot_2fg7_driver",
                    executable="onrobot_2fg7_driver_node",
                    name="onrobot_2fg7_driver",
                    output="screen",
                    parameters=[{
                        "serial_port": "/tmp/ttyUR",
                        "slave_id": 65,
                        "baudrate": 1000000,
                        "use_fake_hardware": use_mock_hardware == "true",
                    }],
                ),
            ],
        ))
    elif gripper == "pipettor":
        actions.append(TimerAction(
            period=3.0,  # Wait for tool_communication to create /tmp/ttyUR
            actions=[
                Node(
                    package="pipette_driver",
                    executable="pipette_driver_node",
                    name="pipette_driver_node",
                    output="screen",
                    parameters=[
                        {"serial_port": "/tmp/ttyUR"},
                        {"use_fake_hardware": use_mock_hardware == "true"},
                    ],
                ),
            ],
        ))

    return actions


def generate_launch_description():
    return LaunchDescription([
        # ── Arguments ───────────────────────────────────────────────────
        DeclareLaunchArgument(
            "gripper", default_value="none",
            choices=SUPPORTED_GRIPPERS,
            description="Gripper type: none, epick, hande, 2fg7, pipettor",
        ),
        DeclareLaunchArgument(
            "robot_ip", default_value="192.168.1.101",
            description="Default matches CMS beamline YAML; orchestrator always "
                        "overrides via robot_ip:= from $BEAMBOT_BEAMLINE_CONFIG.",
        ),
        DeclareLaunchArgument(
            "ur_type", default_value="ur5e",
        ),
        DeclareLaunchArgument(
            "use_mock_hardware", default_value="false",
        ),
        DeclareLaunchArgument(
            "tf_prefix", default_value="",
            description="Joint/link name prefix; must match controllers' $(var tf_prefix).",
        ),
        DeclareLaunchArgument(
            "enable_joystick", default_value="false",
            description="Start 8BitDo/Xbox-compatible gamepad teleoperation",
        ),
        DeclareLaunchArgument(
            "joystick_device_id", default_value="0",
            description="SDL game-controller device ID from joy_enumerate_devices",
        ),
        # ePick cup profile (only used when gripper:=epick).
        # Profile name resolves to dimensions via suction_cups.yaml in the xacro.
        DeclareLaunchArgument(
            "cup_profile", default_value="3mm_dia_external_air",
            description="ePick suction cup profile name (from suction_cups.yaml)",
        ),
        # ── OpaqueFunction resolves gripper then builds all nodes ───────
        OpaqueFunction(function=launch_setup),
    ])