"""单臂/双臂通用调试入口。 该 launch 文件用于现场调试阶段按需启动左臂、右臂或双臂，并可通过 `use_mock` 在 mock 模式和 RM75 真机模式之间切换。它会固定启动 UDP 手柄接收节点，再根据 `arm:=left|right|both` 选择对应的遥操作节点。 """ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def _as_bool(value: str) -> bool: """把 launch 字符串参数转换成 Python bool，便于在 OpaqueFunction 中分支。""" return value.strip().lower() in ("1", "true", "yes", "on") def _config_file(name: str) -> PathJoinSubstitution: """生成 xr_rm_bringup/config 下配置文件的可安装路径。""" return PathJoinSubstitution([ FindPackageShare("xr_rm_bringup"), "config", name, ]) def _udp_receiver_node() -> Node: """接收 PICO/XR UDP 数据，并发布左右手柄 ROS2 话题。""" return Node( package="xr_rm_input", executable="udp_controller_receiver", name="udp_controller_receiver", output="screen", parameters=[{ "udp_host": LaunchConfiguration("udp_host"), "udp_port": LaunchConfiguration("udp_port"), "left_topic": "/xr/left_controller", "right_topic": "/xr/right_controller", }], ) def _single_arm_node( arm: str, use_mock: bool, move_to_initial_pose: bool, avoid_singularity: int, frame_type: int, configure_safety_limits: bool, ) -> Node: """创建单臂调试节点；左/右臂分别使用独立 YAML，节点名保持单臂默认名。""" config_name = "left_arm_rm75.yaml" if arm == "left" else "right_arm_rm75.yaml" robot_ip = LaunchConfiguration("left_robot_ip" if arm == "left" else "right_robot_ip") return Node( package="xr_rm_teleop", executable="single_arm_velocity_teleop", name="single_arm_velocity_teleop", output="screen", parameters=[ _config_file(config_name), { "use_mock": use_mock, "robot_ip": robot_ip, "robot_port": LaunchConfiguration("robot_port"), "avoid_singularity": avoid_singularity, "frame_type": frame_type, "configure_safety_limits": configure_safety_limits, "move_to_initial_pose_on_connect": move_to_initial_pose, }, ], ) def _dual_arm_nodes( use_mock: bool, move_to_initial_pose: bool, left_avoid_singularity: int, right_avoid_singularity: int, frame_type: int, configure_safety_limits: bool, ) -> list[Node]: """创建双臂节点；两个节点共用双臂 YAML，但节点名区分左右臂参数命名空间。""" config_file = _config_file("dual_arm_rm75.yaml") return [ Node( package="xr_rm_teleop", executable="single_arm_velocity_teleop", name="left_arm_teleop", output="screen", parameters=[ config_file, { "use_mock": use_mock, "robot_ip": LaunchConfiguration("left_robot_ip"), "robot_port": LaunchConfiguration("robot_port"), "avoid_singularity": left_avoid_singularity, "frame_type": frame_type, "configure_safety_limits": configure_safety_limits, "move_to_initial_pose_on_connect": move_to_initial_pose, }, ], ), Node( package="xr_rm_teleop", executable="single_arm_velocity_teleop", name="right_arm_teleop", output="screen", parameters=[ config_file, { "use_mock": use_mock, "robot_ip": LaunchConfiguration("right_robot_ip"), "robot_port": LaunchConfiguration("robot_port"), "avoid_singularity": right_avoid_singularity, "frame_type": frame_type, "configure_safety_limits": configure_safety_limits, "move_to_initial_pose_on_connect": move_to_initial_pose, }, ], ), ] def _launch_setup(context, *args, **kwargs): """运行时读取 launch 参数，决定启动单臂还是双臂。""" del args, kwargs arm = LaunchConfiguration("arm").perform(context).strip().lower() use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context)) move_to_initial_pose = _as_bool( LaunchConfiguration("move_to_initial_pose_on_connect").perform(context) ) avoid_override = LaunchConfiguration("avoid_singularity").perform(context).strip() left_avoid_singularity = int( avoid_override or LaunchConfiguration("left_avoid_singularity").perform(context) ) right_avoid_singularity = int( avoid_override or LaunchConfiguration("right_avoid_singularity").perform(context) ) frame_type = int(LaunchConfiguration("frame_type").perform(context)) configure_safety_limits = _as_bool( LaunchConfiguration("configure_safety_limits").perform(context) ) if arm not in ("left", "right", "both"): raise ValueError("arm must be one of: left, right, both") nodes = [_udp_receiver_node()] if arm == "both": nodes.extend( _dual_arm_nodes( use_mock, move_to_initial_pose, left_avoid_singularity, right_avoid_singularity, frame_type, configure_safety_limits, ) ) else: avoid_singularity = left_avoid_singularity if arm == "left" else right_avoid_singularity nodes.append( _single_arm_node( arm, use_mock, move_to_initial_pose, avoid_singularity, frame_type, configure_safety_limits, ) ) return nodes def generate_launch_description() -> LaunchDescription: return LaunchDescription([ # 调试目标：left/right/both；默认右臂方便单臂逐步上真机。 DeclareLaunchArgument("arm", default_value="right"), # true 时只跑 mock，不连接 RM75；false 时通过 RealMan SDK 连接真机。 DeclareLaunchArgument("use_mock", default_value="true"), # UDP 监听参数，需要与 PICO 端或 sample_udp_sender 保持一致。 DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), DeclareLaunchArgument("udp_port", default_value="15000"), # 左右 RM75 默认 IP，可在命令行中按现场网络覆盖。 DeclareLaunchArgument("left_robot_ip", default_value="192.168.192.18"), DeclareLaunchArgument("right_robot_ip", default_value="192.168.192.19"), DeclareLaunchArgument("robot_port", default_value="8080"), # 真机速度透传与安全配置参数。左臂现场控制器对硬件避奇异初始化 # 可能返回超时，因此默认关闭硬件避奇异，只保留软件侧限幅。 DeclareLaunchArgument("left_avoid_singularity", default_value="0"), DeclareLaunchArgument("right_avoid_singularity", default_value="1"), # 非空时作为左右臂全局覆盖，例如 avoid_singularity:=0。 DeclareLaunchArgument("avoid_singularity", default_value=""), DeclareLaunchArgument("frame_type", default_value="1"), DeclareLaunchArgument("configure_safety_limits", default_value="true"), # 默认不自动移动到初始点，确认安全区后再显式打开。 DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"), # OpaqueFunction 允许根据 arm/use_mock 等运行时参数动态生成节点。 OpaqueFunction(function=_launch_setup), ])