diff --git a/xr_rm_bringup/config/dual_arm_rm75.yaml b/xr_rm_bringup/config/dual_arm_rm75.yaml index b9808ba..8ce6962 100755 --- a/xr_rm_bringup/config/dual_arm_rm75.yaml +++ b/xr_rm_bringup/config/dual_arm_rm75.yaml @@ -23,13 +23,13 @@ left_arm_teleop: kp_linear: 1.8 deadband_m: 0.002 low_pass_alpha: 0.35 - max_linear_speed: 0.04 + max_linear_speed: 0.12 enable_position_axes: [true, true, true] # 来自 acDual-arm 的 bounds_p[left]： # x[-0.50, 0.50]，y[-0.60, -0.20]，z[0.10, 0.50]。 - workspace_min: [-0.50, -0.60, 0.10] - workspace_max: [0.50, -0.20, 0.50] + workspace_min: [-0.70, -0.60, 0.10] + workspace_max: [0.70, 0.40, 0.70] cyl_radius_limit: [0.20, 0.60] low_z_threshold: 0.20 low_z_min_radius: 0.21 @@ -45,7 +45,7 @@ left_arm_teleop: mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400] robot_ip: 192.168.192.18 robot_port: 8080 - avoid_singularity: 1 + avoid_singularity: 0 frame_type: 1 follow: false configure_safety_limits: true @@ -60,6 +60,9 @@ left_arm_teleop: initial_joint_pose: [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51] initial_tcp_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400] init_move_speed: 20 + command_mode: pose_canfd + canfd_trajectory_mode: 2 + canfd_radio: 0 debug_topic_prefix: /xr_rm right_arm_teleop: @@ -73,7 +76,7 @@ right_arm_teleop: kp_linear: 1.8 deadband_m: 0.002 low_pass_alpha: 0.35 - max_linear_speed: 0.04 + max_linear_speed: 0.12 enable_position_axes: [true, true, true] # 来自 acDual-arm 的 bounds_p[right]： diff --git a/xr_rm_bringup/config/left_arm_rm75.yaml b/xr_rm_bringup/config/left_arm_rm75.yaml index d5bec28..d42ab32 100644 --- a/xr_rm_bringup/config/left_arm_rm75.yaml +++ b/xr_rm_bringup/config/left_arm_rm75.yaml @@ -11,11 +11,11 @@ single_arm_velocity_teleop: kp_linear: 1.8 deadband_m: 0.002 low_pass_alpha: 0.35 - max_linear_speed: 0.04 + max_linear_speed: 0.2 enable_position_axes: [true, true, true] - workspace_min: [-0.50, -0.60, 0.10] - workspace_max: [0.50, -0.20, 0.50] + workspace_min: [-0.70, -0.60, 0.10] + workspace_max: [0.70, 0.40, 0.70] cyl_radius_limit: [0.20, 0.60] low_z_threshold: 0.20 low_z_min_radius: 0.21 @@ -28,7 +28,7 @@ single_arm_velocity_teleop: mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400] robot_ip: 192.168.192.18 robot_port: 8080 - avoid_singularity: 1 + avoid_singularity: 0 frame_type: 1 follow: false configure_safety_limits: true @@ -42,4 +42,7 @@ single_arm_velocity_teleop: initial_joint_pose: [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51] initial_tcp_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400] init_move_speed: 20 + command_mode: pose_canfd + canfd_trajectory_mode: 2 + canfd_radio: 0 debug_topic_prefix: /xr_rm diff --git a/xr_rm_bringup/config/right_arm_rm75.yaml b/xr_rm_bringup/config/right_arm_rm75.yaml index 4a5d9b0..4251d08 100644 --- a/xr_rm_bringup/config/right_arm_rm75.yaml +++ b/xr_rm_bringup/config/right_arm_rm75.yaml @@ -11,11 +11,11 @@ single_arm_velocity_teleop: kp_linear: 1.8 deadband_m: 0.002 low_pass_alpha: 0.35 - max_linear_speed: 0.04 + max_linear_speed: 0.2 enable_position_axes: [true, true, true] workspace_min: [-0.70, -0.60, 0.10] - workspace_max: [0.50, 0.40, 0.70] + workspace_max: [0.70, 0.40, 0.70] cyl_radius_limit: [0.20, 0.60] low_z_threshold: 0.20 low_z_min_radius: 0.21 diff --git a/xr_rm_bringup/launch/arm_debug.launch.py b/xr_rm_bringup/launch/arm_debug.launch.py index 00ca1ef..1e2a735 100644 --- a/xr_rm_bringup/launch/arm_debug.launch.py +++ b/xr_rm_bringup/launch/arm_debug.launch.py @@ -1,3 +1,10 @@ +"""单臂/双臂通用调试入口。 + +该 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 @@ -6,10 +13,12 @@ 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", @@ -18,6 +27,7 @@ def _config_file(name: str) -> PathJoinSubstitution: def _udp_receiver_node() -> Node: + """接收 PICO/XR UDP 数据，并发布左右手柄 ROS2 话题。""" return Node( package="xr_rm_input", executable="udp_controller_receiver", @@ -32,7 +42,15 @@ def _udp_receiver_node() -> Node: ) -def _single_arm_node(arm: str, use_mock: bool, move_to_initial_pose: bool) -> Node: +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( @@ -46,13 +64,24 @@ def _single_arm_node(arm: str, use_mock: bool, move_to_initial_pose: bool) -> No "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) -> list[Node]: +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( @@ -66,6 +95,9 @@ def _dual_arm_nodes(use_mock: bool, move_to_initial_pose: bool) -> list[Node]: "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, }, ], @@ -81,6 +113,9 @@ def _dual_arm_nodes(use_mock: bool, move_to_initial_pose: bool) -> list[Node]: "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, }, ], @@ -89,33 +124,78 @@ def _dual_arm_nodes(use_mock: bool, move_to_initial_pose: bool) -> list[Node]: 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)) + nodes.extend( + _dual_arm_nodes( + use_mock, + move_to_initial_pose, + left_avoid_singularity, + right_avoid_singularity, + frame_type, + configure_safety_limits, + ) + ) else: - nodes.append(_single_arm_node(arm, use_mock, move_to_initial_pose)) + 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), ]) diff --git a/xr_rm_bringup/launch/dual_arm_mock.launch.py b/xr_rm_bringup/launch/dual_arm_mock.launch.py index b0b6b41..07cf13d 100755 --- a/xr_rm_bringup/launch/dual_arm_mock.launch.py +++ b/xr_rm_bringup/launch/dual_arm_mock.launch.py @@ -1,3 +1,10 @@ +"""双臂 mock 遥操作启动入口。 + +该 launch 文件用于不连接真实 RM75 的离线闭环验证：启动 UDP 手柄接收节点， +并分别启动左、右两个 `single_arm_velocity_teleop` 节点。两个遥操作节点使用 +同一份双臂配置文件，但通过节点名读取各自的参数命名空间。 +""" + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition @@ -7,6 +14,7 @@ from launch_ros.substitutions import FindPackageShare def generate_launch_description() -> LaunchDescription: + # 通过 robot_config 参数选择 xr_rm_bringup/config 下的双臂配置文件。 config_file = PathJoinSubstitution([ FindPackageShare("xr_rm_bringup"), "config", @@ -14,9 +22,11 @@ def generate_launch_description() -> LaunchDescription: ]) return LaunchDescription([ + # UDP 监听参数，需要和 PICO 端或 sample_udp_sender 的目标地址保持一致。 DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), DeclareLaunchArgument("udp_port", default_value="15000"), DeclareLaunchArgument("robot_config", default_value="dual_arm_rm75.yaml"), + # 这些开关便于单独验证 UDP、左臂或右臂链路。 DeclareLaunchArgument("run_udp", default_value="true"), DeclareLaunchArgument("run_left_arm", default_value="true"), DeclareLaunchArgument("run_right_arm", default_value="true"), diff --git a/xr_rm_bringup/launch/dual_arm_realman.launch.py b/xr_rm_bringup/launch/dual_arm_realman.launch.py index eb8531d..3b0de23 100755 --- a/xr_rm_bringup/launch/dual_arm_realman.launch.py +++ b/xr_rm_bringup/launch/dual_arm_realman.launch.py @@ -1,3 +1,10 @@ +"""双 RM75 真机遥操作启动入口。 + +该 launch 文件用于连接左右两台真实 RM75：启动 UDP 手柄接收节点，并分别启动 +左、右两个真机遥操作节点。默认不会自动移动到初始点，只有显式设置 +`move_to_initial_pose_on_connect:=true` 后才会执行初始化点位移动。 +""" + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition @@ -8,6 +15,7 @@ from launch_ros.substitutions import FindPackageShare def generate_launch_description() -> LaunchDescription: + # 双臂真机默认读取 xr_rm_bringup/config/dual_arm_rm75.yaml。 config_file = PathJoinSubstitution([ FindPackageShare("xr_rm_bringup"), "config", @@ -15,16 +23,21 @@ def generate_launch_description() -> LaunchDescription: ]) return LaunchDescription([ + # UDP 监听参数，需要与 PICO 端发送地址和端口匹配。 DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), DeclareLaunchArgument("udp_port", default_value="15000"), + # 左右 RM75 的默认现场 IP，可在 launch 命令中按实际网络覆盖。 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("robot_config", default_value="dual_arm_rm75.yaml"), + # 调试时可关闭部分节点，只验证 UDP 或单侧机械臂。 DeclareLaunchArgument("run_udp", default_value="true"), DeclareLaunchArgument("run_left_arm", default_value="true"), DeclareLaunchArgument("run_right_arm", default_value="true"), + # 真机默认不自动动臂，确认安全区清空后再手动打开。 DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"), + # 接收 PICO/XR 侧 UDP 数据，并按左右手字段分发到对应手柄话题。 Node( package="xr_rm_input", executable="udp_controller_receiver", @@ -38,6 +51,7 @@ def generate_launch_description() -> LaunchDescription: "right_topic": "/xr/right_controller", }], ), + # 左臂真机节点：连接 left_robot_ip，并读取 left_arm_teleop 参数命名空间。 Node( package="xr_rm_teleop", executable="single_arm_velocity_teleop", @@ -57,6 +71,7 @@ def generate_launch_description() -> LaunchDescription: }, ], ), + # 右臂真机节点：连接 right_robot_ip，并读取 right_arm_teleop 参数命名空间。 Node( package="xr_rm_teleop", executable="single_arm_velocity_teleop", diff --git a/xr_rm_bringup/tools/launcher_ui.py b/xr_rm_bringup/tools/launcher_ui.py index 25bf202..8cc15ce 100755 --- a/xr_rm_bringup/tools/launcher_ui.py +++ b/xr_rm_bringup/tools/launcher_ui.py @@ -1,4 +1,10 @@ #!/usr/bin/env python3 +"""XR-RM 桌面调试启动器。 + +提供 Tkinter 图形界面，按“仿真/左臂/右臂/双臂/诊断”组织常用 ROS2 +launch、sample_udp_sender、topic 监控和环境检查命令，降低现场调试时的命令输入成本。 +""" + from __future__ import annotations import ctypes @@ -25,6 +31,13 @@ except ImportError as exc: DEFAULT_LEFT_IP = "192.168.192.18" DEFAULT_RIGHT_IP = "192.168.192.19" DEFAULT_PORT = "15000" +SAMPLE_SENDER_SECONDS = "30" +SAMPLE_SENDER_STAGGERED_SECONDS = "60" +SAMPLE_SENDER_ARGS = ( + f"--host 127.0.0.1 --port {DEFAULT_PORT} " + "--pattern axis_sweep --amplitude 0.5 " + "--hold-seconds 4.0 --center-seconds 1.2 --initial-center-seconds 1.0" +) TERMINAL_TITLE_PREFIX = "XR-RM Terminal - " TOPIC_MONITOR_TITLE = "XR-RM Topic Monitor" TOPIC_MONITOR_ACTION = "__xr_rm_topic_monitor__" @@ -62,7 +75,22 @@ def _with_index(items: list[tuple[str, str]]) -> list[tuple[str, str]]: return [(f"{idx}. {title}", cmd) for idx, (title, cmd) in enumerate(items, start=1)] +def _sample_udp_sender_command( + hand: str, + seconds: str = SAMPLE_SENDER_SECONDS, + both_mode: str = "synchronized", +) -> str: + command = ( + f"ros2 run xr_rm_input sample_udp_sender --hand {hand} " + f"{SAMPLE_SENDER_ARGS} --seconds {seconds}" + ) + if hand == "both": + command += f" --both-mode {both_mode}" + return command + + def _find_workspace_root() -> Path: + # 优先使用显式环境变量，便于从安装目录、源码目录或桌面快捷方式启动。 env_workspace = os.environ.get("XR_RM_WS") if env_workspace: return Path(env_workspace).expanduser().resolve() @@ -77,6 +105,7 @@ def _find_workspace_root() -> Path: def _source_lines(workspace_root: Path) -> list[str]: + # 每个新终端都重新 source ROS 和工作空间，避免依赖启动器自身的 shell 环境。 ros_setup = Path("/opt/ros/humble/setup.bash") install_setup = workspace_root / "install" / "setup.bash" lines = [ @@ -100,11 +129,13 @@ def _source_lines(workspace_root: Path) -> list[str]: def _one_click_mock(arm: str, hand: str) -> str: + sender_seconds = SAMPLE_SENDER_STAGGERED_SECONDS if hand == "both" else SAMPLE_SENDER_SECONDS + both_mode = "staggered" if hand == "both" else "synchronized" return "\n".join([ f"ros2 launch xr_rm_bringup arm_debug.launch.py arm:={arm} use_mock:=true &", "launch_pid=$!", "sleep 2", - f"ros2 run xr_rm_input sample_udp_sender --hand {hand} --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 20", + _sample_udp_sender_command(hand, sender_seconds, both_mode), "echo", "echo 'Sample sender finished. The launch process is still running in this terminal.'", "echo 'Press Ctrl-C here, or use the cleanup button in the launcher, to stop it.'", @@ -133,22 +164,23 @@ def _finalize_items( def build_commands_by_mode(mode: str) -> list[tuple[str, str]]: + # UI 列表只维护命令模板；真正执行时统一套上工作空间 source 和终端包装。 if mode == "Simulation": items = [ ("Left Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true"), ("Right Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true"), ("Dual Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true"), ( - "Sample UDP Sender (Left, 10s)", - f"ros2 run xr_rm_input sample_udp_sender --hand left --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10", + "Sample UDP Sender (Left, 30s)", + _sample_udp_sender_command("left"), ), ( - "Sample UDP Sender (Right, 10s)", - f"ros2 run xr_rm_input sample_udp_sender --hand right --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10", + "Sample UDP Sender (Right, 30s)", + _sample_udp_sender_command("right"), ), ( - "Sample UDP Sender (Both, 10s)", - f"ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10", + "Sample UDP Sender (Both Staggered, 60s)", + _sample_udp_sender_command("both", SAMPLE_SENDER_STAGGERED_SECONDS, "staggered"), ), ("One-Click Left Mock Demo", _one_click_mock("left", "left")), ("One-Click Right Mock Demo", _one_click_mock("right", "right")), @@ -165,8 +197,8 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]: "move_to_initial_pose_on_connect:=false", ), ( - "Sample UDP Sender (Left, 10s)", - f"ros2 run xr_rm_input sample_udp_sender --hand left --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10", + "Sample UDP Sender (Left, 30s)", + _sample_udp_sender_command("left"), ), ] one_click = None @@ -180,8 +212,8 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]: "move_to_initial_pose_on_connect:=false", ), ( - "Sample UDP Sender (Right, 10s)", - f"ros2 run xr_rm_input sample_udp_sender --hand right --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10", + "Sample UDP Sender (Right, 30s)", + _sample_udp_sender_command("right"), ), ] one_click = None @@ -196,8 +228,8 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]: "move_to_initial_pose_on_connect:=false", ), ( - "Sample UDP Sender (Both, 10s)", - f"ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10", + "Sample UDP Sender (Both Staggered, 60s)", + _sample_udp_sender_command("both", SAMPLE_SENDER_STAGGERED_SECONDS, "staggered"), ), ] one_click = None @@ -213,6 +245,8 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]: class LauncherApp: + """XR-RM 调试启动器主窗口，负责命令列表、终端启动和监控窗口管理。""" + def __init__(self, root: tk.Tk) -> None: self.root = root self.workspace_root = _find_workspace_root() @@ -421,6 +455,7 @@ class LauncherApp: return result.returncode == 0 def check_prerequisites(self) -> None: + # 环境检查只做轻量探测，避免在 UI 线程里执行耗时的网络或硬件连接动作。 ok: list[str] = [] warnings: list[str] = [] errors: list[str] = [] @@ -604,16 +639,72 @@ class LauncherApp: return Path(script_file.name) def topic_monitor_rect(self) -> tuple[int, int, int, int]: - screen_width = max(self.root.winfo_screenwidth(), 1) - screen_height = max(self.root.winfo_screenheight(), 1) - width = min(screen_width - 160, max(960, int(screen_width * 0.75))) - height = min(screen_height - 160, max(560, int(screen_height * 0.66))) - width = max(width, min(screen_width, 900)) - height = max(height, min(screen_height, 520)) - left = max((screen_width - width) // 2, 0) - top = max((screen_height - height) // 2, 0) + # 监控窗口尽量放在启动器所在显示器，双屏现场调试时更容易找回窗口。 + monitor_left, monitor_top, monitor_width, monitor_height = self.active_monitor_rect() + width = min(monitor_width - 120, max(960, int(monitor_width * 0.75))) + height = min(monitor_height - 120, max(560, int(monitor_height * 0.66))) + width = max(width, min(monitor_width, 900)) + height = max(height, min(monitor_height, 520)) + left = monitor_left + max((monitor_width - width) // 2, 0) + top = monitor_top + max((monitor_height - height) // 2, 0) return width, height, left, top + def active_monitor_rect(self) -> tuple[int, int, int, int]: + monitors = self.xrandr_monitor_rects() + if not monitors: + return (0, 0, max(self.root.winfo_screenwidth(), 1), max(self.root.winfo_screenheight(), 1)) + + self.root.update_idletasks() + root_center = ( + self.root.winfo_rootx() + max(self.root.winfo_width(), 1) // 2, + self.root.winfo_rooty() + max(self.root.winfo_height(), 1) // 2, + ) + pointer = (self.root.winfo_pointerx(), self.root.winfo_pointery()) + + for point_x, point_y in (root_center, pointer): + for left, top, width, height, _primary in monitors: + if left <= point_x < left + width and top <= point_y < top + height: + return (left, top, width, height) + + for left, top, width, height, primary in monitors: + if primary: + return (left, top, width, height) + + left, top, width, height, _primary = monitors[0] + return (left, top, width, height) + + @staticmethod + def xrandr_monitor_rects() -> list[tuple[int, int, int, int, bool]]: + if not shutil.which("xrandr"): + return [] + + try: + output = subprocess.check_output( + ["xrandr", "--query"], + text=True, + stderr=subprocess.DEVNULL, + ) + except Exception: + return [] + + monitors: list[tuple[int, int, int, int, bool]] = [] + pattern = re.compile( + r"^\S+ connected(?P primary)? " + r"(?P\d+)x(?P\d+)\+(?P-?\d+)\+(?P-?\d+)" + ) + for line in output.splitlines(): + match = pattern.match(line) + if not match: + continue + monitors.append(( + int(match.group("x")), + int(match.group("y")), + int(match.group("w")), + int(match.group("h")), + bool(match.group("primary")), + )) + return monitors + def window_ids_matching_title(self, title: str) -> list[int]: if not shutil.which("xprop"): return [] @@ -641,16 +732,87 @@ class LauncherApp: window_ids.append(int(xid_text, 16)) return window_ids - def force_monitor_window_geometry(self, title: str, attempts_left: int = 40) -> None: + def force_monitor_window_geometry( + self, + title: str, + target_rect: tuple[int, int, int, int], + attempts_left: int = 30, + ) -> None: if attempts_left <= 0 or not self.root.winfo_exists(): return window_ids = self.window_ids_matching_title(title) - if window_ids: - width, height, left, top = self.topic_monitor_rect() + if not window_ids: + self.root.after( + 80, + lambda: self.force_monitor_window_geometry(title, target_rect, attempts_left - 1), + ) + return + + if any(self.monitor_window_needs_geometry(window_id, target_rect) for window_id in window_ids): + width, height, left, top = target_rect self.apply_x11_window_geometry(window_ids, left, top, width, height) - self.root.after(250, lambda: self.force_monitor_window_geometry(title, attempts_left - 1)) + def monitor_window_needs_geometry(self, window_id: int, target_rect: tuple[int, int, int, int]) -> bool: + props = self.x11_window_props(window_id) + if "_NET_WM_STATE_FULLSCREEN" in props or "_NET_WM_STATE_MAXIMIZED_HORZ" in props: + return True + if "_NET_WM_STATE_MAXIMIZED_VERT" in props: + return True + + geometry = self.x11_window_geometry(window_id) + if geometry is None: + return True + + actual_left, actual_top, actual_width, actual_height = geometry + target_width, target_height, target_left, target_top = target_rect + if abs(actual_width - target_width) > 180 or abs(actual_height - target_height) > 180: + return True + if abs(actual_left - target_left) > 180 or abs(actual_top - target_top) > 180: + return True + return False + + @staticmethod + def x11_window_props(window_id: int) -> str: + if not shutil.which("xprop"): + return "" + try: + return subprocess.check_output( + ["xprop", "-id", hex(window_id), "_NET_WM_STATE"], + text=True, + stderr=subprocess.DEVNULL, + ) + except Exception: + return "" + + @staticmethod + def x11_window_geometry(window_id: int) -> tuple[int, int, int, int] | None: + if not shutil.which("xwininfo"): + return None + try: + output = subprocess.check_output( + ["xwininfo", "-id", hex(window_id)], + text=True, + stderr=subprocess.DEVNULL, + ) + except Exception: + return None + + values: dict[str, int] = {} + patterns = { + "left": r"Absolute upper-left X:\s+(-?\d+)", + "top": r"Absolute upper-left Y:\s+(-?\d+)", + "width": r"Width:\s+(\d+)", + "height": r"Height:\s+(\d+)", + } + for key, pattern in patterns.items(): + match = re.search(pattern, output) + if match: + values[key] = int(match.group(1)) + + if len(values) != 4: + return None + return values["left"], values["top"], values["width"], values["height"] def apply_x11_window_geometry(self, window_ids: list[int], left: int, top: int, width: int, height: int) -> None: lib_path = ctypes.util.find_library("X11") @@ -769,8 +931,8 @@ class LauncherApp: event_mask = (1 << 19) | (1 << 20) # SubstructureNotifyMask | SubstructureRedirectMask xlib.XSendEvent(display, root, False, event_mask, ctypes.byref(event)) - def write_topic_monitor_config(self) -> Path: - width, height, left, top = self.topic_monitor_rect() + def write_topic_monitor_config(self, target_rect: tuple[int, int, int, int] | None = None) -> Path: + width, height, left, top = target_rect or self.topic_monitor_rect() left_script, right_script = [ self.write_topic_monitor_script(title, topic) for title, topic in TOPIC_MONITORS @@ -778,6 +940,8 @@ class LauncherApp: layout_name = "xr_rm_topic_monitor" config_text = "\n".join([ "[global_config]", + " geometry_hinting = False", + " window_state = normal", " suppress_multiple_term_dialog = True", "[keybindings]", "[profiles]", @@ -822,8 +986,8 @@ class LauncherApp: config_file.write(config_text) return Path(config_file.name) - def write_ros_graph_monitor_config(self) -> Path: - width, height, left, top = self.topic_monitor_rect() + def write_ros_graph_monitor_config(self, target_rect: tuple[int, int, int, int] | None = None) -> Path: + width, height, left, top = target_rect or self.topic_monitor_rect() topic_script, node_script = [ self.write_ros_graph_monitor_script(title, command) for title, command in ROS_GRAPH_MONITORS @@ -831,6 +995,8 @@ class LauncherApp: layout_name = "xr_rm_ros_graph_monitor" config_text = "\n".join([ "[global_config]", + " geometry_hinting = False", + " window_state = normal", " suppress_multiple_term_dialog = True", "[keybindings]", "[profiles]", @@ -892,7 +1058,8 @@ class LauncherApp: ) return - config_path = self.write_topic_monitor_config() + target_rect = self.topic_monitor_rect() + config_path = self.write_topic_monitor_config(target_rect) command = [ terminal, "--no-dbus", @@ -910,7 +1077,7 @@ class LauncherApp: stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, ) - self.root.after(150, lambda: self.force_monitor_window_geometry(TOPIC_MONITOR_TITLE)) + self.root.after(60, lambda: self.force_monitor_window_geometry(TOPIC_MONITOR_TITLE, target_rect)) self.status.config(text="Opened Terminator controller topic monitor.") except Exception as exc: messagebox.showerror("Topic Monitor Failed", f"Failed to launch topic monitor:\n{exc}") @@ -933,7 +1100,8 @@ class LauncherApp: ) return - config_path = self.write_ros_graph_monitor_config() + target_rect = self.topic_monitor_rect() + config_path = self.write_ros_graph_monitor_config(target_rect) command = [ terminal, "--no-dbus", @@ -951,7 +1119,7 @@ class LauncherApp: stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, ) - self.root.after(150, lambda: self.force_monitor_window_geometry(ROS_GRAPH_MONITOR_TITLE)) + self.root.after(60, lambda: self.force_monitor_window_geometry(ROS_GRAPH_MONITOR_TITLE, target_rect)) self.status.config(text="Opened Terminator ROS graph monitor.") except Exception as exc: messagebox.showerror("ROS Graph Monitor Failed", f"Failed to launch ROS graph monitor:\n{exc}") @@ -997,6 +1165,7 @@ class LauncherApp: if not confirm: return + # 只清理由启动器常用命令创建的 ROS/监控进程，并保护当前 UI 进程。 patterns = [ "ros2 launch xr_rm_bringup", "ros2 run xr_rm_input", diff --git a/xr_rm_bringup/tools/realman_dual_arm_state_monitor.py b/xr_rm_bringup/tools/realman_dual_arm_state_monitor.py new file mode 100755 index 0000000..3f0cda7 --- /dev/null +++ b/xr_rm_bringup/tools/realman_dual_arm_state_monitor.py @@ -0,0 +1,215 @@ +#!/usr/bin/env python3 +"""独立双臂状态监控脚本（轮询版）。 + +优化点： +- 不再依赖 UDP 回调； +- 直接通过 rm_get_current_arm_state() 按句柄轮询左右臂状态； +- 避免同进程双回调潜在冲突导致的左臂显示异常。 +""" + +from __future__ import annotations + +import argparse +import time +from dataclasses import dataclass +from typing import Optional + +import numpy as np + +from realman_pkg.Robotic_Arm.rm_robot_interface import RoboticArm, rm_thread_mode_e +from realman_pkg.core.arg_cfg import host_ip, ip_l, ip_r, port_l, port_r + + +@dataclass +class ArmSnapshot: + """单臂状态快照。""" + + connected: bool + ret_code: int + last_update: float + joint_deg: list[float] + pose_euler: np.ndarray + err_text: str + + +class ArmStatePoller: + """单臂状态轮询器。""" + + def __init__( + self, + name: str, + robot_ip: str, + robot_port: int, + level: int = 3, + mode: int = 2, + ): + self.name = name + self.robot_ip = robot_ip + self.robot_port = int(robot_port) + self.level = int(level) + self.mode = int(mode) + + self.robot: Optional[RoboticArm] = None + self.robot_handle = None + self.connected = False + + self.last_snapshot = ArmSnapshot( + connected=False, + ret_code=-999, + last_update=0.0, + joint_deg=[0.0] * 7, + pose_euler=np.zeros(6, dtype=np.float64), + err_text="not connected", + ) + + self._connect() + + def _connect(self): + """连接机械臂。""" + try: + self.robot = RoboticArm(rm_thread_mode_e(self.mode)) + self.robot_handle = self.robot.rm_create_robot_arm(self.robot_ip, self.robot_port, self.level) + if self.robot_handle.id == -1: + self.connected = False + self.last_snapshot.connected = False + self.last_snapshot.err_text = f"connect failed ({self.robot_ip}:{self.robot_port})" + print(f"[{self.name}] 连接失败: {self.last_snapshot.err_text}") + return + + self.connected = True + self.last_snapshot.connected = True + self.last_snapshot.err_text = "ok" + print(f"[{self.name}] 已连接, handle_id={self.robot_handle.id}, ip={self.robot_ip}, port={self.robot_port}") + except Exception as exc: + self.connected = False + self.last_snapshot.connected = False + self.last_snapshot.err_text = f"connect exception: {exc}" + print(f"[{self.name}] 连接异常: {exc}") + + def poll(self) -> ArmSnapshot: + """轮询一次当前状态。""" + if (not self.connected) or (self.robot is None): + return self.last_snapshot + + try: + ret, state = self.robot.rm_get_current_arm_state() + now_t = time.time() + + if ret != 0: + self.last_snapshot.ret_code = ret + self.last_snapshot.err_text = f"rm_get_current_arm_state ret={ret}" + return self.last_snapshot + + joint = list(state.get("joint", [0.0] * 7)) + pose = list(state.get("pose", [0.0] * 6)) + err = state.get("err", {}) + + if len(joint) < 7: + joint.extend([0.0] * (7 - len(joint))) + elif len(joint) > 7: + joint = joint[:7] + + if len(pose) < 6: + pose.extend([0.0] * (6 - len(pose))) + elif len(pose) > 6: + pose = pose[:6] + + self.last_snapshot = ArmSnapshot( + connected=True, + ret_code=ret, + last_update=now_t, + joint_deg=joint, + pose_euler=np.array(pose, dtype=np.float64), + err_text=str(err), + ) + return self.last_snapshot + except Exception as exc: + self.last_snapshot.err_text = f"poll exception: {exc}" + return self.last_snapshot + + def close(self): + """断开连接。""" + if self.robot is None: + return + try: + self.robot.rm_delete_robot_arm() + except Exception: + pass + + +def format_arm_block(name: str, snap: ArmSnapshot, now_t: float) -> str: + """格式化单臂输出。""" + if not snap.connected: + return f"[{name}] 未连接: {snap.err_text}" + + age = now_t - snap.last_update if snap.last_update > 0 else float("inf") + stale_flag = "(stale)" if age > 2.0 else "" + + j = ", ".join(f"{v:7.2f}" for v in snap.joint_deg) + p = snap.pose_euler + return ( + f"[{name}] update_age={age:5.2f}s {stale_flag}, ret={snap.ret_code}\n" + f" Joint(deg): [{j}]\n" + f" Pose[x y z rx ry rz]: [{p[0]: .4f}, {p[1]: .4f}, {p[2]: .4f}, {p[3]: .4f}, {p[4]: .4f}, {p[5]: .4f}]\n" + f" Err: {snap.err_text}" + ) + + +def build_arg_parser() -> argparse.ArgumentParser: + parser = argparse.ArgumentParser(description="Realman 双臂状态监控脚本（轮询版）") + parser.add_argument("--left-ip", default=ip_l, help="左臂 IP") + parser.add_argument("--right-ip", default=ip_r, help="右臂 IP") + parser.add_argument("--left-port", type=int, default=port_l, help="左臂 TCP 端口") + parser.add_argument("--right-port", type=int, default=port_r, help="右臂 TCP 端口") + parser.add_argument("--refresh", type=float, default=0.5, help="刷新周期（秒）") + parser.add_argument("--level", type=int, default=3, help="连接级别") + parser.add_argument("--mode", type=int, default=2, help="线程模式 0/1/2") + parser.add_argument("--host-ip", default=host_ip, help="仅展示当前配置用，轮询版不使用") + return parser + + +def main(): + args = build_arg_parser().parse_args() + + print(f"配置: left={args.left_ip}:{args.left_port}, right={args.right_ip}:{args.right_port}, host_ip={args.host_ip}") + + left = ArmStatePoller( + name="LEFT", + robot_ip=args.left_ip, + robot_port=args.left_port, + level=args.level, + mode=args.mode, + ) + right = ArmStatePoller( + name="RIGHT", + robot_ip=args.right_ip, + robot_port=args.right_port, + level=args.level, + mode=args.mode, + ) + + print("\n开始监控双臂状态（轮询），按 Ctrl+C 退出。\n") + + try: + while True: + now_t = time.time() + l_snap = left.poll() + r_snap = right.poll() + + print("\033[2J\033[H", end="") + print(time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(now_t))) + print("=" * 88) + print(format_arm_block("LEFT", l_snap, now_t)) + print("-" * 88) + print(format_arm_block("RIGHT", r_snap, now_t)) + print("=" * 88) + time.sleep(max(args.refresh, 0.05)) + except KeyboardInterrupt: + print("\n退出监控...") + finally: + left.close() + right.close() + + +if __name__ == "__main__": + main() diff --git a/xr_rm_input/launch/udp_receiver.launch.py b/xr_rm_input/launch/udp_receiver.launch.py index b1a85b5..48ec659 100755 --- a/xr_rm_input/launch/udp_receiver.launch.py +++ b/xr_rm_input/launch/udp_receiver.launch.py @@ -1,3 +1,10 @@ +"""XR 手柄 UDP 接收节点的独立启动入口。 + +该 launch 文件只启动 `udp_controller_receiver`，用于低层通信调试：确认 PICO +或 sample_udp_sender 发出的 UDP JSON 能被 ROS2 接收，并发布到指定手柄话题。 +完整单臂/双臂遥操作请优先使用 xr_rm_bringup 中的 launch 文件。 +""" + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration @@ -6,9 +13,12 @@ from launch_ros.actions import Node def generate_launch_description() -> LaunchDescription: return LaunchDescription([ + # UDP 监听地址和端口，需要与发送端配置一致。 DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), DeclareLaunchArgument("udp_port", default_value="15000"), + # 兼容旧版单手柄调试入口；双臂调试会使用 left_topic/right_topic。 DeclareLaunchArgument("topic", default_value="/xr/right_controller"), + # 仅启动 UDP 接收节点，不启动任何机械臂控制节点。 Node( package="xr_rm_input", executable="udp_controller_receiver", diff --git a/xr_rm_input/setup.py b/xr_rm_input/setup.py index 13b0df3..f567f39 100755 --- a/xr_rm_input/setup.py +++ b/xr_rm_input/setup.py @@ -1,3 +1,9 @@ +"""xr_rm_input 包安装配置。 + +该包负责把 XR/PICO 端发来的 UDP 手柄数据接入 ROS2，并提供一个 +sample_udp_sender 方便在没有真实手柄时做链路调试。 +""" + from glob import glob from setuptools import setup diff --git a/xr_rm_input/xr_rm_input/__init__.py b/xr_rm_input/xr_rm_input/__init__.py index 8b13789..312ef38 100755 --- a/xr_rm_input/xr_rm_input/__init__.py +++ b/xr_rm_input/xr_rm_input/__init__.py @@ -1 +1,4 @@ +"""XR 输入包。 +包含 UDP 手柄数据接收节点和用于现场调试的模拟 UDP 发送脚本。 +""" diff --git a/xr_rm_input/xr_rm_input/sample_udp_sender.py b/xr_rm_input/xr_rm_input/sample_udp_sender.py index 8419079..1819f7a 100755 --- a/xr_rm_input/xr_rm_input/sample_udp_sender.py +++ b/xr_rm_input/xr_rm_input/sample_udp_sender.py @@ -1,3 +1,9 @@ +"""XR 手柄 UDP 数据模拟发送器。 + +用于在没有真实 PICO/XR 手柄时向 udp_controller_receiver 发送测试数据。 +默认轨迹按 +X/-X、+Y/-Y、+Z/-Z 成对扫轴，便于用肉眼检查机械臂方向映射。 +""" + import argparse import json import math @@ -5,14 +11,188 @@ import socket import time +BASE_POS = [0.0, 1.0, 0.0] +# 方向名称会打印到终端，方便现场观察机械臂运动时对照坐标轴。 +AXIS_DIRECTIONS = ( + ("XR +X", (1.0, 0.0, 0.0)), + ("XR -X", (-1.0, 0.0, 0.0)), + ("XR +Y", (0.0, 1.0, 0.0)), + ("XR -Y", (0.0, -1.0, 0.0)), + ("XR +Z", (0.0, 0.0, 1.0)), + ("XR -Z", (0.0, 0.0, -1.0)), +) +AXIS_SWEEP_SEGMENTS = ( + ("XR +X right", (1.0, 0.0, 0.0), "hold"), + ("XR -X left", (-1.0, 0.0, 0.0), "hold"), + ("CENTER / X done", (0.0, 0.0, 0.0), "center"), + ("XR +Y up", (0.0, 1.0, 0.0), "hold"), + ("XR -Y down", (0.0, -1.0, 0.0), "hold"), + ("CENTER / Y done", (0.0, 0.0, 0.0), "center"), + ("XR +Z back", (0.0, 0.0, 1.0), "hold"), + ("XR -Z front", (0.0, 0.0, -1.0), "hold"), + ("CENTER / Z done", (0.0, 0.0, 0.0), "center"), +) + + +def _validate_positive(name: str, value: float) -> float: + if value <= 0.0: + raise argparse.ArgumentTypeError(f"{name} must be > 0") + return value + + +def _positive_float(name: str): + def parser(value: str) -> float: + return _validate_positive(name, float(value)) + + return parser + + +def _position_from_direction(direction: tuple[float, float, float], amplitude: float) -> list[float]: + return [ + BASE_POS[0] + amplitude * direction[0], + BASE_POS[1] + amplitude * direction[1], + BASE_POS[2] + amplitude * direction[2], + ] + + +def _pattern_period(pattern: str, hold_seconds: float, center_seconds: float) -> float: + if pattern == "axis_sweep": + return sum( + hold_seconds if segment_type == "hold" else center_seconds + for _, _, segment_type in AXIS_SWEEP_SEGMENTS + ) + if pattern == "axis_steps": + return (hold_seconds + center_seconds) * len(AXIS_DIRECTIONS) + return 5.0 + + +def _axis_sweep_position( + t: float, + amplitude: float, + hold_seconds: float, + center_seconds: float, + initial_center_seconds: float, +) -> tuple[str, list[float]]: + if t < initial_center_seconds: + return "CENTER / lock origin", BASE_POS.copy() + + # +方向和-方向连续切换，比每次都很快回中心更容易看清实际运动方向。 + cycle_t = (t - initial_center_seconds) % _pattern_period( + "axis_sweep", + hold_seconds, + center_seconds, + ) + for label, direction, segment_type in AXIS_SWEEP_SEGMENTS: + duration = hold_seconds if segment_type == "hold" else center_seconds + if cycle_t < duration: + return label, _position_from_direction(direction, amplitude) + cycle_t -= duration + return "CENTER", BASE_POS.copy() + + +def _axis_step_position( + t: float, + amplitude: float, + hold_seconds: float, + center_seconds: float, + initial_center_seconds: float, +) -> tuple[str, list[float]]: + if t < initial_center_seconds: + return "CENTER / lock origin", BASE_POS.copy() + + # 每个方向由“保持目标点”和“回中心”两段组成，循环遍历六个方向。 + segment_seconds = hold_seconds + center_seconds + cycle_t = (t - initial_center_seconds) % (segment_seconds * len(AXIS_DIRECTIONS)) + direction_index = int(cycle_t // segment_seconds) + segment_t = cycle_t - direction_index * segment_seconds + label, direction = AXIS_DIRECTIONS[direction_index] + + if segment_t >= hold_seconds: + return f"CENTER after {label}", BASE_POS.copy() + + return label, _position_from_direction(direction, amplitude) + + +def _sine_position(t: float, amplitude: float, phase: float) -> tuple[str, list[float]]: + return "SINE X", [ + amplitude * math.sin(2.0 * math.pi * 0.2 * t + phase), + BASE_POS[1], + BASE_POS[2], + ] + + +def _pattern_position( + pattern: str, + t: float, + amplitude: float, + hold_seconds: float, + center_seconds: float, + initial_center_seconds: float, + phase: float, +) -> tuple[str, list[float]]: + if pattern == "sine": + return _sine_position(t, amplitude, phase) + if pattern == "axis_steps": + return _axis_step_position( + t, + amplitude, + hold_seconds, + center_seconds, + initial_center_seconds, + ) + return _axis_sweep_position( + t, + amplitude, + hold_seconds, + center_seconds, + initial_center_seconds, + ) + + def main() -> None: parser = argparse.ArgumentParser(description="通过 UDP 发送一段模拟 XR 左/右手柄数据。") parser.add_argument("--host", default="127.0.0.1") parser.add_argument("--port", type=int, default=15000) - parser.add_argument("--seconds", type=float, default=5.0) - parser.add_argument("--hz", type=float, default=60.0) - parser.add_argument("--amplitude", type=float, default=0.04) + parser.add_argument("--seconds", type=_positive_float("seconds"), default=30.0) + parser.add_argument("--hz", type=_positive_float("hz"), default=60.0) + parser.add_argument("--amplitude", type=_positive_float("amplitude"), default=0.50) + parser.add_argument( + "--pattern", + choices=("axis_sweep", "axis_steps", "sine"), + default="axis_sweep", + help="axis_sweep 成对扫轴最适合肉眼看方向；axis_steps 每个方向后回中心；sine 保留旧轨迹。", + ) + parser.add_argument( + "--hold-seconds", + type=_positive_float("hold-seconds"), + default=4.0, + help="axis_sweep/axis_steps 模式下，每个方向保持多久。增大可让机械臂走得更明显。", + ) + parser.add_argument( + "--center-seconds", + type=_positive_float("center-seconds"), + default=1.2, + help="axis_sweep/axis_steps 模式下，回中心段保持多久。", + ) + parser.add_argument( + "--initial-center-seconds", + type=_positive_float("initial-center-seconds"), + default=1.0, + help="axis_sweep/axis_steps 模式下，开头保持中心点多久，用于让遥操节点锁定原点。", + ) + parser.add_argument( + "--trigger", + type=float, + default=0.0, + help="发送的 trigger 值。方向调试默认不驱动夹爪。", + ) parser.add_argument("--hand", choices=("left", "right", "both"), default="right") + parser.add_argument( + "--both-mode", + choices=("synchronized", "staggered"), + default="synchronized", + help="hand=both 时 synchronized 左右臂同时动；staggered 先左后右，便于逐只观察。", + ) args = parser.parse_args() sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) @@ -20,37 +200,86 @@ def main() -> None: t0 = time.monotonic() hands = ("left", "right") if args.hand == "both" else (args.hand,) + # 每只手柄独立发送同格式数据包，接收端根据 hand 字段分发左右话题。 packets = { hand: { "t": 0.0, "hand": hand, "grip": False, "trigger": 0.0, - "pos": [0.0, 1.0, 0.0], + "pos": BASE_POS.copy(), "quat": [0.0, 0.0, 0.0, 1.0], } for hand in hands } + pattern_period = _pattern_period(args.pattern, args.hold_seconds, args.center_seconds) + one_hand_seconds = args.initial_center_seconds + pattern_period + full_sweep_seconds = one_hand_seconds * len(hands) if args.both_mode == "staggered" else one_hand_seconds + print( + "Sample UDP sender: " + f"hand={args.hand}, both_mode={args.both_mode}, pattern={args.pattern}, " + f"amplitude={args.amplitude:.3f}m, hold={args.hold_seconds:.2f}s, " + f"center={args.center_seconds:.2f}s, seconds={args.seconds:.1f}s", + flush=True, + ) + if args.pattern != "sine" and args.seconds < full_sweep_seconds: + print( + "提示：当前 seconds 不足以跑完一轮完整方向序列；" + f"建议至少 {full_sweep_seconds:.1f}s。", + flush=True, + ) + last_labels = {hand: None for hand in hands} while time.monotonic() - t0 < args.seconds: t = time.monotonic() - t0 - # 模拟按住握持键后缓慢移动手柄，用来测试相对位移控制链路。 + # 默认用清晰的成对扫轴测试相对位移链路，便于肉眼确认坐标映射。 for index, packet in enumerate(packets.values()): - phase = index * math.pi + hand = packet["hand"] + pattern_t = t + if args.hand == "both" and args.both_mode == "staggered": + hand_start_t = index * one_hand_seconds + hand_end_t = hand_start_t + one_hand_seconds + if t < hand_start_t: + label, pos = "WAIT / center", BASE_POS.copy() + elif t >= hand_end_t: + label, pos = "DONE / center", BASE_POS.copy() + else: + pattern_t = t - hand_start_t + label, pos = _pattern_position( + args.pattern, + pattern_t, + args.amplitude, + args.hold_seconds, + args.center_seconds, + args.initial_center_seconds, + index * math.pi, + ) + else: + label, pos = _pattern_position( + args.pattern, + pattern_t, + args.amplitude, + args.hold_seconds, + args.center_seconds, + args.initial_center_seconds, + index * math.pi, + ) + + if label != last_labels[hand]: + print(f"[{t:5.2f}s] {hand:<5} {label:<22} pos={pos}", flush=True) + last_labels[hand] = label + packet["t"] = t packet["grip"] = True - packet["trigger"] = max(0.0, math.sin(2.0 * math.pi * 0.1 * t)) - packet["pos"] = [ - args.amplitude * math.sin(2.0 * math.pi * 0.2 * t + phase), - 1.0, - 0.0, - ] + packet["trigger"] = min(max(args.trigger, 0.0), 1.0) + packet["pos"] = pos sock.sendto(json.dumps(packet).encode("utf-8"), (args.host, args.port)) time.sleep(dt) for packet in packets.values(): packet["grip"] = False packet["trigger"] = 0.0 + packet["pos"] = BASE_POS.copy() sock.sendto(json.dumps(packet).encode("utf-8"), (args.host, args.port)) diff --git a/xr_rm_input/xr_rm_input/udp_controller_receiver.py b/xr_rm_input/xr_rm_input/udp_controller_receiver.py index df677c0..6b6a69e 100755 --- a/xr_rm_input/xr_rm_input/udp_controller_receiver.py +++ b/xr_rm_input/xr_rm_input/udp_controller_receiver.py @@ -1,3 +1,9 @@ +"""XR 手柄 UDP 接收节点。 + +从 UDP JSON 数据包中解析左右手柄位姿、握持键和扳机值，并发布为 +`xr_rm_interfaces/XrController` 消息，供遥操作和夹爪节点订阅。 +""" + import json import socket from collections.abc import Iterable, Mapping @@ -90,6 +96,7 @@ class UdpControllerReceiver(Node): controllers = payload.get("controllers", payload.get("controller")) if isinstance(controllers, Mapping): + # 兼容两类格式：controllers 本身就是单个手柄，或按 left/right 分组。 if self._contains_pose(controllers): hand = self._normalize_hand(controllers.get("hand", self._default_hand)) yield hand, controllers @@ -143,6 +150,7 @@ class UdpControllerReceiver(Node): def _extract_pose(self, payload: Mapping[str, Any]) -> tuple[list[float], list[float]]: pose = payload.get("pose") + # 支持多种常见字段命名，方便 Unity/OpenXR/Python 调试脚本接入同一节点。 if isinstance(pose, Mapping): pos = pose.get("position", pose.get("pos", pose.get("p"))) quat = pose.get("orientation", pose.get("quat", pose.get("q", [0.0, 0.0, 0.0, 1.0]))) diff --git a/xr_rm_teleop/setup.py b/xr_rm_teleop/setup.py index 0e61bc5..3ebf1c3 100755 --- a/xr_rm_teleop/setup.py +++ b/xr_rm_teleop/setup.py @@ -1,3 +1,9 @@ +"""xr_rm_teleop 包安装配置。 + +该包提供基于 XR 相对位移的 RM75 笛卡尔速度遥操作节点，以及把 +手柄 trigger 映射到夹爪力控命令的桥接节点。 +""" + from setuptools import setup package_name = "xr_rm_teleop" diff --git a/xr_rm_teleop/xr_rm_teleop/__init__.py b/xr_rm_teleop/xr_rm_teleop/__init__.py index 8b13789..f8b6293 100755 --- a/xr_rm_teleop/xr_rm_teleop/__init__.py +++ b/xr_rm_teleop/xr_rm_teleop/__init__.py @@ -1 +1,4 @@ +"""XR 到 RM75 遥操作节点包。 +核心模块包括单臂速度遥操作、RealMan SDK 适配层和夹爪 trigger 桥接。 +""" diff --git a/xr_rm_teleop/xr_rm_teleop/gripper_trigger_bridge.py b/xr_rm_teleop/xr_rm_teleop/gripper_trigger_bridge.py index 46d6e5b..414a1a6 100755 --- a/xr_rm_teleop/xr_rm_teleop/gripper_trigger_bridge.py +++ b/xr_rm_teleop/xr_rm_teleop/gripper_trigger_bridge.py @@ -1,3 +1,9 @@ +"""手柄 trigger 到夹爪力控指令的桥接节点。 + +订阅 XR 手柄消息，将 trigger 压力映射为 OmniPicker 可使用的归一化 +闭合力比例，并在消息超时或未握持时自动回到开爪指令。 +""" + from __future__ import annotations import rclpy @@ -62,6 +68,7 @@ class GripperTriggerBridge(Node): def _publish_command(self) -> None: command = self._open_command + # 手柄消息超时后立即退回开爪，避免夹爪在通信中断时保持闭合力。 if self._last_msg is not None and self._last_msg_time is not None: age = (self.get_clock().now() - self._last_msg_time).nanoseconds * 1e-9 if age <= self._stale_timeout_sec: diff --git a/xr_rm_teleop/xr_rm_teleop/realman_adapter.py b/xr_rm_teleop/xr_rm_teleop/realman_adapter.py index c37103d..250fcec 100755 --- a/xr_rm_teleop/xr_rm_teleop/realman_adapter.py +++ b/xr_rm_teleop/xr_rm_teleop/realman_adapter.py @@ -1,5 +1,12 @@ +"""RM75 机械臂适配层。 + +对上提供统一的当前位姿读取、笛卡尔速度发送和停止接口；对下根据配置 +选择 mock 积分模拟器或睿尔曼 Python API2 真机通信。 +""" + from __future__ import annotations +import time from dataclasses import dataclass from numbers import Number from typing import Any @@ -72,6 +79,9 @@ class RealManAdapter: initial_joint_pose: list[float] | None = None, initial_tcp_pose: list[float] | None = None, init_move_speed: int = 20, + command_mode: str = "velocity", + canfd_trajectory_mode: int = 2, + canfd_radio: int = 0, ) -> None: self._robot_ip = robot_ip self._robot_port = robot_port @@ -90,7 +100,12 @@ class RealManAdapter: self._initial_joint_pose = initial_joint_pose self._initial_tcp_pose = initial_tcp_pose self._init_move_speed = init_move_speed + self._command_mode = command_mode + self._canfd_trajectory_mode = canfd_trajectory_mode + self._canfd_radio = canfd_radio self._arm: Any | None = None + if self._command_mode not in ("velocity", "pose_canfd"): + raise ValueError("command_mode must be one of: velocity, pose_canfd") def connect(self) -> None: try: @@ -103,17 +118,21 @@ class RealManAdapter: self._arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E) handle = self._arm.rm_create_robot_arm(self._robot_ip, self._robot_port) self._check_robot_handle(handle) + self._log_info( + "RealMan connected: " + f"ip={self._robot_ip}, port={self._robot_port}, " + f"avoid_singularity={self._avoid_singularity}, " + f"frame_type={self._frame_type}, dt_ms={self._dt_ms}, " + f"command_mode={self._command_mode}" + ) if self._configure_safety_limits: self._apply_safety_limits() if self._move_to_initial_pose_on_connect: self._move_to_initial_pose() - # 速度透传初始化必须和控制循环周期一致，避免真实机械臂出现周期不稳定。 - ret = self._arm.rm_set_movev_canfd_init( - self._avoid_singularity, - self._frame_type, - self._dt_ms, - ) - self._check_return(ret, "rm_set_movev_canfd_init") + if self._command_mode == "velocity": + self._init_movev_canfd() + else: + self._log_info("command_mode=pose_canfd，跳过 rm_set_movev_canfd_init。") def get_current_pose(self) -> ArmPose: self._require_arm() @@ -125,14 +144,32 @@ class RealManAdapter: def send_cartesian_velocity(self, velocity: list[float], follow: bool) -> None: self._require_arm() + if self._command_mode == "pose_canfd": + return ret = self._arm.rm_movev_canfd(velocity, follow, 0, 0) self._check_return(ret, "rm_movev_canfd") + def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None: + self._require_arm() + if self._command_mode != "pose_canfd": + raise RuntimeError("send_cartesian_target requires command_mode=pose_canfd") + ret = self._arm.rm_movep_canfd( + [pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz], + follow, + self._canfd_trajectory_mode, + self._canfd_radio, + ) + self._check_return(ret, "rm_movep_canfd") + + def uses_pose_targets(self) -> bool: + return self._command_mode == "pose_canfd" + def stop(self) -> None: if self._arm is None: return try: - self._arm.rm_movev_canfd([0.0] * 6, False, 0, 0) + if self._command_mode == "velocity": + self._arm.rm_movev_canfd([0.0] * 6, False, 0, 0) self._arm.rm_set_arm_slow_stop() except Exception: pass @@ -151,7 +188,8 @@ class RealManAdapter: raise RuntimeError("睿尔曼机械臂尚未连接") def _apply_safety_limits(self) -> None: - self._try_call("rm_set_avoid_singularity_mode", True) + # 真机安全限幅尽量下发到控制器；不支持的 SDK 接口会在 _try_call 中降级为警告。 + self._try_call("rm_set_avoid_singularity_mode", int(self._avoid_singularity)) self._try_call("rm_set_arm_max_line_speed", self._max_line_speed) self._try_call("rm_set_arm_max_angular_speed", self._max_angular_speed) self._try_call("rm_set_arm_max_line_acc", self._max_line_acc) @@ -169,6 +207,46 @@ class RealManAdapter: ret = self._arm.rm_movel(self._initial_tcp_pose, self._init_move_speed, 0, 0, 1) self._check_return(ret, "rm_movel(initial_tcp_pose)") + def _init_movev_canfd(self) -> None: + attempts = 3 + candidates = [self._avoid_singularity] + if self._avoid_singularity != 0: + candidates.append(0) + + # 某些现场控制器初始化避奇异模式会超时，失败后自动降级到 0 再重试。 + last_ret: Any = None + for avoid_singularity in candidates: + if avoid_singularity != self._avoid_singularity: + self._log_warn("rm_set_movev_canfd_init 降级为 avoid_singularity=0 后重试。") + + for attempt in range(1, attempts + 1): + ret = self._arm.rm_set_movev_canfd_init( + avoid_singularity, + self._frame_type, + self._dt_ms, + ) + last_ret = ret + code = self._return_code(ret) + if code == 0: + self._avoid_singularity = avoid_singularity + self._log_info( + "rm_set_movev_canfd_init 成功：" + f"avoid_singularity={avoid_singularity}, " + f"frame_type={self._frame_type}, dt_ms={self._dt_ms}" + ) + return + + self._log_warn( + "rm_set_movev_canfd_init 失败：" + f"avoid_singularity={avoid_singularity}, " + f"frame_type={self._frame_type}, dt_ms={self._dt_ms}, " + f"attempt={attempt}/{attempts}, ret={ret!r}" + ) + if attempt < attempts: + time.sleep(0.2) + + self._check_return(last_ret, "rm_set_movev_canfd_init") + def _try_call(self, name: str, *args: Any) -> None: func = getattr(self._arm, name, None) if func is None: @@ -185,18 +263,28 @@ class RealManAdapter: if self._logger is not None: self._logger.warn(message) - @staticmethod - def _check_robot_handle(handle: Any) -> None: + def _log_info(self, message: str) -> None: + if self._logger is not None: + self._logger.info(message) + + def _check_robot_handle(self, handle: Any) -> None: handle_id = getattr(handle, "id", None) if handle_id == -1: - raise RuntimeError("rm_create_robot_arm failed: socket error or robot unreachable") + raise RuntimeError( + "rm_create_robot_arm failed: TCP may be reachable, but the RealMan " + f"controller did not return robot info from {self._robot_ip}:{self._robot_port}" + ) @staticmethod def _check_return(ret: Any, name: str) -> None: - code = ret[0] if isinstance(ret, tuple) and ret else ret + code = RealManAdapter._return_code(ret) if isinstance(code, int) and code != 0: raise RuntimeError(f"{name} failed with code {code}: {ret!r}") + @staticmethod + def _return_code(ret: Any) -> Any: + return ret[0] if isinstance(ret, tuple) and ret else ret + @classmethod def _find_pose(cls, obj: Any) -> list[float] | None: # 不同 SDK 版本返回字段可能略有差异，因此递归查找常见 TCP 位姿字段。 diff --git a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py index 87dce51..f37879a 100755 --- a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py +++ b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py @@ -1,3 +1,9 @@ +"""RM75 单臂 XR 相对位移遥操作节点。 + +节点订阅左/右手柄位姿，在 grip 按下时锁定手柄和 TCP 起点，把手柄相对位移 +映射到机器人坐标系中的目标 TCP，并通过笛卡尔速度命令跟随该目标。 +""" + from __future__ import annotations import math @@ -60,6 +66,9 @@ class SingleArmVelocityTeleop(Node): self.declare_parameter("initial_joint_pose", [0.0] * 7) self.declare_parameter("initial_tcp_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0]) self.declare_parameter("init_move_speed", 20) + self.declare_parameter("command_mode", "velocity") + self.declare_parameter("canfd_trajectory_mode", 2) + self.declare_parameter("canfd_radio", 0) self.declare_parameter("debug_topic_prefix", "/xr_rm") self._arm_name = str(self.get_parameter("arm_name").value) @@ -107,6 +116,7 @@ class SingleArmVelocityTeleop(Node): self.get_logger().info(f"{self._arm_name} 速度遥操节点已启动，监听话题：{topic}") def _make_adapter(self): + # mock 和真机共享同一控制逻辑，只在适配层切换运动命令的执行方式。 if self._bool_parameter("use_mock"): return MockRealManAdapter( [float(v) for v in self.get_parameter("mock_initial_pose").value], @@ -131,6 +141,9 @@ class SingleArmVelocityTeleop(Node): initial_joint_pose=self._float_list_parameter("initial_joint_pose", 7), initial_tcp_pose=self._float_list_parameter("initial_tcp_pose", 6), init_move_speed=int(self.get_parameter("init_move_speed").value), + command_mode=str(self.get_parameter("command_mode").value), + canfd_trajectory_mode=int(self.get_parameter("canfd_trajectory_mode").value), + canfd_radio=int(self.get_parameter("canfd_radio").value), ) def _on_controller(self, msg: XrController) -> None: @@ -206,7 +219,24 @@ class SingleArmVelocityTeleop(Node): ] cartesian_velocity = self._filtered_velocity + [0.0, 0.0, 0.0] self._publish_debug(robot_pose, target, cartesian_velocity) - self._send_cartesian_velocity(cartesian_velocity) + if self._adapter_uses_pose_targets(): + step_target = [ + robot_now[i] + cartesian_velocity[i] * self._dt + for i in range(3) + ] + step_target = self._clamp_workspace(step_target) + self._send_cartesian_target( + ArmPose( + x=step_target[0], + y=step_target[1], + z=step_target[2], + rx=robot_pose.rx, + ry=robot_pose.ry, + rz=robot_pose.rz, + ) + ) + else: + self._send_cartesian_velocity(cartesian_velocity) @staticmethod def _controller_xyz(msg: XrController) -> list[float]: @@ -221,6 +251,7 @@ class SingleArmVelocityTeleop(Node): ] def _clamp_workspace(self, target: list[float]) -> list[float]: + # 先做盒状工作空间限幅，再叠加底座圆柱半径限制。 clamped = [ _clamp(target[i], self._workspace_min[i], self._workspace_max[i]) for i in range(3) @@ -259,6 +290,8 @@ class SingleArmVelocityTeleop(Node): def _safe_stop(self) -> None: # 所有断连、松手、退出路径都走这里，确保发送零速度。 self._filtered_velocity = [0.0, 0.0, 0.0] + if self._adapter_uses_pose_targets(): + return self._send_cartesian_velocity([0.0] * 6) def _send_cartesian_velocity(self, velocity: list[float]) -> None: @@ -271,6 +304,20 @@ class SingleArmVelocityTeleop(Node): ) self._active = False + def _send_cartesian_target(self, pose: ArmPose) -> None: + try: + self._adapter.send_cartesian_target(pose, self._follow) + except Exception as exc: + self.get_logger().error( + f"{self._arm_name} 发送位姿透传命令失败：{exc}", + throttle_duration_sec=1.0, + ) + self._active = False + + def _adapter_uses_pose_targets(self) -> bool: + uses_pose_targets = getattr(self._adapter, "uses_pose_targets", None) + return bool(uses_pose_targets is not None and uses_pose_targets()) + def _publish_debug(self, current_pose: ArmPose, target_xyz: list[float], velocity: list[float]) -> None: stamp = self.get_clock().now().to_msg() current_msg = self._pose_msg(stamp, current_pose)