diff --git a/README.md b/README.md index b30af26..5f2e5df 100755 --- a/README.md +++ b/README.md @@ -6,7 +6,6 @@ PICO 4 Ultra 双手柄 UDP 位姿/按键 -> /xr/left_controller 与 /xr/right_controller -> 左右 RM75 相对位移速度遥操 - -> 右手 trigger 输出 OmniPicker 归一化力控指令 -> PC 端查看 D405/D405/D435 视频，后续再接记录节点 ``` @@ -14,8 +13,8 @@ PICO 4 Ultra 双手柄 UDP 位姿/按键 ## 当前范围 -- 已纳入：PICO 双手柄输入、左右 RM75 相对位移控制、右手 OmniPicker trigger 指令出口。 -- 暂不纳入：AUBO 移动底盘、升降柱、剪刀手、XR 视频流、自动采摘算法。 +- 已纳入：PICO 双手柄输入、左右 RM75 相对位移控制、左右臂独立/双臂调试 launch。 +- 暂不纳入：末端执行器、AUBO 移动底盘、升降柱、XR 视频流、自动采摘算法。 ## 关键配置来源 @@ -24,8 +23,10 @@ PICO 4 Ultra 双手柄 UDP 位姿/按键 - 左臂 IP：`192.168.192.18` - 右臂 IP：`192.168.192.19` - 左右臂 TCP 端口：`8080` -- 左右臂工作空间：`x[-0.50, 0.50] y[-0.60, -0.20] z[0.10, 0.50]` -- 左右臂工作空间和 IP 沿用最新 acDual-arm 配置。 +- 左臂工作空间：`x[-0.50, 0.50] y[-0.60, -0.20] z[0.10, 0.50]` +- 右臂工作空间：`x[-0.70, 0.50] y[-0.60, 0.40] z[0.10, 0.70]` +- 圆柱半径限制：左右臂均为 `r[0.20, 0.60]`，低高度区域最小半径收紧到 `0.21` +- 左右臂初始化关节/末端点位、工作空间和 IP 沿用最新 acRealman 配置。 `dual_arm_rm75.yaml` 里的左右 `xr_to_robot_matrix` 依据论文中的 OpenXR/PICO 手柄坐标和现场双臂 base 坐标图设置： @@ -47,9 +48,25 @@ colcon build --symlink-install source install/setup.bash ``` +## UI 启动面板 + +项目内置了一个类似 acRealman 的 Tkinter 启动面板，包含仿真、左臂真机、右臂真机、双臂真机、sample UDP 发送、常用 topic 检查和清理按钮。 + +从工作空间根目录直接运行源码脚本： + +```bash +python3 src/xr_rm_bringup/tools/launcher_ui.py +``` + +构建并 `source install/setup.bash` 后，也可以通过 ROS2 入口运行： + +```bash +ros2 run xr_rm_bringup launcher_ui +``` + ## 优先验证：双臂 mock -mock 模式不会连接真实 RM75，适合先验证 UDP 输入、左右手柄路由、相对位移控制和 trigger 夹爪指令。 +mock 模式不会连接真实 RM75，适合先验证 UDP 输入、左右手柄路由和相对位移控制。 终端 1： @@ -72,7 +89,16 @@ ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000 ```bash ros2 topic echo /xr/left_controller ros2 topic echo /xr/right_controller -ros2 topic echo /omnipicker/right/force_ratio +ros2 topic echo /xr_rm/left_rm75/cmd_vel +ros2 topic echo /xr_rm/right_rm75/cmd_vel +``` + +也可以使用统一调试入口分别验证： + +```bash +ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true +ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true +ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true ``` ## 连接双 RM75 @@ -87,11 +113,30 @@ ros2 launch xr_rm_bringup dual_arm_realman.launch.py \ right_robot_ip:=192.168.192.19 ``` +更推荐用统一调试入口按阶段放开： + +```bash +# 左臂单独调试 +ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false + +# 右臂单独调试 +ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false + +# 双臂一起调试 +ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false +``` + +真机 launch 默认不会让机械臂自动移动到 acRealman 初始化点。若现场确认安全区已清空，可显式开启： + +```bash +ros2 launch xr_rm_bringup dual_arm_realman.launch.py move_to_initial_pose_on_connect:=true +``` + 单臂入口仍保留，默认按右臂配置： ```bash -ros2 launch xr_rm_bringup single_arm_mock.launch.py -ros2 launch xr_rm_bringup single_arm_realman.launch.py robot_ip:=192.168.192.19 +ros2 launch xr_rm_bringup single_arm_mock.launch.py arm:=right +ros2 launch xr_rm_bringup single_arm_realman.launch.py arm:=right robot_ip:=192.168.192.19 ``` ## UDP JSON 格式 @@ -131,16 +176,6 @@ ros2 launch xr_rm_bringup single_arm_realman.launch.py robot_ip:=192.168.192.19 `grip` 是机械臂运动使能：按下时锁定当前手柄和 TCP 起点，之后只跟随相对位移；松开、超时或节点退出都会发送零速度。 -## OmniPicker 接口 - -`gripper_trigger_bridge` 会把右手 `trigger` 映射为： - -```text -/omnipicker/right/force_ratio std_msgs/Float32 -``` - -默认范围是 `0.0` 开爪、`0.15-0.45` 柔性闭合/保持。实际接 OmniPicker 驱动前，请根据番茄压伤测试收紧 `dual_arm_rm75.yaml` 里的 `max_close_command`。 - ## 真机前方向验证 第一轮保持 `max_linear_speed <= 0.04`，先只上右臂： @@ -150,6 +185,12 @@ ros2 launch xr_rm_bringup single_arm_realman.launch.py robot_ip:=192.168.192.19 3. 手柄缓慢左移/右移，确认 TCP 方向。 4. 若任一方向相反，只改 `dual_arm_rm75.yaml` 中对应臂的 `xr_to_robot_matrix` 符号。 5. 右臂确认后再开放左臂。 -6. 最后接 OmniPicker，并从低力控上限开始抓取番茄。 当前双臂是两个独立控制节点，没有跨臂碰撞模型；首次测试请让两臂工作区物理分离。 + +## 从 acRealman 迁入的优化 + +- 右臂工作空间改为 acRealman 独立右臂边界，不再沿用左臂窄范围。 +- 速度遥操目标在笛卡尔边界外增加基座圆柱半径限制，减少低位近基座不可达目标。 +- 真机连接后会下发 acRealman 同款 RM75 硬件速度、加速度和避奇异配置。 +- 保留 acRealman 的左右臂 `movej`/`movel` 初始化点位，可通过 launch 参数显式启用。 diff --git a/xr_rm_bringup/CMakeLists.txt b/xr_rm_bringup/CMakeLists.txt index 6fa71e8..c8be864 100755 --- a/xr_rm_bringup/CMakeLists.txt +++ b/xr_rm_bringup/CMakeLists.txt @@ -9,4 +9,10 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME} ) +install(PROGRAMS + tools/launcher_ui.py + DESTINATION lib/${PROJECT_NAME} + RENAME launcher_ui +) + ament_package() diff --git a/xr_rm_bringup/config/dual_arm_rm75.yaml b/xr_rm_bringup/config/dual_arm_rm75.yaml index e699765..b9808ba 100755 --- a/xr_rm_bringup/config/dual_arm_rm75.yaml +++ b/xr_rm_bringup/config/dual_arm_rm75.yaml @@ -3,7 +3,7 @@ # 参数来源：acDual-arm-YikaiFuTest/realman_pkg/core/arg_cfg.py # - 左臂 IP：192.168.192.18 # - 右臂 IP：192.168.192.19 -# - 左右臂工作空间和 IP 沿用最新 acDual-arm 配置。 +# - 左右臂工作空间、圆柱半径约束、初始化点位和 IP 沿用最新 acRealman 配置。 # - PICO 手柄位移映射根据论文 OpenXR 坐标和现场双臂 base 坐标图单独设置。 # # 注意：当前项目仍然采用“手柄相对位移”控制方式。 @@ -30,6 +30,9 @@ left_arm_teleop: # 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] + cyl_radius_limit: [0.20, 0.60] + low_z_threshold: 0.20 + low_z_min_radius: 0.21 # PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。 # 左臂 base 坐标：+X 向下，+Z 向右，前方工作区对应 -Y。 @@ -39,12 +42,25 @@ left_arm_teleop: 1.0, 0.0, 0.0] use_mock: false - mock_initial_pose: [-0.1560, -0.3476, 0.2689, -3.1100, -0.2840, -1.3180] + 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 frame_type: 1 follow: false + configure_safety_limits: true + max_line_speed: 1.0 + max_angular_speed: 1.5 + max_line_acc: 1.0 + max_angular_acc: 2.0 + joint_max_speed: 180.0 + joint_max_acc: 180.0 + # 真机 launch 默认不自动移动；需要沿用 acRealman 上电初始化时再显式打开。 + move_to_initial_pose_on_connect: false + 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 + debug_topic_prefix: /xr_rm right_arm_teleop: ros__parameters: @@ -60,9 +76,13 @@ right_arm_teleop: max_linear_speed: 0.04 enable_position_axes: [true, true, true] - # 来自 acDual-arm 的 bounds_p[right]。 - workspace_min: [-0.50, -0.60, 0.10] - workspace_max: [0.50, -0.20, 0.50] + # 来自 acDual-arm 的 bounds_p[right]： + # x[-0.70, 0.50]，y[-0.60, 0.40]，z[0.10, 0.70]。 + workspace_min: [-0.70, -0.60, 0.10] + workspace_max: [0.50, 0.40, 0.70] + cyl_radius_limit: [0.20, 0.60] + low_z_threshold: 0.20 + low_z_min_radius: 0.21 # PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。 # 右臂 base 坐标：+X 向上，+Z 向左，前方工作区对应 -Y。 @@ -72,23 +92,21 @@ right_arm_teleop: -1.0, 0.0, 0.0] use_mock: false - mock_initial_pose: [0.1924, -0.3025, 0.2923, 3.0560, 0.1310, 0.9070] + mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910] robot_ip: 192.168.192.19 robot_port: 8080 avoid_singularity: 1 frame_type: 1 follow: false - -right_omnipicker_trigger_bridge: - ros__parameters: - # 阶段一只使用右手两指夹爪；右手扳机键输出归一化力控指令。 - controller_topic: /xr/right_controller - command_topic: /omnipicker/right/force_ratio - publish_rate_hz: 30.0 - stale_timeout_sec: 0.25 - require_grip: true - trigger_deadband: 0.05 - open_command: 0.0 - min_close_command: 0.15 - max_close_command: 0.45 - low_pass_alpha: 0.5 + configure_safety_limits: true + max_line_speed: 1.0 + max_angular_speed: 1.5 + max_line_acc: 1.0 + max_angular_acc: 2.0 + joint_max_speed: 180.0 + joint_max_acc: 180.0 + move_to_initial_pose_on_connect: false + initial_joint_pose: [-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67] + initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910] + init_move_speed: 20 + debug_topic_prefix: /xr_rm diff --git a/xr_rm_bringup/config/left_arm_rm75.yaml b/xr_rm_bringup/config/left_arm_rm75.yaml new file mode 100644 index 0000000..d5bec28 --- /dev/null +++ b/xr_rm_bringup/config/left_arm_rm75.yaml @@ -0,0 +1,45 @@ +# 左臂单独调试配置：无末端执行器，仅 XR 相对位移控制 RM75 TCP。 + +single_arm_velocity_teleop: + ros__parameters: + arm_name: left_rm75 + controller_topic: /xr/left_controller + control_rate_hz: 50.0 + command_timeout_sec: 0.12 + + scale: 0.75 + kp_linear: 1.8 + deadband_m: 0.002 + low_pass_alpha: 0.35 + max_linear_speed: 0.04 + enable_position_axes: [true, true, true] + + workspace_min: [-0.50, -0.60, 0.10] + workspace_max: [0.50, -0.20, 0.50] + cyl_radius_limit: [0.20, 0.60] + low_z_threshold: 0.20 + low_z_min_radius: 0.21 + + xr_to_robot_matrix: [0.0, -1.0, 0.0, + 0.0, 0.0, -1.0, + 1.0, 0.0, 0.0] + + use_mock: false + 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 + frame_type: 1 + follow: false + configure_safety_limits: true + max_line_speed: 1.0 + max_angular_speed: 1.5 + max_line_acc: 1.0 + max_angular_acc: 2.0 + joint_max_speed: 180.0 + joint_max_acc: 180.0 + move_to_initial_pose_on_connect: false + 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 + 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 new file mode 100644 index 0000000..4a5d9b0 --- /dev/null +++ b/xr_rm_bringup/config/right_arm_rm75.yaml @@ -0,0 +1,45 @@ +# 右臂单独调试配置：无末端执行器，仅 XR 相对位移控制 RM75 TCP。 + +single_arm_velocity_teleop: + ros__parameters: + arm_name: right_rm75 + controller_topic: /xr/right_controller + control_rate_hz: 50.0 + command_timeout_sec: 0.12 + + scale: 0.75 + kp_linear: 1.8 + deadband_m: 0.002 + low_pass_alpha: 0.35 + max_linear_speed: 0.04 + enable_position_axes: [true, true, true] + + workspace_min: [-0.70, -0.60, 0.10] + workspace_max: [0.50, 0.40, 0.70] + cyl_radius_limit: [0.20, 0.60] + low_z_threshold: 0.20 + low_z_min_radius: 0.21 + + xr_to_robot_matrix: [0.0, 1.0, 0.0, + 0.0, 0.0, -1.0, + -1.0, 0.0, 0.0] + + use_mock: false + mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910] + robot_ip: 192.168.192.19 + robot_port: 8080 + avoid_singularity: 1 + frame_type: 1 + follow: false + configure_safety_limits: true + max_line_speed: 1.0 + max_angular_speed: 1.5 + max_line_acc: 1.0 + max_angular_acc: 2.0 + joint_max_speed: 180.0 + joint_max_acc: 180.0 + move_to_initial_pose_on_connect: false + initial_joint_pose: [-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67] + initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910] + init_move_speed: 20 + debug_topic_prefix: /xr_rm diff --git a/xr_rm_bringup/config/single_arm_rm75.yaml b/xr_rm_bringup/config/single_arm_rm75.yaml index 99efd0c..f66e5b9 100755 --- a/xr_rm_bringup/config/single_arm_rm75.yaml +++ b/xr_rm_bringup/config/single_arm_rm75.yaml @@ -15,8 +15,11 @@ single_arm_velocity_teleop: max_linear_speed: 0.04 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.50, 0.40, 0.70] + cyl_radius_limit: [0.20, 0.60] + low_z_threshold: 0.20 + low_z_min_radius: 0.21 # 右臂 PICO/OpenXR 相对位移到 RM75 base 坐标的初始映射： # 机器人位移增量 = [手柄y, -手柄z, -手柄x]。 @@ -25,9 +28,21 @@ single_arm_velocity_teleop: -1.0, 0.0, 0.0] use_mock: false - mock_initial_pose: [0.1924, -0.3025, 0.2923, 3.0560, 0.1310, 0.9070] + mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910] robot_ip: 192.168.192.19 robot_port: 8080 avoid_singularity: 1 frame_type: 1 follow: false + configure_safety_limits: true + max_line_speed: 1.0 + max_angular_speed: 1.5 + max_line_acc: 1.0 + max_angular_acc: 2.0 + joint_max_speed: 180.0 + joint_max_acc: 180.0 + move_to_initial_pose_on_connect: false + initial_joint_pose: [-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67] + initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910] + init_move_speed: 20 + debug_topic_prefix: /xr_rm diff --git a/xr_rm_bringup/launch/arm_debug.launch.py b/xr_rm_bringup/launch/arm_debug.launch.py new file mode 100644 index 0000000..00ca1ef --- /dev/null +++ b/xr_rm_bringup/launch/arm_debug.launch.py @@ -0,0 +1,121 @@ +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: + return value.strip().lower() in ("1", "true", "yes", "on") + + +def _config_file(name: str) -> PathJoinSubstitution: + return PathJoinSubstitution([ + FindPackageShare("xr_rm_bringup"), + "config", + name, + ]) + + +def _udp_receiver_node() -> Node: + 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) -> Node: + 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"), + "move_to_initial_pose_on_connect": move_to_initial_pose, + }, + ], + ) + + +def _dual_arm_nodes(use_mock: bool, move_to_initial_pose: bool) -> list[Node]: + 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"), + "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"), + "move_to_initial_pose_on_connect": move_to_initial_pose, + }, + ], + ), + ] + + +def _launch_setup(context, *args, **kwargs): + 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) + ) + + 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)) + else: + nodes.append(_single_arm_node(arm, use_mock, move_to_initial_pose)) + return nodes + + +def generate_launch_description() -> LaunchDescription: + return LaunchDescription([ + DeclareLaunchArgument("arm", default_value="right"), + DeclareLaunchArgument("use_mock", default_value="true"), + DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), + DeclareLaunchArgument("udp_port", default_value="15000"), + 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("move_to_initial_pose_on_connect", default_value="false"), + 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 8816064..b0b6b41 100755 --- a/xr_rm_bringup/launch/dual_arm_mock.launch.py +++ b/xr_rm_bringup/launch/dual_arm_mock.launch.py @@ -1,5 +1,6 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -9,18 +10,23 @@ def generate_launch_description() -> LaunchDescription: config_file = PathJoinSubstitution([ FindPackageShare("xr_rm_bringup"), "config", - "dual_arm_rm75.yaml", + LaunchConfiguration("robot_config"), ]) return LaunchDescription([ DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), DeclareLaunchArgument("udp_port", default_value="15000"), + DeclareLaunchArgument("robot_config", default_value="dual_arm_rm75.yaml"), + DeclareLaunchArgument("run_udp", default_value="true"), + DeclareLaunchArgument("run_left_arm", default_value="true"), + DeclareLaunchArgument("run_right_arm", default_value="true"), # 接收 PICO/XR 侧 UDP 数据，并按左右手字段分发到对应手柄话题。 Node( package="xr_rm_input", executable="udp_controller_receiver", name="udp_controller_receiver", output="screen", + condition=IfCondition(LaunchConfiguration("run_udp")), parameters=[{ "udp_host": LaunchConfiguration("udp_host"), "udp_port": LaunchConfiguration("udp_port"), @@ -34,6 +40,7 @@ def generate_launch_description() -> LaunchDescription: executable="single_arm_velocity_teleop", name="left_arm_teleop", output="screen", + condition=IfCondition(LaunchConfiguration("run_left_arm")), parameters=[config_file, {"use_mock": True}], ), # 右臂模拟节点：与左臂共用同一配置文件，但读取右手柄话题。 @@ -42,14 +49,7 @@ def generate_launch_description() -> LaunchDescription: executable="single_arm_velocity_teleop", name="right_arm_teleop", output="screen", + condition=IfCondition(LaunchConfiguration("run_right_arm")), parameters=[config_file, {"use_mock": True}], ), - # 右手扳机键到 OmniPicker 归一化力控指令的桥接，模拟阶段用于检查话题输出。 - Node( - package="xr_rm_teleop", - executable="gripper_trigger_bridge", - name="right_omnipicker_trigger_bridge", - output="screen", - parameters=[config_file], - ), ]) diff --git a/xr_rm_bringup/launch/dual_arm_realman.launch.py b/xr_rm_bringup/launch/dual_arm_realman.launch.py index 6fe4549..eb8531d 100755 --- a/xr_rm_bringup/launch/dual_arm_realman.launch.py +++ b/xr_rm_bringup/launch/dual_arm_realman.launch.py @@ -1,7 +1,9 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue from launch_ros.substitutions import FindPackageShare @@ -19,11 +21,16 @@ def generate_launch_description() -> LaunchDescription: 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"), + 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"), Node( package="xr_rm_input", executable="udp_controller_receiver", name="udp_controller_receiver", output="screen", + condition=IfCondition(LaunchConfiguration("run_udp")), parameters=[{ "udp_host": LaunchConfiguration("udp_host"), "udp_port": LaunchConfiguration("udp_port"), @@ -36,12 +43,17 @@ def generate_launch_description() -> LaunchDescription: executable="single_arm_velocity_teleop", name="left_arm_teleop", output="screen", + condition=IfCondition(LaunchConfiguration("run_left_arm")), parameters=[ config_file, { "use_mock": False, "robot_ip": LaunchConfiguration("left_robot_ip"), "robot_port": LaunchConfiguration("robot_port"), + "move_to_initial_pose_on_connect": ParameterValue( + LaunchConfiguration("move_to_initial_pose_on_connect"), + value_type=bool, + ), }, ], ), @@ -50,20 +62,18 @@ def generate_launch_description() -> LaunchDescription: executable="single_arm_velocity_teleop", name="right_arm_teleop", output="screen", + condition=IfCondition(LaunchConfiguration("run_right_arm")), parameters=[ config_file, { "use_mock": False, "robot_ip": LaunchConfiguration("right_robot_ip"), "robot_port": LaunchConfiguration("robot_port"), + "move_to_initial_pose_on_connect": ParameterValue( + LaunchConfiguration("move_to_initial_pose_on_connect"), + value_type=bool, + ), }, ], ), - Node( - package="xr_rm_teleop", - executable="gripper_trigger_bridge", - name="right_omnipicker_trigger_bridge", - output="screen", - parameters=[config_file], - ), ]) diff --git a/xr_rm_bringup/launch/single_arm_mock.launch.py b/xr_rm_bringup/launch/single_arm_mock.launch.py index eee3ec6..d99be86 100755 --- a/xr_rm_bringup/launch/single_arm_mock.launch.py +++ b/xr_rm_bringup/launch/single_arm_mock.launch.py @@ -1,36 +1,28 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description() -> LaunchDescription: - config_file = PathJoinSubstitution([ - FindPackageShare("xr_rm_teleop"), - "config", - "single_arm.yaml", + arm_debug_launch = PathJoinSubstitution([ + FindPackageShare("xr_rm_bringup"), + "launch", + "arm_debug.launch.py", ]) return LaunchDescription([ + DeclareLaunchArgument("arm", default_value="right"), DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), DeclareLaunchArgument("udp_port", default_value="15000"), - Node( - package="xr_rm_input", - executable="udp_controller_receiver", - name="udp_controller_receiver", - output="screen", - parameters=[{ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(arm_debug_launch), + launch_arguments={ + "arm": LaunchConfiguration("arm"), + "use_mock": "true", "udp_host": LaunchConfiguration("udp_host"), "udp_port": LaunchConfiguration("udp_port"), - "topic": "/xr/right_controller", - }], - ), - Node( - package="xr_rm_teleop", - executable="single_arm_velocity_teleop", - name="single_arm_velocity_teleop", - output="screen", - parameters=[config_file, {"use_mock": True}], + }.items(), ), ]) diff --git a/xr_rm_bringup/launch/single_arm_realman.launch.py b/xr_rm_bringup/launch/single_arm_realman.launch.py index 28a416d..1c58ee6 100755 --- a/xr_rm_bringup/launch/single_arm_realman.launch.py +++ b/xr_rm_bringup/launch/single_arm_realman.launch.py @@ -1,46 +1,37 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description() -> LaunchDescription: - config_file = PathJoinSubstitution([ + arm_debug_launch = PathJoinSubstitution([ FindPackageShare("xr_rm_bringup"), - "config", - LaunchConfiguration("robot_config"), + "launch", + "arm_debug.launch.py", ]) return LaunchDescription([ + DeclareLaunchArgument("arm", default_value="right"), DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), DeclareLaunchArgument("udp_port", default_value="15000"), + DeclareLaunchArgument("left_robot_ip", default_value="192.168.192.18"), + # 兼容旧命令：robot_ip 仍作为右臂 IP 覆盖入口。 DeclareLaunchArgument("robot_ip", default_value="192.168.192.19"), DeclareLaunchArgument("robot_port", default_value="8080"), - DeclareLaunchArgument("robot_config", default_value="single_arm_rm75.yaml"), - Node( - package="xr_rm_input", - executable="udp_controller_receiver", - name="udp_controller_receiver", - output="screen", - parameters=[{ + DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(arm_debug_launch), + launch_arguments={ + "arm": LaunchConfiguration("arm"), + "use_mock": "false", "udp_host": LaunchConfiguration("udp_host"), "udp_port": LaunchConfiguration("udp_port"), - "topic": "/xr/right_controller", - }], - ), - Node( - package="xr_rm_teleop", - executable="single_arm_velocity_teleop", - name="single_arm_velocity_teleop", - output="screen", - parameters=[ - config_file, - { - "use_mock": False, - "robot_ip": LaunchConfiguration("robot_ip"), - "robot_port": LaunchConfiguration("robot_port"), - }, - ], + "left_robot_ip": LaunchConfiguration("left_robot_ip"), + "right_robot_ip": LaunchConfiguration("robot_ip"), + "robot_port": LaunchConfiguration("robot_port"), + "move_to_initial_pose_on_connect": LaunchConfiguration("move_to_initial_pose_on_connect"), + }.items(), ), ]) diff --git a/xr_rm_bringup/package.xml b/xr_rm_bringup/package.xml index 701b0db..2a895da 100755 --- a/xr_rm_bringup/package.xml +++ b/xr_rm_bringup/package.xml @@ -11,6 +11,7 @@ xr_rm_input xr_rm_teleop + python3-tk ament_cmake diff --git a/xr_rm_bringup/tools/launcher_ui.py b/xr_rm_bringup/tools/launcher_ui.py new file mode 100755 index 0000000..2b878e4 --- /dev/null +++ b/xr_rm_bringup/tools/launcher_ui.py @@ -0,0 +1,893 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import os +import shlex +import shutil +import signal +import subprocess +import tempfile +import time +from pathlib import Path + +try: + import tkinter as tk + from tkinter import messagebox, ttk +except ImportError as exc: + print(f"Missing dependency: {exc}") + print("Install Tkinter with: sudo apt-get install python3-tk") + raise SystemExit(1) + + +DEFAULT_LEFT_IP = "192.168.192.18" +DEFAULT_RIGHT_IP = "192.168.192.19" +DEFAULT_PORT = "15000" +TERMINAL_TITLE_PREFIX = "XR-RM Terminal - " +TOPIC_MONITOR_TITLE = "XR-RM Topic Monitor" +TOPIC_MONITOR_ACTION = "__xr_rm_topic_monitor__" +ROS_GRAPH_MONITOR_TITLE = "XR-RM ROS Graph Monitor" +ROS_GRAPH_MONITOR_ACTION = "__xr_rm_ros_graph_monitor__" + +TOPIC_MONITORS = [ + ("Left Controller", "/xr/left_controller"), + ("Right Controller", "/xr/right_controller"), +] + +ROS_GRAPH_MONITORS = [ + ("ROS Topic List", "ros2 topic list"), + ("ROS Node List", "ros2 node list"), +] + +MODES = [ + "Simulation", + "Left Arm", + "Right Arm", + "Dual Arm", + "Diagnostics", +] + + +def _quote(value: str | Path) -> str: + return shlex.quote(str(value)) + + +def _config_quote(value: str) -> str: + return '"' + value.replace("\\", "\\\\").replace('"', '\\"') + '"' + + +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 _find_workspace_root() -> Path: + env_workspace = os.environ.get("XR_RM_WS") + if env_workspace: + return Path(env_workspace).expanduser().resolve() + + here = Path(__file__).resolve() + for parent in (here.parent, *here.parents): + if (parent / "install" / "setup.bash").exists() and (parent / "src").is_dir(): + return parent + if parent.name == "src" and (parent / "xr_rm_bringup" / "package.xml").exists(): + return parent.parent + return Path.cwd().resolve() + + +def _source_lines(workspace_root: Path) -> list[str]: + ros_setup = Path("/opt/ros/humble/setup.bash") + install_setup = workspace_root / "install" / "setup.bash" + lines = [ + "set +e", + f"cd {_quote(workspace_root)}", + ] + if ros_setup.exists(): + lines.append(f"source {_quote(ros_setup)}") + else: + lines.append("echo 'Warning: /opt/ros/humble/setup.bash was not found.'") + + lines.extend([ + ( + f"if [ -f {_quote(install_setup)} ]; then " + f"source {_quote(install_setup)}; " + "else echo 'Warning: workspace install/setup.bash was not found. Run colcon build first.'; fi" + ), + "echo", + ]) + return lines + + +def _one_click_mock(arm: str, hand: str) -> str: + 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", + "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.'", + "wait \"$launch_pid\"", + ]) + + +def _diagnostic_commands() -> list[tuple[str, str]]: + return [ + ("Open ROS Topic/Node List Monitor", ROS_GRAPH_MONITOR_ACTION), + ] + + +def _topic_monitor_item() -> tuple[str, str]: + return ("Open Controller Topic Monitor", TOPIC_MONITOR_ACTION) + + +def _finalize_items( + items: list[tuple[str, str]], + one_click: tuple[str, str] | None = None, +) -> list[tuple[str, str]]: + final_items = items + _diagnostic_commands() + [_topic_monitor_item()] + if one_click is not None: + final_items.append(one_click) + return _with_index(final_items) + + +def build_commands_by_mode(mode: str) -> list[tuple[str, str]]: + 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 (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 (Both, 10s)", + f"ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10", + ), + ("One-Click Left Mock Demo", _one_click_mock("left", "left")), + ("One-Click Right Mock Demo", _one_click_mock("right", "right")), + ("One-Click Dual Mock Demo", _one_click_mock("both", "both")), + ] + one_click = None + elif mode == "Left Arm": + items = [ + ("Ping Left RM75", f"ping -c 4 {DEFAULT_LEFT_IP}"), + ( + "Left Arm RealMan Launch", + "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false " + f"left_robot_ip:={DEFAULT_LEFT_IP} " + "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", + ), + ] + one_click = None + elif mode == "Right Arm": + items = [ + ("Ping Right RM75", f"ping -c 4 {DEFAULT_RIGHT_IP}"), + ( + "Right Arm RealMan Launch", + "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false " + f"right_robot_ip:={DEFAULT_RIGHT_IP} " + "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", + ), + ] + one_click = None + elif mode == "Dual Arm": + items = [ + ("Ping Left RM75", f"ping -c 4 {DEFAULT_LEFT_IP}"), + ("Ping Right RM75", f"ping -c 4 {DEFAULT_RIGHT_IP}"), + ( + "Dual Arm RealMan Launch", + "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false " + f"left_robot_ip:={DEFAULT_LEFT_IP} right_robot_ip:={DEFAULT_RIGHT_IP} " + "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", + ), + ] + one_click = None + else: + items = [ + ("ROS Doctor Report", "ros2 doctor --report"), + ("XR-RM Bringup Prefix", "ros2 pkg prefix xr_rm_bringup"), + ("XR-RM Input Prefix", "ros2 pkg prefix xr_rm_input"), + ("XR-RM Teleop Prefix", "ros2 pkg prefix xr_rm_teleop"), + ] + one_click = None + return _finalize_items(items, one_click) + + +class LauncherApp: + def __init__(self, root: tk.Tk) -> None: + self.root = root + self.workspace_root = _find_workspace_root() + self.current_commands: list[tuple[str, str]] = [] + + self.root.title("XR-RM Teleop Launcher") + self.root.geometry("760x640") + self.root.minsize(640, 560) + self.root.columnconfigure(0, weight=1) + self.root.rowconfigure(0, weight=1) + + main_frame = ttk.Frame(root, padding=(10, 8, 10, 8)) + main_frame.grid(row=0, column=0, sticky="nsew") + main_frame.columnconfigure(0, weight=1) + main_frame.rowconfigure(2, weight=1) + + title_lbl = ttk.Label(main_frame, text="XR-RM Teleop Launcher", font=("Helvetica", 12, "bold")) + title_lbl.grid(row=0, column=0, sticky="w") + + mode_frame = ttk.Frame(main_frame) + mode_frame.grid(row=1, column=0, sticky="ew", pady=(8, 6)) + mode_frame.columnconfigure(1, weight=1) + ttk.Label(mode_frame, text="Mode:", font=("Helvetica", 10)).grid(row=0, column=0, sticky="w", padx=(0, 6)) + self.mode_var = tk.StringVar(value=MODES[0]) + self.mode_combo = ttk.Combobox( + mode_frame, + textvariable=self.mode_var, + values=MODES, + width=24, + state="readonly", + ) + self.mode_combo.grid(row=0, column=1, sticky="ew") + self.mode_combo.bind("<>", self.on_mode_changed) + + list_frame = ttk.Frame(main_frame) + list_frame.grid(row=2, column=0, sticky="nsew") + list_frame.columnconfigure(0, weight=1) + list_frame.rowconfigure(0, weight=1) + self.listbox = tk.Listbox(list_frame, selectmode=tk.SINGLE, font=("Courier", 10), height=8, width=1) + self.listbox.grid(row=0, column=0, sticky="nsew") + list_scroll = ttk.Scrollbar(list_frame, orient=tk.VERTICAL, command=self.listbox.yview) + list_scroll.grid(row=0, column=1, sticky="ns") + self.listbox.configure(yscrollcommand=list_scroll.set) + self.listbox.bind("", self.run_command) + self.listbox.bind("<>", self.on_selection_changed) + + preview_label = ttk.Label(main_frame, text="Command Preview", anchor=tk.W, font=("Helvetica", 10, "bold")) + preview_label.grid(row=3, column=0, sticky="w", pady=(8, 2)) + self.preview = tk.Text(main_frame, height=3, width=1, wrap=tk.WORD, font=("Courier", 9)) + self.preview.grid(row=4, column=0, sticky="ew") + self.preview.configure(state=tk.DISABLED) + + button_frame = ttk.Frame(main_frame) + button_frame.grid(row=5, column=0, sticky="ew", pady=(8, 0)) + for index in range(3): + button_frame.columnconfigure(index, weight=1, uniform="actions") + + run_btn = tk.Button( + button_frame, + text="Run Selected", + command=self.run_command, + bg="#2e7d32", + fg="white", + font=("Helvetica", 10, "bold"), + height=1, + ) + run_btn.grid(row=0, column=0, sticky="ew", padx=(0, 5)) + + check_btn = tk.Button( + button_frame, + text="Check Env", + command=self.check_prerequisites, + bg="#1976d2", + fg="white", + font=("Helvetica", 10, "bold"), + height=1, + ) + check_btn.grid(row=0, column=1, sticky="ew", padx=5) + + kill_btn = tk.Button( + button_frame, + text="Stop All", + command=self.kill_launched_processes, + bg="#c62828", + fg="white", + font=("Helvetica", 10, "bold"), + height=1, + ) + kill_btn.grid(row=0, column=2, sticky="ew", padx=(5, 0)) + + self.status = tk.Label(root, text="Ready", bd=1, relief=tk.SUNKEN, anchor=tk.W) + self.status.grid(row=1, column=0, sticky="ew") + + self.refresh_command_list() + + def refresh_command_list(self) -> None: + self.current_commands = build_commands_by_mode(self.mode_var.get()) + self.listbox.delete(0, tk.END) + for name, _ in self.current_commands: + self.listbox.insert(tk.END, name) + if self.current_commands: + self.listbox.select_set(0) + self.update_preview() + + def on_mode_changed(self, event=None) -> None: + self.refresh_command_list() + self.status.config(text=f"Mode: {self.mode_var.get()}") + + def on_selection_changed(self, event=None) -> None: + self.update_preview() + + def selected_command(self) -> tuple[str, str] | None: + selection = self.listbox.curselection() + if not selection: + return None + return self.current_commands[selection[0]] + + def update_preview(self) -> None: + selected = self.selected_command() + text = selected[1] if selected else "" + if text == TOPIC_MONITOR_ACTION: + topics = ", ".join(topic for _title, topic in TOPIC_MONITORS) + text = f"Open one Terminator window split into controller topic panes:\n{topics}" + elif text == ROS_GRAPH_MONITOR_ACTION: + text = "Open one Terminator window split into 2 horizontal panes:\nros2 topic list | ros2 node list" + self.preview.configure(state=tk.NORMAL) + self.preview.delete("1.0", tk.END) + self.preview.insert(tk.END, text) + self.preview.configure(state=tk.DISABLED) + + def build_terminal_script(self, title: str, command: str) -> str: + window_title = TERMINAL_TITLE_PREFIX + title + lines = _source_lines(self.workspace_root) + lines.extend([ + "export XR_RM_LAUNCHER_SESSION=1", + f"printf '\\033]0;%s\\007' {_quote(window_title)}", + command, + "exit_code=$?", + "echo", + "echo \"Command exited with code ${exit_code}.\"", + "read -r -p 'Press Enter to close...'", + "exit ${exit_code}", + ]) + return "\n".join(lines) + + def terminal_command(self, title: str, script: str) -> list[str] | None: + window_title = TERMINAL_TITLE_PREFIX + title + if shutil.which("x-terminal-emulator"): + return ["x-terminal-emulator", "--no-dbus", "--title", window_title, "-e", "bash", "-ic", script] + if shutil.which("gnome-terminal"): + return ["gnome-terminal", f"--title={window_title}", "--", "bash", "-ic", script] + if shutil.which("konsole"): + return ["konsole", "--new-tab", "-p", f"tabtitle={window_title}", "-e", "bash", "-ic", script] + if shutil.which("xfce4-terminal"): + return ["xfce4-terminal", "--title", window_title, "--command", f"bash -ic {_quote(script)}"] + if shutil.which("xterm"): + return ["xterm", "-T", window_title, "-e", "bash", "-ic", script] + return None + + def run_command(self, event=None) -> None: + selected = self.selected_command() + if selected is None: + messagebox.showwarning("No Selection", "Please select a command first.") + return + + name, command = selected + if command == TOPIC_MONITOR_ACTION: + self.launch_topic_monitor() + return + if command == ROS_GRAPH_MONITOR_ACTION: + self.launch_ros_graph_monitor() + return + + terminal_cmd = self.terminal_command(name, self.build_terminal_script(name, command)) + if terminal_cmd is None: + messagebox.showerror( + "Terminal Not Found", + "No supported terminal emulator was found. Install gnome-terminal, konsole, xfce4-terminal, or xterm.", + ) + return + + try: + subprocess.Popen(terminal_cmd, cwd=str(self.workspace_root)) + self.status.config(text=f"Launched: {name}") + except Exception as exc: + messagebox.showerror("Launch Failed", f"Failed to launch command:\n{exc}") + self.status.config(text="Launch failed") + + def _ros_package_available(self, package: str) -> bool: + script = "\n".join(_source_lines(self.workspace_root) + [f"ros2 pkg prefix {package} >/dev/null 2>&1"]) + try: + result = subprocess.run( + ["bash", "-lc", script], + cwd=str(self.workspace_root), + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, + timeout=8, + check=False, + ) + except Exception: + return False + return result.returncode == 0 + + def check_prerequisites(self) -> None: + ok: list[str] = [] + warnings: list[str] = [] + errors: list[str] = [] + + if Path("/opt/ros/humble/setup.bash").exists(): + ok.append("[OK] ROS2 Humble setup found.") + else: + errors.append("[MISS] /opt/ros/humble/setup.bash not found.") + + if (self.workspace_root / "src" / "xr_rm_bringup" / "package.xml").exists(): + ok.append(f"[OK] Workspace root: {self.workspace_root}") + else: + warnings.append(f"[WARN] Workspace root guess may be wrong: {self.workspace_root}") + + if (self.workspace_root / "install" / "setup.bash").exists(): + ok.append("[OK] Workspace install/setup.bash found.") + else: + warnings.append("[WARN] Workspace has not been built yet, or install/setup.bash is missing.") + + if self.terminal_command("test", "true") is not None: + ok.append("[OK] Supported terminal emulator found.") + else: + errors.append("[MISS] No supported terminal emulator found.") + + if shutil.which("x-terminal-emulator"): + ok.append("[OK] x-terminal-emulator found.") + else: + errors.append("[MISS] x-terminal-emulator not found. Split monitors require it.") + + terminal_target = self.x_terminal_target() + if terminal_target == "terminator": + ok.append("[OK] x-terminal-emulator uses Terminator split-layout support.") + elif terminal_target: + warnings.append( + f"[WARN] x-terminal-emulator points to {terminal_target}. " + "The split monitors are designed for Terminator." + ) + else: + warnings.append("[WARN] Could not identify x-terminal-emulator target.") + + for package in ("xr_rm_bringup", "xr_rm_input", "xr_rm_teleop"): + if self._ros_package_available(package): + ok.append(f"[OK] ROS package available: {package}") + else: + warnings.append(f"[WARN] ROS package not discoverable yet: {package}") + + try: + import Robotic_Arm # noqa: F401 + + ok.append("[OK] Robotic_Arm Python package found for RealMan mode.") + except ImportError: + warnings.append("[WARN] Robotic_Arm Python package not found. Mock mode is still usable.") + + sections = [] + if errors: + sections.append("Errors:\n" + "\n".join(errors)) + if warnings: + sections.append("Warnings:\n" + "\n".join(warnings)) + if ok: + sections.append("Passed:\n" + "\n".join(ok)) + + message = "\n\n".join(sections) if sections else "No checks were run." + if errors: + self.show_text_dialog("Prerequisite Check", "Errors Found", message) + self.status.config(text="Prerequisite check failed") + elif warnings: + self.show_text_dialog("Prerequisite Check", "Warnings Found", message) + self.status.config(text="Prerequisite check passed with warnings") + else: + self.show_text_dialog("Prerequisite Check", "All Checks Passed", message) + self.status.config(text="Prerequisite check passed") + + def show_text_dialog(self, title: str, heading: str, message: str) -> None: + dialog = tk.Toplevel(self.root) + dialog.title(title) + dialog.geometry("760x520") + dialog.minsize(620, 360) + dialog.transient(self.root) + dialog.grab_set() + dialog.columnconfigure(0, weight=1) + dialog.rowconfigure(0, weight=1) + + frame = ttk.Frame(dialog, padding=(12, 10, 12, 12)) + frame.grid(row=0, column=0, sticky="nsew") + frame.columnconfigure(0, weight=1) + frame.rowconfigure(1, weight=1) + + ttk.Label(frame, text=heading, font=("Helvetica", 12, "bold")).grid(row=0, column=0, sticky="w") + + text_frame = ttk.Frame(frame) + text_frame.grid(row=1, column=0, sticky="nsew", pady=(8, 10)) + text_frame.columnconfigure(0, weight=1) + text_frame.rowconfigure(0, weight=1) + + text = tk.Text(text_frame, width=88, height=22, wrap=tk.WORD, font=("Courier", 10)) + text.grid(row=0, column=0, sticky="nsew") + y_scroll = ttk.Scrollbar(text_frame, orient=tk.VERTICAL, command=text.yview) + y_scroll.grid(row=0, column=1, sticky="ns") + text.configure(yscrollcommand=y_scroll.set) + text.insert("1.0", message) + text.configure(state=tk.DISABLED) + + ok_btn = ttk.Button(frame, text="OK", command=dialog.destroy) + ok_btn.grid(row=2, column=0, sticky="e") + ok_btn.focus_set() + dialog.bind("", lambda _event: dialog.destroy()) + dialog.bind("", lambda _event: dialog.destroy()) + + @staticmethod + def x_terminal_target() -> str: + terminal = shutil.which("x-terminal-emulator") + if terminal is None: + return "" + try: + return Path(terminal).resolve().name + except OSError: + return Path(terminal).name + + def topic_pane_command(self, title: str, topic: str) -> str: + pane_title = f"{TOPIC_MONITOR_TITLE} - {title}" + lines = _source_lines(self.workspace_root) + lines.extend([ + "export XR_RM_LAUNCHER_SESSION=1", + f"printf '\\033]0;%s\\007' {_quote(pane_title)}", + "clear", + f"echo {_quote('=== ' + title + ' ===')}", + f"echo {_quote('ros2 topic echo ' + topic)}", + "echo", + f"ros2 topic echo {topic}", + "echo", + "echo 'Topic echo stopped. This pane is left open for inspection.'", + "exec bash", + ]) + return "\n".join(lines) + + def ros_graph_pane_command(self, title: str, command: str) -> str: + pane_title = f"{ROS_GRAPH_MONITOR_TITLE} - {title}" + lines = _source_lines(self.workspace_root) + lines.extend([ + "export XR_RM_LAUNCHER_SESSION=1", + f"printf '\\033]0;%s\\007' {_quote(pane_title)}", + "trap 'echo; echo Monitor stopped.; exec bash' INT", + "while true; do", + " clear", + f" echo {_quote('=== ' + title + ' ===')}", + f" echo {_quote(command + ' (refreshes every 1s)')}", + " date '+%H:%M:%S'", + " echo", + f" {command}", + " sleep 1", + "done", + ]) + return "\n".join(lines) + + def write_topic_monitor_script(self, title: str, topic: str) -> Path: + script_file = tempfile.NamedTemporaryFile( + mode="w", + prefix="xr_rm_topic_monitor_", + suffix=".sh", + delete=False, + encoding="utf-8", + ) + with script_file: + script_file.write(self.topic_pane_command(title, topic)) + script_file.write("\n") + os.chmod(script_file.name, 0o755) + return Path(script_file.name) + + def write_ros_graph_monitor_script(self, title: str, command: str) -> Path: + script_file = tempfile.NamedTemporaryFile( + mode="w", + prefix="xr_rm_ros_graph_monitor_", + suffix=".sh", + delete=False, + encoding="utf-8", + ) + with script_file: + script_file.write(self.ros_graph_pane_command(title, command)) + script_file.write("\n") + os.chmod(script_file.name, 0o755) + 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 - 80, max(900, int(screen_width * 0.75))) + height = min(screen_height - 80, max(560, int(screen_height * (2.0 / 3.0)))) + width = max(width, min(screen_width, 640)) + height = max(height, min(screen_height, 420)) + left = max((screen_width - width) // 2, 0) + top = max((screen_height - height) // 2, 0) + return width, height, left, top + + def write_topic_monitor_config(self) -> Path: + width, height, left, top = self.topic_monitor_rect() + left_script, right_script = [ + self.write_topic_monitor_script(title, topic) + for title, topic in TOPIC_MONITORS + ] + layout_name = "xr_rm_topic_monitor" + config_text = "\n".join([ + "[global_config]", + " suppress_multiple_term_dialog = True", + "[keybindings]", + "[profiles]", + " [[default]]", + " scrollback_infinite = True", + "[layouts]", + f" [[{layout_name}]]", + " [[[window0]]]", + " type = Window", + " parent = \"\"", + f" title = {_config_quote(TOPIC_MONITOR_TITLE)}", + f" position = {_config_quote(f'{left}:{top}')}", + f" size = {width}, {height}", + " maximised = False", + " fullscreen = False", + " [[[split0]]]", + " type = HPaned", + " parent = window0", + " order = 0", + " ratio = 0.5", + " [[[terminal_left]]]", + " type = Terminal", + " parent = split0", + " order = 0", + f" command = {_config_quote('bash ' + str(left_script))}", + " [[[terminal_right]]]", + " type = Terminal", + " parent = split0", + " order = 1", + f" command = {_config_quote('bash ' + str(right_script))}", + "[plugins]", + "", + ]) + config_file = tempfile.NamedTemporaryFile( + mode="w", + prefix="xr_rm_topic_monitor_", + suffix=".conf", + delete=False, + encoding="utf-8", + ) + with config_file: + 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() + topic_script, node_script = [ + self.write_ros_graph_monitor_script(title, command) + for title, command in ROS_GRAPH_MONITORS + ] + layout_name = "xr_rm_ros_graph_monitor" + config_text = "\n".join([ + "[global_config]", + " suppress_multiple_term_dialog = True", + "[keybindings]", + "[profiles]", + " [[default]]", + " scrollback_infinite = True", + "[layouts]", + f" [[{layout_name}]]", + " [[[window0]]]", + " type = Window", + " parent = \"\"", + f" title = {_config_quote(ROS_GRAPH_MONITOR_TITLE)}", + f" position = {_config_quote(f'{left}:{top}')}", + f" size = {width}, {height}", + " maximised = False", + " fullscreen = False", + " [[[split0]]]", + " type = HPaned", + " parent = window0", + " order = 0", + " ratio = 0.5", + " [[[terminal_topic_list]]]", + " type = Terminal", + " parent = split0", + " order = 0", + f" command = {_config_quote('bash ' + str(topic_script))}", + " [[[terminal_node_list]]]", + " type = Terminal", + " parent = split0", + " order = 1", + f" command = {_config_quote('bash ' + str(node_script))}", + "[plugins]", + "", + ]) + config_file = tempfile.NamedTemporaryFile( + mode="w", + prefix="xr_rm_ros_graph_monitor_", + suffix=".conf", + delete=False, + encoding="utf-8", + ) + with config_file: + config_file.write(config_text) + return Path(config_file.name) + + def topic_monitor_geometry(self) -> str: + width, height, left, top = self.topic_monitor_rect() + return f"{width}x{height}+{left}+{top}" + + def launch_topic_monitor(self) -> None: + terminal = shutil.which("x-terminal-emulator") + if terminal is None: + messagebox.showerror( + "Terminal Not Found", + "x-terminal-emulator was not found. Install a terminal emulator to use the topic monitor.", + ) + return + + if self.x_terminal_target() != "terminator": + messagebox.showwarning( + "Topic Monitor", + "The controller topic monitor uses Terminator's split-layout support. " + "Please set x-terminal-emulator to Terminator.", + ) + return + + config_path = self.write_topic_monitor_config() + command = [ + terminal, + "--no-dbus", + "--title", + TOPIC_MONITOR_TITLE, + "--config", + str(config_path), + "--layout", + "xr_rm_topic_monitor", + ] + try: + subprocess.Popen(command, cwd=str(self.workspace_root)) + 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}") + self.status.config(text="Topic monitor launch failed") + + def launch_ros_graph_monitor(self) -> None: + terminal = shutil.which("x-terminal-emulator") + if terminal is None: + messagebox.showerror( + "Terminal Not Found", + "x-terminal-emulator was not found. Install a terminal emulator to use the ROS graph monitor.", + ) + return + + if self.x_terminal_target() != "terminator": + messagebox.showwarning( + "ROS Graph Monitor", + "The ROS graph monitor uses Terminator's split-layout support. " + "Please set x-terminal-emulator to Terminator.", + ) + return + + config_path = self.write_ros_graph_monitor_config() + command = [ + terminal, + "--no-dbus", + "--title", + ROS_GRAPH_MONITOR_TITLE, + "--config", + str(config_path), + "--layout", + "xr_rm_ros_graph_monitor", + ] + try: + subprocess.Popen(command, cwd=str(self.workspace_root)) + 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}") + self.status.config(text="ROS graph monitor launch failed") + + def close_related_terminal_windows(self) -> int: + title_patterns = (TERMINAL_TITLE_PREFIX, TOPIC_MONITOR_TITLE, ROS_GRAPH_MONITOR_TITLE) + closed = 0 + + if shutil.which("wmctrl"): + try: + output = subprocess.check_output(["wmctrl", "-l"], text=True) + except Exception: + output = "" + for line in output.splitlines(): + parts = line.split(None, 3) + if len(parts) < 4: + continue + window_id, title = parts[0], parts[3] + if any(pattern in title for pattern in title_patterns): + subprocess.run(["wmctrl", "-ic", window_id], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) + closed += 1 + + if closed == 0 and shutil.which("xdotool"): + for pattern in title_patterns: + try: + output = subprocess.check_output(["xdotool", "search", "--name", pattern], text=True) + except subprocess.CalledProcessError: + continue + except Exception: + continue + for window_id in output.splitlines(): + subprocess.run(["xdotool", "windowclose", window_id], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) + closed += 1 + + return closed + + def kill_launched_processes(self) -> None: + confirm = messagebox.askyesno( + "Confirm Stop", + "Stop XR-RM launcher terminals, topic monitors, and ROS nodes started from this workspace?", + ) + if not confirm: + return + + patterns = [ + "ros2 launch xr_rm_bringup", + "ros2 run xr_rm_input", + "ros2 run xr_rm_teleop", + "XR_RM_LAUNCHER_SESSION=1", + TERMINAL_TITLE_PREFIX, + TOPIC_MONITOR_TITLE, + ROS_GRAPH_MONITOR_TITLE, + "xr_rm_topic_monitor_", + "xr_rm_ros_graph_monitor_", + "udp_controller_receiver", + "sample_udp_sender", + "single_arm_velocity_teleop", + "ros2 topic echo /xr/left_controller", + "ros2 topic echo /xr/right_controller", + "ros2 topic list", + "ros2 node list", + ] + protected = {os.getpid(), os.getppid()} + killed: set[int] = set() + + for pattern in patterns: + try: + output = subprocess.check_output(["pgrep", "-f", pattern], text=True) + except subprocess.CalledProcessError: + continue + except Exception as exc: + print(f"Failed to search process pattern {pattern!r}: {exc}") + continue + + for pid_text in output.splitlines(): + try: + pid = int(pid_text) + except ValueError: + continue + if pid in protected or pid in killed: + continue + try: + os.kill(pid, signal.SIGTERM) + killed.add(pid) + except OSError: + pass + + time.sleep(0.3) + closed_windows = self.close_related_terminal_windows() + self.status.config( + text=f"Stop requested: {len(killed)} matching processes, {closed_windows} terminal windows." + ) + messagebox.showinfo( + "Stop Complete", + "Stop requested for:\n" + f"- {len(killed)} matching processes, including launcher terminal wrappers\n" + f"- {closed_windows} related terminal windows", + ) + + +def main() -> None: + root = tk.Tk() + LauncherApp(root) + root.mainloop() + + +if __name__ == "__main__": + main() diff --git a/xr_rm_teleop/config/single_arm.yaml b/xr_rm_teleop/config/single_arm.yaml index c9720cb..82b8afd 100755 --- a/xr_rm_teleop/config/single_arm.yaml +++ b/xr_rm_teleop/config/single_arm.yaml @@ -15,8 +15,11 @@ single_arm_velocity_teleop: max_linear_speed: 0.04 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.50, 0.40, 0.70] + cyl_radius_limit: [0.20, 0.60] + low_z_threshold: 0.20 + low_z_min_radius: 0.21 # 右臂 PICO/OpenXR 相对位移到 RM75 base 坐标的初始映射： # 机器人位移增量 = [手柄y, -手柄z, -手柄x]。 @@ -25,10 +28,22 @@ single_arm_velocity_teleop: -1.0, 0.0, 0.0] use_mock: true - mock_initial_pose: [0.1924, -0.3025, 0.2923, 3.0560, 0.1310, 0.9070] + mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910] robot_ip: 192.168.192.19 robot_port: 8080 avoid_singularity: 1 frame_type: 1 follow: false + configure_safety_limits: true + max_line_speed: 1.0 + max_angular_speed: 1.5 + max_line_acc: 1.0 + max_angular_acc: 2.0 + joint_max_speed: 180.0 + joint_max_acc: 180.0 + move_to_initial_pose_on_connect: false + initial_joint_pose: [-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67] + initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910] + init_move_speed: 20 + debug_topic_prefix: /xr_rm diff --git a/xr_rm_teleop/xr_rm_teleop/realman_adapter.py b/xr_rm_teleop/xr_rm_teleop/realman_adapter.py index b4ee73d..c37103d 100755 --- a/xr_rm_teleop/xr_rm_teleop/realman_adapter.py +++ b/xr_rm_teleop/xr_rm_teleop/realman_adapter.py @@ -60,12 +60,36 @@ class RealManAdapter: dt: float, avoid_singularity: int, frame_type: int, + logger: Any | None = None, + configure_safety_limits: bool = True, + max_line_speed: float = 1.0, + max_angular_speed: float = 1.5, + max_line_acc: float = 1.0, + max_angular_acc: float = 2.0, + joint_max_speed: float = 180.0, + joint_max_acc: float = 180.0, + move_to_initial_pose_on_connect: bool = False, + initial_joint_pose: list[float] | None = None, + initial_tcp_pose: list[float] | None = None, + init_move_speed: int = 20, ) -> None: self._robot_ip = robot_ip self._robot_port = robot_port self._dt_ms = int(round(dt * 1000.0)) self._avoid_singularity = avoid_singularity self._frame_type = frame_type + self._logger = logger + self._configure_safety_limits = configure_safety_limits + self._max_line_speed = max_line_speed + self._max_angular_speed = max_angular_speed + self._max_line_acc = max_line_acc + self._max_angular_acc = max_angular_acc + self._joint_max_speed = joint_max_speed + self._joint_max_acc = joint_max_acc + self._move_to_initial_pose_on_connect = move_to_initial_pose_on_connect + self._initial_joint_pose = initial_joint_pose + self._initial_tcp_pose = initial_tcp_pose + self._init_move_speed = init_move_speed self._arm: Any | None = None def connect(self) -> None: @@ -77,8 +101,12 @@ class RealManAdapter: ) from exc self._arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E) - ret = self._arm.rm_create_robot_arm(self._robot_ip, self._robot_port) - self._check_return(ret, "rm_create_robot_arm") + handle = self._arm.rm_create_robot_arm(self._robot_ip, self._robot_port) + self._check_robot_handle(handle) + 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, @@ -122,6 +150,47 @@ class RealManAdapter: if self._arm is None: raise RuntimeError("睿尔曼机械臂尚未连接") + def _apply_safety_limits(self) -> None: + self._try_call("rm_set_avoid_singularity_mode", True) + 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) + self._try_call("rm_set_arm_max_angular_acc", self._max_angular_acc) + for joint_index in range(1, 8): + self._try_call("rm_set_joint_max_speed", joint_index, self._joint_max_speed) + self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc) + + def _move_to_initial_pose(self) -> None: + if self._initial_joint_pose is None or self._initial_tcp_pose is None: + raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose 和 initial_tcp_pose") + + ret = self._arm.rm_movej(self._initial_joint_pose, self._init_move_speed, 0, 0, 1) + self._check_return(ret, "rm_movej(initial_joint_pose)") + 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 _try_call(self, name: str, *args: Any) -> None: + func = getattr(self._arm, name, None) + if func is None: + self._log_warn(f"当前睿尔曼 SDK 不支持 {name}，跳过该安全配置。") + return + + try: + ret = func(*args) + self._check_return(ret, name) + except Exception as exc: + self._log_warn(f"{name} 安全配置失败，继续使用软件侧限幅：{exc}") + + def _log_warn(self, message: str) -> None: + if self._logger is not None: + self._logger.warn(message) + + @staticmethod + def _check_robot_handle(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") + @staticmethod def _check_return(ret: Any, name: str) -> None: code = ret[0] if isinstance(ret, tuple) and ret else ret @@ -148,6 +217,19 @@ class RealManAdapter: pose = cls._find_pose(value) if pose is not None: return pose + elif hasattr(obj, "to_dictionary"): + try: + return cls._find_pose(obj.to_dictionary(7)) + except TypeError: + return cls._find_pose(obj.to_dictionary()) + elif hasattr(obj, "to_dict"): + return cls._find_pose(obj.to_dict()) + else: + for key in ("pose", "tool_pose", "tcp_pose", "current_pose"): + if hasattr(obj, key): + pose = cls._as_pose(getattr(obj, key)) + if pose is not None: + return pose return None @staticmethod @@ -155,4 +237,33 @@ class RealManAdapter: if isinstance(value, (list, tuple)) and len(value) >= 6: if all(isinstance(item, Number) for item in value[:6]): return [float(item) for item in value[:6]] + if isinstance(value, dict): + position = value.get("position") + euler = value.get("euler") + if isinstance(position, dict) and isinstance(euler, dict): + keys = ("x", "y", "z") + rpy_keys = ("rx", "ry", "rz") + if all(key in position for key in keys) and all(key in euler for key in rpy_keys): + return [ + float(position["x"]), + float(position["y"]), + float(position["z"]), + float(euler["rx"]), + float(euler["ry"]), + float(euler["rz"]), + ] + if all(hasattr(value, attr) for attr in ("position", "euler")): + position = getattr(value, "position") + euler = getattr(value, "euler") + if all(hasattr(position, key) for key in ("x", "y", "z")) and all( + hasattr(euler, key) for key in ("rx", "ry", "rz") + ): + return [ + float(position.x), + float(position.y), + float(position.z), + float(euler.rx), + float(euler.ry), + float(euler.rz), + ] return None 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 608cb52..87dce51 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 @@ -4,11 +4,12 @@ import math from typing import Iterable import rclpy +from geometry_msgs.msg import PoseStamped, TwistStamped from rclpy.node import Node from xr_rm_interfaces.msg import XrController -from .realman_adapter import MockRealManAdapter, RealManAdapter +from .realman_adapter import ArmPose, MockRealManAdapter, RealManAdapter def _norm(values: Iterable[float]) -> float: @@ -37,6 +38,9 @@ class SingleArmVelocityTeleop(Node): self.declare_parameter("enable_position_axes", [True, True, True]) self.declare_parameter("workspace_min", [0.20, -0.35, 0.10]) self.declare_parameter("workspace_max", [0.65, 0.35, 0.60]) + self.declare_parameter("cyl_radius_limit", [0.20, 0.60]) + self.declare_parameter("low_z_threshold", 0.20) + self.declare_parameter("low_z_min_radius", 0.21) self.declare_parameter("xr_to_robot_matrix", [0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0]) self.declare_parameter("use_mock", True) self.declare_parameter("mock_initial_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0]) @@ -45,10 +49,24 @@ class SingleArmVelocityTeleop(Node): self.declare_parameter("avoid_singularity", 1) self.declare_parameter("frame_type", 1) self.declare_parameter("follow", False) + self.declare_parameter("configure_safety_limits", True) + self.declare_parameter("max_line_speed", 1.0) + self.declare_parameter("max_angular_speed", 1.5) + self.declare_parameter("max_line_acc", 1.0) + self.declare_parameter("max_angular_acc", 2.0) + self.declare_parameter("joint_max_speed", 180.0) + self.declare_parameter("joint_max_acc", 180.0) + self.declare_parameter("move_to_initial_pose_on_connect", False) + 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("debug_topic_prefix", "/xr_rm") self._arm_name = str(self.get_parameter("arm_name").value) topic = self.get_parameter("controller_topic").value control_rate_hz = float(self.get_parameter("control_rate_hz").value) + if control_rate_hz <= 0.0: + raise ValueError("control_rate_hz must be > 0") self._dt = 1.0 / control_rate_hz self._command_timeout_sec = float(self.get_parameter("command_timeout_sec").value) self._scale = float(self.get_parameter("scale").value) @@ -59,9 +77,15 @@ class SingleArmVelocityTeleop(Node): self._enable_position_axes = self._bool_list_parameter("enable_position_axes", 3) self._workspace_min = self._float_list_parameter("workspace_min", 3) self._workspace_max = self._float_list_parameter("workspace_max", 3) + self._cyl_radius_limit = self._float_list_parameter("cyl_radius_limit", 2) + self._low_z_threshold = float(self.get_parameter("low_z_threshold").value) + self._low_z_min_radius = float(self.get_parameter("low_z_min_radius").value) self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9) - self._follow = bool(self.get_parameter("follow").value) - self._validate_workspace() + self._follow = self._bool_parameter("follow") + self._debug_topic_prefix = str(self.get_parameter("debug_topic_prefix").value).rstrip("/") + if not self._debug_topic_prefix: + self._debug_topic_prefix = "/xr_rm" + self._validate_parameters() self._last_msg: XrController | None = None self._last_msg_time = None @@ -73,12 +97,17 @@ class SingleArmVelocityTeleop(Node): self._adapter = self._make_adapter() self._adapter.connect() + debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}" + self._current_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/current_pose", 10) + self._target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/target_pose", 10) + self._cmd_vel_pub = self.create_publisher(TwistStamped, f"{debug_ns}/cmd_vel", 10) + self.create_subscription(XrController, topic, self._on_controller, 10) self.create_timer(self._dt, self._control_tick) self.get_logger().info(f"{self._arm_name} 速度遥操节点已启动，监听话题：{topic}") def _make_adapter(self): - if bool(self.get_parameter("use_mock").value): + if self._bool_parameter("use_mock"): return MockRealManAdapter( [float(v) for v in self.get_parameter("mock_initial_pose").value], self._dt, @@ -90,6 +119,18 @@ class SingleArmVelocityTeleop(Node): dt=self._dt, avoid_singularity=int(self.get_parameter("avoid_singularity").value), frame_type=int(self.get_parameter("frame_type").value), + logger=self.get_logger(), + configure_safety_limits=self._bool_parameter("configure_safety_limits"), + max_line_speed=float(self.get_parameter("max_line_speed").value), + max_angular_speed=float(self.get_parameter("max_angular_speed").value), + max_line_acc=float(self.get_parameter("max_line_acc").value), + max_angular_acc=float(self.get_parameter("max_angular_acc").value), + joint_max_speed=float(self.get_parameter("joint_max_speed").value), + joint_max_acc=float(self.get_parameter("joint_max_acc").value), + move_to_initial_pose_on_connect=self._bool_parameter("move_to_initial_pose_on_connect"), + 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), ) def _on_controller(self, msg: XrController) -> None: @@ -119,7 +160,8 @@ class SingleArmVelocityTeleop(Node): controller_now = self._controller_xyz(self._last_msg) try: - robot_now = self._adapter.get_current_pose().xyz() + robot_pose = self._adapter.get_current_pose() + robot_now = robot_pose.xyz() except Exception as exc: self.get_logger().error( f"{self._arm_name} 读取 TCP 位姿失败，停止输出：{exc}", @@ -162,7 +204,9 @@ class SingleArmVelocityTeleop(Node): alpha * velocity[i] + (1.0 - alpha) * self._filtered_velocity[i] for i in range(3) ] - self._send_cartesian_velocity(self._filtered_velocity + [0.0, 0.0, 0.0]) + 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) @staticmethod def _controller_xyz(msg: XrController) -> list[float]: @@ -177,10 +221,32 @@ class SingleArmVelocityTeleop(Node): ] def _clamp_workspace(self, target: list[float]) -> list[float]: - return [ + clamped = [ _clamp(target[i], self._workspace_min[i], self._workspace_max[i]) for i in range(3) ] + return self._clamp_cylinder_radius(clamped) + + def _clamp_cylinder_radius(self, target: list[float]) -> list[float]: + min_radius, max_radius = self._cyl_radius_limit + if target[2] < self._low_z_threshold: + min_radius = max(min_radius, self._low_z_min_radius) + + radius = math.hypot(target[0], target[1]) + if max_radius > 0.0 and radius > max_radius: + scale = max_radius / radius + target[0] *= scale + target[1] *= scale + elif min_radius > 0.0 and radius < min_radius: + if radius > 1e-9: + scale = min_radius / radius + target[0] *= scale + target[1] *= scale + else: + target[0] = min_radius + target[1] = 0.0 + + return target @staticmethod def _clamp_vector_norm(vector: list[float], max_norm: float) -> list[float]: @@ -205,6 +271,64 @@ class SingleArmVelocityTeleop(Node): ) self._active = False + 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) + target_msg = self._pose_msg( + stamp, + ArmPose( + x=target_xyz[0], + y=target_xyz[1], + z=target_xyz[2], + rx=current_pose.rx, + ry=current_pose.ry, + rz=current_pose.rz, + ), + ) + velocity_msg = TwistStamped() + velocity_msg.header.stamp = stamp + velocity_msg.header.frame_id = "rm_base" + velocity_msg.twist.linear.x = float(velocity[0]) + velocity_msg.twist.linear.y = float(velocity[1]) + velocity_msg.twist.linear.z = float(velocity[2]) + velocity_msg.twist.angular.x = float(velocity[3]) + velocity_msg.twist.angular.y = float(velocity[4]) + velocity_msg.twist.angular.z = float(velocity[5]) + + self._current_pose_pub.publish(current_msg) + self._target_pose_pub.publish(target_msg) + self._cmd_vel_pub.publish(velocity_msg) + + @staticmethod + def _pose_msg(stamp, pose: ArmPose) -> PoseStamped: + qx, qy, qz, qw = SingleArmVelocityTeleop._euler_to_quaternion(pose.rx, pose.ry, pose.rz) + msg = PoseStamped() + msg.header.stamp = stamp + msg.header.frame_id = "rm_base" + msg.pose.position.x = float(pose.x) + msg.pose.position.y = float(pose.y) + msg.pose.position.z = float(pose.z) + msg.pose.orientation.x = qx + msg.pose.orientation.y = qy + msg.pose.orientation.z = qz + msg.pose.orientation.w = qw + return msg + + @staticmethod + def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> tuple[float, float, float, float]: + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + + qw = cr * cp * cy + sr * sp * sy + qx = sr * cp * cy - cr * sp * sy + qy = cr * sp * cy + sr * cp * sy + qz = cr * cp * sy - sr * sp * cy + return qx, qy, qz, qw + def _float_list_parameter(self, name: str, expected_length: int) -> list[float]: values = [float(value) for value in self.get_parameter(name).value] if len(values) != expected_length: @@ -212,15 +336,41 @@ class SingleArmVelocityTeleop(Node): return values def _bool_list_parameter(self, name: str, expected_length: int) -> list[bool]: - values = [bool(value) for value in self.get_parameter(name).value] + values = [self._as_bool(value) for value in self.get_parameter(name).value] if len(values) != expected_length: raise ValueError(f"{name} must contain {expected_length} values") return values - def _validate_workspace(self) -> None: + def _bool_parameter(self, name: str) -> bool: + return self._as_bool(self.get_parameter(name).value) + + @staticmethod + def _as_bool(value) -> bool: + if isinstance(value, bool): + return value + if isinstance(value, str): + return value.strip().lower() in ("1", "true", "yes", "on") + return bool(value) + + def _validate_parameters(self) -> None: + if self._command_timeout_sec <= 0.0: + raise ValueError("command_timeout_sec must be > 0") + if self._deadband_m < 0.0: + raise ValueError("deadband_m must be >= 0") + if not 0.0 <= self._low_pass_alpha <= 1.0: + raise ValueError("low_pass_alpha must be in [0, 1]") + if self._max_linear_speed < 0.0: + raise ValueError("max_linear_speed must be >= 0") for axis, (low, high) in enumerate(zip(self._workspace_min, self._workspace_max)): if low >= high: raise ValueError(f"workspace_min[{axis}] must be smaller than workspace_max[{axis}]") + min_radius, max_radius = self._cyl_radius_limit + if min_radius < 0.0: + raise ValueError("cyl_radius_limit[0] must be >= 0") + if max_radius <= min_radius: + raise ValueError("cyl_radius_limit[1] must be > cyl_radius_limit[0]") + if self._low_z_min_radius < 0.0: + raise ValueError("low_z_min_radius must be >= 0") def destroy_node(self) -> bool: self._adapter.close()