From 72353f3773316609678708d66c0cdfffe1a68f60 Mon Sep 17 00:00:00 2001 From: arm_dev Date: Wed, 23 Sep 2026 17:40:55 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B3=A8=E9=87=8A=E4=B8=8E=E6=8F=90=E7=A4=BA?= =?UTF-8?q?=E4=BF=A1=E6=81=AF=E5=85=A8=E9=9D=A2=E4=B8=AD=E6=96=87=E5=8C=96?= =?UTF-8?q?=EF=BC=9A=E6=BA=90=E7=A0=81=2066=20=E4=B8=AA=20.py=20+=20?= =?UTF-8?q?=E8=84=9A=E6=9C=AC/=E9=85=8D=E7=BD=AE=2045=20=E4=B8=AA=E6=96=87?= =?UTF-8?q?=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 背景：现场读代码的人只读中文。此前代码里的 docstring 是英文、行内注释是中文， 一半英文一半中文反而更难看懂 —— 新手打开一个 .py 不知道这个模块为什么存在。 范围（本仓库自己的代码，厂商上游不动） src/arm_application/** 37 个文件：servo_node / perception_node / handeye_* / dh_pgea / yolo_detector / kalman_filter / goto_ee_plane / dual_gripper … src/arm_interfaces/** 11 个文件：paths / manifest / loader / config_guard / errors … src/arm_control/** 5 个文件 src/arm_bringup/** 3 个 launch scripts/** 6 个 .py + 29 个 .sh（scripts/ 是"唯一操作入口"，用户屏幕上 看到的就是这些提示，所以提示信息也一并中文化） config/ handeye/ devices/ src/arm_*/config/ 17 个 .yaml/.xml/.xacro/.cfg 口径（写进 docs/FRAMEWORK_RULES.md 新增的 R6.5，作为长期约束） 注释、docstring、print/日志/异常里给人看的句子 = 中文； 标识符、API 名、话题名、参数名、字段名、命令名 = 保留英文原文（用 `` `` 包起来）； 单位、十六进制、协议报文、CRC 值、命令示例 = 原样保留； src/flexiv_*/ = 厂商上游代码，不许改也不许翻译（R4.4）。 翻译保原意与原有详细程度：那些"为什么不能用另一种做法""踩过什么坑"的说明 全部完整保留 —— 它们才是这份代码里最贵的部分。 新增 scripts/check_text_only.py：只改文字时的自证工具 人工 review 靠不住（115 个文件、3000 多行 diff）。这个脚本把改动前后的源码解析成 语法树，删掉 docstring、抹平字符串常量、把 f-string 只留"插值了几次 + 插值是什么"， 再比对：完全相同 = 改动范围内没有逻辑变化。 留了一个口子：f-string 的插值按内容排序后再比 —— 因为中文语序常常要和英文反着来 （f"on {port} read {idx} failed" → f"在 {port} 上读 {idx} 失败"），只换显示顺序不该判失败； 但多插一个、少插一个、换掉被插入的表达式，立刻会报出来。 本次 66 个 .py 全部通过该闸门。 验收 改动的 66 个 .py：scripts/check_text_only.py 全部通过（零逻辑变化） 45 个非 .py 文件：去注释后与 HEAD 逐字节相同（零配置变化） docstring 总数：没有一个文件减少（不存在"翻译时顺手删了说明"） 单测：arm_application 233 / arm_driver_flexiv 69 / arm_interfaces 46 / arm_kinematics 24 / arm_sim_gazebo 236 = 608 passed，与改动前一致 全部 .sh：bash -n 通过；并额外实际执行了 --help 验证（见下一个提交： bash -n 对 CRLF 脚本会漏报，只有真跑才暴露） Co-authored-by: Cursor --- .cursor/rules/framework-core.mdc | 15 ++ config/fastdds_visual_servo.xml | 20 +- docs/FRAMEWORK_RULES.md | 16 ++ handeye/calibrate.yaml | 8 +- handeye/poses/89_eye_in_hand.yaml | 32 +-- handeye/poses/91_eye_in_hand.yaml | 30 +-- handeye/results/89_eye_in_hand.yaml | 4 +- handeye/waypoints.yaml | 6 +- scripts/README.md | 9 + scripts/build_all.sh | 2 +- scripts/build_flexiv_rdk.sh | 4 +- scripts/build_real.sh | 2 +- scripts/check_robot_network.sh | 2 +- scripts/check_text_only.py | 170 +++++++++++++ scripts/convert_flexiv_traj.py | 30 +-- scripts/convert_traj.sh | 4 +- scripts/env.sh | 8 +- scripts/fetch_flexiv.sh | 8 +- scripts/fetch_isaac_flexiv.sh | 4 +- scripts/fix_robot_lan.sh | 4 +- scripts/gen_site_example.py | 2 +- scripts/go_home.sh | 30 +-- scripts/install_rdk_offline.sh | 6 +- scripts/lib/export_site_env.py | 86 +++---- scripts/lib/load_site_config.sh | 6 +- scripts/lib/parse_arm_args.sh | 6 +- scripts/list_realsense.sh | 2 +- scripts/merge_handeye_poses.py | 8 +- scripts/rdk_drag_bridge.py | 48 ++-- scripts/rdk_gripper.py | 54 ++-- scripts/rdk_switch_tool.py | 76 +++--- scripts/rdk_switch_tool.sh | 8 +- scripts/run_camera_d405.sh | 16 +- scripts/run_camera_d435.sh | 2 +- scripts/run_display.sh | 2 +- scripts/run_dual_gripper.sh | 6 +- scripts/run_gripper.sh | 12 +- scripts/run_handeye.sh | 12 +- scripts/run_hole_detect.sh | 10 +- scripts/run_real.sh | 8 +- scripts/run_sim.sh | 2 +- scripts/run_tactile.sh | 4 +- scripts/run_visual_servo_rt.sh | 12 +- scripts/show_site_config.sh | 2 +- scripts/test_rdk_python.sh | 2 +- scripts/use_local_flexiv.sh | 6 +- .../arm_application/__init__.py | 56 ++--- .../arm_application/box_tracker.py | 8 +- .../arm_application/demo_named_poses.py | 14 +- .../arm_application/dh_official.py | 62 ++--- .../arm_application/dh_pgea.py | 92 +++---- .../arm_application/dh_pgea_node.py | 28 +-- .../arm_application/dual_gripper.py | 2 +- .../arm_application/dual_gripper_cli.py | 48 ++-- .../arm_application/dual_gripper_node.py | 14 +- .../arm_application/goto_ee_plane.py | 98 ++++---- .../arm_application/handeye_calibrate.py | 137 ++++++----- .../arm_application/handeye_lib.py | 32 +-- .../arm_application/handeye_realsense.py | 44 ++-- .../arm_application/handeye_teach.py | 172 ++++++------- .../arm_application/handeye_waypoints.py | 89 ++++--- .../arm_application/hole_detect_node.py | 2 +- .../arm_application/hole_detector.py | 5 +- .../arm_application/kalman_filter.py | 14 +- .../arm_application/move_to_pose_client.py | 16 +- .../arm_application/orisys_tactile.py | 6 +- .../arm_application/perception_node.py | 151 ++++++------ .../arm_application/pid_controller.py | 10 +- .../arm_application/plane_feature.py | 27 +- .../arm_application/realsense_color_pub.py | 180 +++++++------- .../arm_application/realsense_sdk_view.py | 16 +- .../arm_application/realsense_view.py | 37 ++- .../arm_application/run_joint_trajectory.py | 48 ++-- .../arm_application/servo_node.py | 230 +++++++++--------- .../arm_application/site_io.py | 2 +- .../arm_application/stage2_locate_node.py | 4 +- .../arm_application/tactile_cli.py | 10 +- .../arm_application/tactile_interface.py | 2 +- .../arm_application/yolo_detector.py | 76 +++--- src/arm_application/config/dual_grippers.yaml | 4 +- src/arm_application/config/perception.yaml | 10 +- src/arm_application/config/servo.yaml | 12 +- src/arm_application/launch/dh_pgea.launch.py | 6 +- .../launch/dual_gripper.launch.py | 4 +- .../launch/hole_detect.launch.py | 2 +- .../launch/visual_servo.launch.py | 22 +- src/arm_application/setup.py | 16 +- src/arm_bringup/config/bringup.yaml | 6 +- src/arm_bringup/launch/real.launch.py | 18 +- src/arm_bringup/launch/sim.launch.py | 4 +- src/arm_bringup/launch/stack.launch.py | 34 +-- src/arm_control/arm_control/__init__.py | 2 +- .../arm_control/arm_status_node.py | 8 +- .../arm_control/named_pose_server.py | 20 +- .../arm_control/real_trajectory_adapter.py | 20 +- .../arm_control/trajectory_bridge.py | 4 +- src/arm_control/config/controllers.yaml | 8 +- src/arm_control/config/named_poses.yaml | 4 +- src/arm_control/config/named_poses_91.yaml | 4 +- src/arm_control/launch/mock_control.launch.py | 2 +- src/arm_description/urdf/arm.macro.xacro | 20 +- .../urdf/arm.ros2_control.xacro | 2 +- src/arm_interfaces/arm_interfaces/__init__.py | 20 +- .../arm_interfaces/capabilities.py | 66 +++-- src/arm_interfaces/arm_interfaces/cli.py | 18 +- .../arm_interfaces/config_guard.py | 96 ++++---- src/arm_interfaces/arm_interfaces/errors.py | 42 ++-- src/arm_interfaces/arm_interfaces/loader.py | 77 +++--- src/arm_interfaces/arm_interfaces/manifest.py | 70 +++--- src/arm_interfaces/arm_interfaces/paths.py | 34 +-- src/arm_interfaces/arm_interfaces/registry.py | 32 ++- src/arm_interfaces/setup.cfg | 8 +- src/arm_interfaces/setup.py | 2 +- src/arm_interfaces/test/test_contract.py | 57 +++-- src/arm_interfaces/test/test_real_devices.py | 39 ++- 115 files changed, 1758 insertions(+), 1573 deletions(-) create mode 100644 scripts/check_text_only.py diff --git a/.cursor/rules/framework-core.mdc b/.cursor/rules/framework-core.mdc index 3443c4c..8b20713 100644 --- a/.cursor/rules/framework-core.mdc +++ b/.cursor/rules/framework-core.mdc @@ -34,6 +34,21 @@ alwaysApply: true - 把"降级跳过"写成"通过"。 - 在中文路径下 `colcon build`。 - 提交信息只写"改了什么"而不写"为什么"（中文，附实测验收数据）。 +- 在 `src/arm_*/**`、`scripts/**` 里写英文注释 / docstring / 提示信息（R6.5）。 + 注释和给人看的句子**一律中文**；标识符、话题名、字段名、命令名**保留英文原文** + （写在中文句子里时用 `` `` 包起来）。`src/flexiv_*/` 是厂商上游代码，不许翻译（R4.4）。 + +## 只改文字时，必须自证没碰逻辑 + +翻译注释、补文档这类"纯文本"改动最容易出事（漏掉 f-string 插值、顺手改个变量名）， +而且人工 review 看不出来。提交前跑： + +```bash +python3 scripts/check_text_only.py +``` + +它比对语法树（删 docstring + 抹平字符串常量 + f-string 只留插值）， +不一致就报出来。基准是 `git HEAD`，所以要在提交前跑。 ## 出问题先查症状表 diff --git a/config/fastdds_visual_servo.xml b/config/fastdds_visual_servo.xml index 06ff552..25a27bf 100644 --- a/config/fastdds_visual_servo.xml +++ b/config/fastdds_visual_servo.xml @@ -1,22 +1,22 @@ udp_transport UDPv4 - + 65500 @@ -29,7 +29,7 @@ false - + 10 0 @@ -50,7 +50,7 @@ - + @@ -66,7 +66,7 @@ - + diff --git a/docs/FRAMEWORK_RULES.md b/docs/FRAMEWORK_RULES.md index 05320cc..38266a2 100644 --- a/docs/FRAMEWORK_RULES.md +++ b/docs/FRAMEWORK_RULES.md @@ -351,6 +351,22 @@ ROS 构建对非 ASCII 路径会出各种问题。工作目录保持 ASCII（推 格式：`<阶段号> <一句话结果>` + 正文说明"问题是什么、为什么这么改、怎么验证的"。 修静默失效时，把"症状"写进去 —— 那是后来人搜索时唯一能匹配上的东西。 +### R6.5 注释、docstring、提示信息一律用中文 + +现场操作的人读中文。所以本仓库（`src/arm_*/**`、`scripts/**`）里： + +- **注释、docstring、`print`/日志/异常里给人看的句子 = 中文** +- **标识符、API 名、话题名、参数名、字段名、文件名、命令名 = 保留英文原文**， + 写在中文句子里时用双反引号包起来，例如 ``SITE_ROS2_ROOT``、``devices///`` +- 单位、十六进制、协议报文、CRC 值、寄存器地址、命令示例**原样保留** + +**例外：`src/flexiv_*/` 是厂商上游代码，不许改、也不许翻译**（见 R4.4）。 + +只改文本的提交，必须能自证"没碰到逻辑"。做法：把新旧两份源码都解析成 AST， +删掉 docstring、把字符串常量抹平、把 f-string 只留"插值了几次 + 插值是什么"， +再比对 —— 必须完全相同。这条检查会抓出"顺手改名""顺手调整语句顺序""漏掉一个 +f-string 插值"这类问题；而"为了中文语序把两个插值换个显示顺序"属于允许的改动。 + --- ## 7. 改动前的检查清单 diff --git a/handeye/calibrate.yaml b/handeye/calibrate.yaml index 6303903..d95bef4 100644 --- a/handeye/calibrate.yaml +++ b/handeye/calibrate.yaml @@ -1,8 +1,8 @@ -# Hand-eye auto calibration config. +# 手眼自动标定配置。 # -# Workspace: ~/ros2 +# 工作空间：~/ros2 # -# Teach poses, then solve (prefer the wrapper): +# 先示教位姿，再求解（推荐用封装脚本）： # bash ~/ros2/scripts/run_handeye.sh teach # bash ~/ros2/scripts/run_handeye.sh merge # bash ~/ros2/scripts/run_handeye.sh calibrate @@ -33,7 +33,7 @@ handeye_calibrate: start_index: 0 min_poses: 5 - # Camera on flange (eye-in-hand): D405 + # 相机装在法兰上（eye-in-hand）：D405 camera_source: "d405" # d405 | d435 | manual camera_serial: "" camera_namespace: "" diff --git a/handeye/poses/89_eye_in_hand.yaml b/handeye/poses/89_eye_in_hand.yaml index 4b7a3bb..57c3e1d 100644 --- a/handeye/poses/89_eye_in_hand.yaml +++ b/handeye/poses/89_eye_in_hand.yaml @@ -1,38 +1,38 @@ -# Hand-eye taught joint poses (7 joints × N). Shared by calibrate + waypoints. +# 手眼标定示教的关节位姿（7 关节 × N）。由 calibrate + waypoints 共用。 # -# Edit here, or: +# 在这里编辑，或者： # bash ~/ros2/scripts/run_handeye.sh teach # bash ~/ros2/scripts/run_handeye.sh merge # -# /** so any node name receives these params. +# /** 这样任意节点名都能收到这些参数。 /**: ros__parameters: num_poses: 12 - # Taught 2026-09-02 via handeye_teach (RDK free-drive) + # 2026-09-02 通过 handeye_teach 示教（RDK 自由驱动） waypoint_positions: [ - # pose 0 + # 位姿 0 -0.751865, -0.769544, 0.713051, 1.140021, -0.189690, 0.133294, -0.342184, - # pose 1 + # 位姿 1 -0.541318, -0.867448, 0.864868, 1.233428, -0.348623, 0.146046, -0.249719, - # pose 2 + # 位姿 2 -0.343594, -0.948720, 0.848287, 1.273279, -0.954339, 0.019624, 0.071314, - # pose 3 + # 位姿 3 -0.379884, -1.002193, 1.166394, 1.579194, -1.097644, -0.115142, -0.347640, - # pose 4 + # 位姿 4 -0.694826, -0.943610, 0.820178, 0.963907, -0.427763, 0.165888, -0.761562, - # pose 5 + # 位姿 5 -0.964023, -1.025531, 1.013898, 1.032490, -0.167151, 0.017429, -0.566073, - # pose 6 + # 位姿 6 -0.812664, -0.984951, 1.072695, 1.473414, -0.772970, 0.267382, 0.401278, - # pose 7 + # 位姿 7 -0.967527, -1.067191, 1.101680, 1.434183, -0.915388, 0.259486, 0.205330, - # pose 8 + # 位姿 8 -0.451483, -1.257408, 0.895481, 0.806594, -0.652401, -0.221453, 0.184234, - # pose 9 + # 位姿 9 -0.981877, -1.415295, 1.339004, 1.131348, -0.863194, 0.170952, 1.615403, - # pose 10 + # 位姿 10 -0.967163, -1.377051, 1.337671, 1.399175, -1.241886, 0.284547, 1.927756, - # pose 11 + # 位姿 11 -0.575271, -1.141574, 1.121935, 1.502172, -1.452029, 0.088544, 2.200019, ] diff --git a/handeye/poses/91_eye_in_hand.yaml b/handeye/poses/91_eye_in_hand.yaml index da2ac25..2de0284 100644 --- a/handeye/poses/91_eye_in_hand.yaml +++ b/handeye/poses/91_eye_in_hand.yaml @@ -1,36 +1,36 @@ -# Right arm 91 taught joints for hand-eye. -# Fill with: +# 右臂 91 手眼标定示教的关节位姿。 +# 用以下命令填充： # bash ~/ros2/scripts/run_handeye.sh --arm 91 teach # bash ~/ros2/scripts/run_handeye.sh --arm 91 merge # -# Do not reuse 89's waypoint_positions. +# 不要复用 89 的 waypoint_positions。 /**: ros__parameters: num_poses: 12 waypoint_positions: [ - # pose 0 + # 位姿 0 0.387144, -0.514770, -0.072695, 1.399918, 0.326501, 0.308501, -0.298212, - # pose 1 + # 位姿 1 0.459205, -0.747691, 0.042380, 0.908430, 0.436933, -0.099413, -0.398336, - # pose 2 + # 位姿 2 0.113162, -0.929267, -0.132969, 0.776759, 0.579499, -0.081271, -0.588715, - # pose 3 + # 位姿 3 0.823056, -1.181803, -0.157161, 0.344860, 0.073450, -0.436449, -0.333484, - # pose 4 + # 位姿 4 0.561660, 0.126479, -0.217063, 2.400061, 1.116363, 1.205544, -0.955972, - # pose 5 + # 位姿 5 0.749011, 0.058548, -0.125615, 1.951037, 0.399065, 0.388437, -1.057397, - # pose 6 + # 位姿 6 0.707611, -0.557010, -0.302672, 1.263980, 0.333093, 0.208229, -0.938372, - # pose 7 + # 位姿 7 0.197846, -0.400530, 0.263064, 1.879643, 0.062586, 1.133715, -0.791285, - # pose 8 + # 位姿 8 0.804027, -0.518162, 0.353947, 1.985246, -0.941195, 0.481238, -1.551113, - # pose 9 + # 位姿 9 0.296161, -0.030159, 0.317117, 2.335652, 1.072158, 1.524893, -0.591372, - # pose 10 + # 位姿 10 0.346492, -0.906418, -0.118014, 0.810448, 0.420788, -0.122104, -0.197859, - # pose 11 + # 位姿 11 1.031802, -0.422436, -0.640939, 1.656629, 0.375163, 0.470365, -0.328041, ] diff --git a/handeye/results/89_eye_in_hand.yaml b/handeye/results/89_eye_in_hand.yaml index 9926f95..ea5d2c2 100644 --- a/handeye/results/89_eye_in_hand.yaml +++ b/handeye/results/89_eye_in_hand.yaml @@ -1,5 +1,5 @@ -# Left arm 89 eye-in-hand. Do not use for 91. -# Hand-eye result +# 左臂 89 eye-in-hand. 不要用于 91。 +# 手眼标定结果 # mode: eye_in_hand # camera_source: d405 # views: 12 diff --git a/handeye/waypoints.yaml b/handeye/waypoints.yaml index d391adb..0ac49d0 100644 --- a/handeye/waypoints.yaml +++ b/handeye/waypoints.yaml @@ -1,7 +1,7 @@ -# Hand-eye interactive waypoint tour (preview + Free Drive confirm). -# Poses: sibling handeye_poses.yaml +# 手眼标定交互式路点巡回（预览 + 自由驱动确认）。 +# 位姿来源：同目录的 handeye_poses.yaml # -# Run: +# 运行： # bash ~/ros2/scripts/run_handeye.sh waypoints handeye_waypoints: diff --git a/scripts/README.md b/scripts/README.md index c7b29e8..642f56f 100644 --- a/scripts/README.md +++ b/scripts/README.md @@ -27,6 +27,15 @@ - 注意 `run_sim.sh` 不是 Gazebo，它走 mock 驱动（假数据 + RViz），永远不会产生接触力 **环境：** `env.sh` · `build_all.sh` **RDK：** `test_rdk_python.sh` · `rdk_switch_tool.sh` · `rdk_drag_bridge.py` +**改注释/写文档后自检：** `python3 scripts/check_text_only.py` + - 用途：证明这次改动**只动了文字**（注释、docstring、提示信息），没碰逻辑。 + 它把改动前后的源码解析成语法树，删掉 docstring、抹平字符串常量后比对； + "翻译 f-string 时漏掉一个 `{}` 插值""顺手改了个变量名"这类问题会被抓出来。 + - `python3 scripts/check_text_only.py` 查所有改过的 `.py`；也可以直接跟文件名。 + 基准是 `git HEAD`，所以要在**提交之前**跑。老手滑手滑的代价比跑一条命令高得多。 + - 规矩写在 [docs/FRAMEWORK_RULES.md](../docs/FRAMEWORK_RULES.md) 的 R6.5。 + 一句话：**注释/docstring/提示信息用中文；标识符、话题名、字段名保留英文** + （但 `src/flexiv_*/` 是厂商上游代码，不许翻译，见 R4.4）。 **搁置未用：** `fetch_isaac_flexiv.sh`（拉 Flexiv 的 Isaac Sim / RDK 源码到 `thirdparty/`；Isaac 路线目前搁置，Gazebo 是选定平台） `lib/parse_arm_args.sh` 解析 `--arm 89|91`。 diff --git a/scripts/build_all.sh b/scripts/build_all.sh index 36e1b8d..516d6fb 100755 --- a/scripts/build_all.sh +++ b/scripts/build_all.sh @@ -1,5 +1,5 @@ #!/usr/bin/env bash -# Build entire workspace: arm stack + flexiv real drivers +# 构建整个工作区：机械臂栈 + Flexiv 真机驱动 set -eo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" diff --git a/scripts/build_flexiv_rdk.sh b/scripts/build_flexiv_rdk.sh index 70af22a..377ce0e 100755 --- a/scripts/build_flexiv_rdk.sh +++ b/scripts/build_flexiv_rdk.sh @@ -1,5 +1,5 @@ #!/usr/bin/env bash -# Build Flexiv RDK v1.x into ~/flexiv_install (required for real robot / flexiv_hardware). +# 把 Flexiv RDK v1.x 构建安装到 ~/flexiv_install（真机 / `flexiv_hardware` 必需）。 set -euo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" RDK="${ROOT}/src/flexiv_rdk" @@ -11,7 +11,7 @@ if [[ ! -d "$RDK" ]]; then fi echo "Building RDK thirdparty deps into $PREFIX ..." -# Prefer system /opt/ros if present; else conda env already has compilers +# 有系统 /opt/ros 就优先用它；否则 conda 环境里已经有编译器 if [[ -f /opt/ros/humble/setup.bash ]]; then # shellcheck disable=SC1091 source /opt/ros/humble/setup.bash diff --git a/scripts/build_real.sh b/scripts/build_real.sh index 7dd0a8e..6f6cb17 100755 --- a/scripts/build_real.sh +++ b/scripts/build_real.sh @@ -1,5 +1,5 @@ #!/usr/bin/env bash -# Build Flexiv hardware packages for real robot (after RDK installed to ~/flexiv_install) +# 构建真机用的 Flexiv 硬件包（前提：RDK 已装到 ~/flexiv_install） set -eo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" diff --git a/scripts/check_robot_network.sh b/scripts/check_robot_network.sh index 5791579..8a3dd93 100755 --- a/scripts/check_robot_network.sh +++ b/scripts/check_robot_network.sh @@ -1,5 +1,5 @@ #!/usr/bin/env bash -# Check PC ↔ Flexiv robot network (addresses from site.yaml) +# 检查 PC ↔ Flexiv 机器人网络（地址取自 site.yaml） set -euo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" diff --git a/scripts/check_text_only.py b/scripts/check_text_only.py new file mode 100644 index 0000000..c523172 --- /dev/null +++ b/scripts/check_text_only.py @@ -0,0 +1,170 @@ +#!/usr/bin/env python3 +"""纯文本改动的安全闸门：证明一次改动**只动了文字**，没碰逻辑。 + +============================================================================= +为什么需要这个工具 +============================================================================= +"把英文注释翻成中文""补一批说明文档"这类改动，看起来零风险，其实经常出事： +改注释的人手滑动了变量名、顺手调了语句顺序、翻译 f-string 时漏掉一个 ``{}`` 插值。 +这些都不会报错，只在某天变成一句"结果怎么不对了"。 + +人工 review 靠不住（几十个文件、几千行 diff，没人真的逐行看）。 +所以这里用 AST 做机器判据：**把两份源码解析成语法树，抹掉所有文字，再比对。** +抹掉规则： + + 1. 删掉所有 docstring + 2. 所有字符串常量换成同一个占位符 + 3. f-string 只保留"插值了几次、插值的是什么表达式"， + 字面量部分整段丢掉、插值按内容排序 + +第 3 条是为中文语序专门留的口子。英文 ``f"on {port} read {idx} failed"`` +翻成 ``f"在 {port} 上读 {idx} 失败"`` 只是把两个插值的**显示顺序**换了， +语义没变，不该判失败。但只要有人**多插一个、少插一个、或换掉被插入的表达式**， +这里立刻报出来。 + +============================================================================= +用法 +============================================================================= + python3 scripts/check_text_only.py # 检查所有改过的 .py + python3 scripts/check_text_only.py <文件> [...] # 只检查指定文件 + python3 scripts/check_text_only.py --staged # 只检查已 git add 的 + +比较基准固定为 ``git HEAD``（也就是"上一次提交"），所以要用在**提交之前**。 + +退出码 0 = 改动范围内没有逻辑变化；非 0 = 有文件动了不该动的东西（会打印差异）。 +============================================================================= +""" + +from __future__ import annotations + +import argparse +import ast +import difflib +import subprocess +import sys +from pathlib import Path + +REPO_ROOT = Path(__file__).resolve().parents[1] + +# 被抹平后的字符串统一长这样。用控制字符是为了保证它不会和真实内容撞车。 +PLACEHOLDER = "\0STR\0" + + +class _Normalize(ast.NodeTransformer): + """把语法树里的"文字"全部抹掉，只留下结构与标识符。""" + + def _strip_docstring(self, node): + """删掉模块 / 类 / 函数的第一个字符串表达式（docstring）。""" + self.generic_visit(node) + body = getattr(node, "body", None) + if body and isinstance(body[0], ast.Expr) and isinstance(body[0].value, ast.Constant) \ + and isinstance(body[0].value.value, str): + node.body = body[1:] + return node + + visit_Module = _strip_docstring + visit_FunctionDef = _strip_docstring + visit_AsyncFunctionDef = _strip_docstring + visit_ClassDef = _strip_docstring + + def visit_Constant(self, node: ast.Constant): + if isinstance(node.value, str): + return ast.copy_location(ast.Constant(value=PLACEHOLDER), node) + return node + + def visit_JoinedStr(self, node: ast.JoinedStr): + """f-string：只留插值，不留字面量；插值按内容排序（见模块 docstring）。""" + self.generic_visit(node) + parts = [ast.dump(v) for v in node.values if isinstance(v, ast.FormattedValue)] + return ast.copy_location( + ast.Constant(value="FSTR[" + "|".join(sorted(parts)) + "]"), node) + + +def normalize(source: str) -> str: + """源码 -> 抹掉文字后的规范化结构字符串。""" + return ast.dump(_Normalize().visit(ast.parse(source))) + + +def _git(*args: str) -> subprocess.CompletedProcess: + return subprocess.run(["git", *args], capture_output=True, text=True, cwd=REPO_ROOT) + + +def head_source(path: str) -> str | None: + """取该文件在 HEAD 里的内容；返回 None 表示 HEAD 里没有它（新文件）。""" + r = _git("show", f"HEAD:{path}") + return r.stdout if r.returncode == 0 else None + + +def changed_python_files() -> list[str]: + r = _git("diff", "--name-only", "HEAD") + return [p for p in r.stdout.split() if p.endswith(".py")] + + +def staged_python_files() -> list[str]: + r = _git("diff", "--cached", "--name-only") + return [p for p in r.stdout.split() if p.endswith(".py")] + + +def first_diff(old: str, new: str) -> str: + """给出一段人能看懂的差异上下文。""" + matcher = difflib.SequenceMatcher(None, old, new, autojunk=False) + for tag, i1, i2, j1, j2 in matcher.get_opcodes(): + if tag != "equal": + return (f" 结构差异类型: {tag}\n" + f" 改前: ...{old[max(0, i1 - 70):i2 + 70]}...\n" + f" 改后: ...{new[max(0, j1 - 70):j2 + 70]}...") + return " （未找到差异片段——理论上不该走到这里）" + + +def main() -> int: + ap = argparse.ArgumentParser( + description="证明一次改动只动了注释/字符串，没有改逻辑", + ) + ap.add_argument("paths", nargs="*", help="要检查的文件；留空 = 所有改过的 .py") + ap.add_argument("--staged", action="store_true", help="只检查已 git add 的文件") + args = ap.parse_args() + + paths = args.paths or (staged_python_files() if args.staged else changed_python_files()) + if not paths: + print("没有需要检查的 .py 文件（工作区是干净的？）。") + return 0 + + ok, problems, skipped = 0, [], [] + for path in paths: + disk = REPO_ROOT / path + if not disk.is_file(): + skipped.append((path, "工作区里不存在（被删除了？）")) + continue + old = head_source(path) + if old is None: + skipped.append((path, "HEAD 里没有它（新增文件），没有基准可比")) + continue + try: + same = normalize(old) == normalize(disk.read_text(encoding="utf-8")) + except SyntaxError as exc: + problems.append((path, f" 语法错误，先修好再看：{exc}")) + continue + if same: + ok += 1 + else: + problems.append((path, first_diff(normalize(old), + normalize(disk.read_text(encoding="utf-8"))))) + + print(f"通过（只改了文字）: {ok}") + for path, why in skipped: + print(f"跳过 {path} —— {why}") + + if problems: + print(f"\n!! 有 {len(problems)} 个文件动了文字之外的东西：") + for path, why in problems: + print(f" {path}") + print(why) + print("\n注意：如果差异只出现在 f-string 的插值顺序上，那是允许的（见脚本说明）。") + return 1 + + print("\n闸门通过：改动范围内没有逻辑变化。") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/scripts/convert_flexiv_traj.py b/scripts/convert_flexiv_traj.py index bedfb86..6463a44 100755 --- a/scripts/convert_flexiv_traj.py +++ b/scripts/convert_flexiv_traj.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Convert Flexiv Elements .traj (text) -> arm_application YAML waypoints.""" +"""把 Flexiv Elements 的 .traj（文本）转换成 arm_application 的 YAML 路点。""" from __future__ import annotations @@ -12,7 +12,7 @@ from typing import Any def parse_flexiv_traj(text: str) -> list[dict[str, Any]]: - """Parse Elements Studio scatter-point .traj file.""" + """解析 Elements Studio 导出的散点 .traj 文件。""" blocks = re.split(r'pattern_params\s*\{', text)[1:] waypoints: list[dict[str, Any]] = [] for block in blocks: @@ -35,7 +35,7 @@ def parse_flexiv_traj(text: str) -> list[dict[str, Any]]: def assign_times(waypoints: list[dict[str, Any]], min_seg: float = 3.0) -> None: - """Cumulative time_from_start from joint-space segment length.""" + """按关节空间的段长累加出 time_from_start。""" t = 0.0 prev = None for wp in waypoints: @@ -44,7 +44,7 @@ def assign_times(waypoints: list[dict[str, Any]], min_seg: float = 3.0) -> None: t += min_seg else: delta = max(abs(a - b) for a, b in zip(pos, prev)) - # ~4 s per radian max joint move, floored at min_seg + # 关节最大运动量每弧度约 4 秒，且不低于 min_seg t += max(min_seg, delta * 4.0) wp['time'] = round(t, 2) prev = pos @@ -65,17 +65,17 @@ def to_yaml( pos_line = ', '.join(f'{v:.6f}' for v in flat) time_line = ', '.join(str(t) for t in times) names = ', '.join(f' # {wp.get("name", "")}' if i == 0 else '' for i, wp in enumerate(waypoints)) - _ = names # names kept in comments below + _ = names # 名称保留在下面的注释里 lines = [ - '# Auto-converted from Flexiv Elements .traj (joints: deg -> rad)', - '# ROS params: flat waypoint_positions + waypoint_times (no nested dicts)', - '# Edit velocity_scale / loop_count as needed (no rebuild).', + '# 由 Flexiv Elements .traj 自动转换而来（关节角：度 -> 弧度）', + '# ROS 参数：扁平的 waypoint_positions + waypoint_times（不使用嵌套字典）', + '# 按需修改 velocity_scale / loop_count（无需重新编译）。', 'run_joint_trajectory:', ' ros__parameters:', f' joint_prefix: "{joint_prefix}"', ' joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]', f' velocity_scale: {velocity_scale}', - ' loop_count: 1 # 1=once, 0=forever, N=repeat N', + ' loop_count: 1 # 1=执行一次，0=无限循环，N=重复 N 次', ' pause_between_loops: 0.5', f' waypoint_times: [{time_line}]', ' waypoint_positions: [', @@ -89,16 +89,16 @@ def to_yaml( def main() -> int: ap = argparse.ArgumentParser(description=__doc__) - ap.add_argument('input', type=Path, help='Flexiv Elements .traj file') - ap.add_argument('-o', '--output', type=Path, help='Output YAML path') - ap.add_argument('--prefix', default='Rizon4-063989_', help='Joint name prefix (site SN + _)') - ap.add_argument('--min-seg', type=float, default=3.0, help='Min seconds per segment') + ap.add_argument('input', type=Path, help='Flexiv Elements 的 .traj 文件') + ap.add_argument('-o', '--output', type=Path, help='输出的 YAML 路径') + ap.add_argument('--prefix', default='Rizon4-063989_', help='关节名前缀（现场 SN + _）') + ap.add_argument('--min-seg', type=float, default=3.0, help='每段的最小秒数') args = ap.parse_args() text = args.input.read_text(encoding='utf-8') wps = parse_flexiv_traj(text) if not wps: - print('ERROR: no waypoints parsed', file=sys.stderr) + print('错误：没有解析到任何路点', file=sys.stderr) return 1 assign_times(wps, min_seg=args.min_seg) yaml_text = to_yaml(wps, joint_prefix=args.prefix) @@ -107,7 +107,7 @@ def main() -> int: if out: out.parent.mkdir(parents=True, exist_ok=True) out.write_text(yaml_text, encoding='utf-8') - print(f'Wrote {len(wps)} waypoints -> {out}') + print(f'已写入 {len(wps)} 个路点 -> {out}') else: print(yaml_text) return 0 diff --git a/scripts/convert_traj.sh b/scripts/convert_traj.sh index 0ae2c15..ac42f9c 100755 --- a/scripts/convert_traj.sh +++ b/scripts/convert_traj.sh @@ -1,6 +1,6 @@ #!/usr/bin/env bash -# Convert Flexiv Elements .traj → arm_application YAML (standard entry for all trajectories). -# Usage: +# 把 Flexiv Elements 的 .traj 转成 `arm_application` 的 YAML（所有轨迹的统一入口）。 +# 用法： # ./scripts/convert_traj.sh ~/文档/my_task.traj # ./scripts/convert_traj.sh ~/文档/my_task.traj -o /path/to/out.yaml # ./scripts/convert_traj.sh ~/文档/my_task.traj --min-seg 2.5 --no-copy diff --git a/scripts/env.sh b/scripts/env.sh index 0516c57..d68fb17 100755 --- a/scripts/env.sh +++ b/scripts/env.sh @@ -1,6 +1,6 @@ #!/usr/bin/env bash -# Source this workspace environment (conda + ROS + flexiv libs). -# Usage: source scripts/env.sh +# 加载本工作区环境（conda + ROS + flexiv 库）。 +# 用法：source scripts/env.sh ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" if [[ -f /opt/ros/humble/setup.bash ]]; then @@ -33,7 +33,7 @@ fi # shellcheck disable=SC1091 source "${ROOT}/install/setup.bash" export LD_LIBRARY_PATH="${HOME}/flexiv_install/lib:${LD_LIBRARY_PATH:-}" -# Pip TensorRT (tensorrt-cu12) — needed to load YOLO .engine +# pip 版 TensorRT（tensorrt-cu12）—— 加载 YOLO `.engine` 需要它 _TRT_SITE="${CONDA_PREFIX:-${HOME}/anaconda3/envs/ros_flexiv}/lib/python3.11/site-packages" for _d in "${_TRT_SITE}/tensorrt" "${_TRT_SITE}/tensorrt_libs" "${_TRT_SITE}/nvidia/tensorrt/lib"; do if [[ -d "${_d}" ]]; then @@ -44,6 +44,6 @@ unset _d _TRT_SITE export YOLO_AUTOINSTALL=false export ROS2_WS="${ROOT}" -# Master site config → SITE_* (YOLO weights, camera, robot SN, …) +# 主站点配置 → `SITE_*`（YOLO 权重、相机、机器人 SN……） # shellcheck disable=SC1091 source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true diff --git a/scripts/fetch_flexiv.sh b/scripts/fetch_flexiv.sh index 0778a50..78b643a 100755 --- a/scripts/fetch_flexiv.sh +++ b/scripts/fetch_flexiv.sh @@ -1,5 +1,5 @@ #!/usr/bin/env bash -# Clone official Flexiv ROS 2 stack for Rizon (RDK v1.x → humble-v1 branch). +# 克隆 Flexiv 官方 ROS 2 栈（Rizon 用，RDK v1.x → humble-v1 分支）。 set -euo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" @@ -9,7 +9,7 @@ BRANCH="${FLEXIV_ROS2_BRANCH:-humble-v1}" mkdir -p "${SRC}" cd "${SRC}" -# Always ensure official URDF package is present (meshes + xacro) +# 无论如何都要保证官方 URDF 包存在（mesh + xacro） if [[ ! -d flexiv_description ]]; then echo "Fetching flexiv_description (${BRANCH})..." if ! git clone --depth 1 -b "${BRANCH}" https://github.com/flexivrobotics/flexiv_description.git; then @@ -58,7 +58,7 @@ if ! command -v vcs >/dev/null 2>&1; then exit 1 fi -# Prefer local Flexiv_Rizon4 RDK (avoids GitHub clone when present) +# 优先用本地 Flexiv_Rizon4 里的 RDK（有本地副本时就不去 GitHub 克隆） LOCAL_FLEXIV="${LOCAL_FLEXIV:-$HOME/文档/Flexiv_Rizon4}" if [[ -d "${LOCAL_FLEXIV}/flexiv_rdk" ]]; then echo "Using local RDK: ${LOCAL_FLEXIV}/flexiv_rdk" @@ -73,7 +73,7 @@ else fi fi -# Always import remaining deps from repos (description already handled; skip-existing) +# 其余依赖一律从 .repos 导入（description 已单独处理；skip-existing 跳过已有） vcs import "${SRC}" < "${REPOS_FILE}" --recursive --skip-existing || true cat </dev/null || true PC_IP_PRIMARY="${SITE_PC_IP:-192.168.2.10}" PC_ADDRS=("192.168.2.10/24" "192.168.100.10/24") ROBOT_IPS=("${SITE_ROBOT_IP:-192.168.2.100}") -# Catalog extras (second arm) +# catalog 里的额外项（第二条臂） if [[ -n "${SITE_ROBOT_CATALOG:-}" ]]; then : fi @@ -132,7 +132,7 @@ for ip in 192.168.2.100 192.168.101.1 192.168.2.101 "${SITE_ROBOT_IP:-}"; do [[ -z "$ip" ]] && continue ping_one "$ip" && ok=1 || true done -# unique +# 去重 echo if ping -c 1 -W 1 "${SITE_ROBOT_IP:-192.168.2.100}" >/dev/null 2>&1; then echo "OK — 当前臂 ${SITE_ROBOT_SN:-} @ ${SITE_ROBOT_IP} 可达。RDK 用 SN 连接。" diff --git a/scripts/gen_site_example.py b/scripts/gen_site_example.py index 6be2bd9..2b83ca9 100644 --- a/scripts/gen_site_example.py +++ b/scripts/gen_site_example.py @@ -88,7 +88,7 @@ REGEX_REPLACEMENTS: list[tuple[str, str]] = [ def render(site_text: str) -> str: - """Turn the live config text into the anonymised template text.""" + """把现场配置文本转换成匿名化后的模板文本。""" out = site_text for old, new in LITERAL_REPLACEMENTS: out = out.replace(old, new) diff --git a/scripts/go_home.sh b/scripts/go_home.sh index 0894129..b4624d5 100755 --- a/scripts/go_home.sh +++ b/scripts/go_home.sh @@ -1,10 +1,10 @@ #!/usr/bin/env bash -# Move Rizon4 to a named pose (default: home) and FORCE stop. +# 把 Rizon4 移动到指定命名的位姿（默认：home）并强制停止。 # -# Critical: visual_servo.launch / servo_node must be killed first, otherwise -# they keep streaming JointTrajectory and the arm never settles. +# 关键：必须先杀掉 visual_servo.launch / servo_node，否则 +# 它们会一直发 JointTrajectory，机械臂永远稳定不下来。 # -# Usage: +# 用法： # bash ~/ros2/scripts/go_home.sh # bash ~/ros2/scripts/go_home.sh --pose ready --speed 0.3 @@ -39,7 +39,7 @@ while [[ $# -gt 0 ]]; do --pose) POSE="${2:?}"; shift 2 ;; --speed) SPEED="${2:?}"; shift 2 ;; --keep-servo) KEEP_SERVO=1; shift ;; - --stop-track) shift ;; # legacy + --stop-track) shift ;; # 历史遗留 -h|--help) usage; exit 0 ;; *) echo "Unknown: $1" >&2; usage >&2; exit 1 ;; esac @@ -56,10 +56,10 @@ SPEED="${SPEED:-${SITE_GO_HOME_SPEED:-0.25}}" kill_streamers() { echo "=== killing visual-servo streamers ===" - # Prefer disable control before kill (if node still up) + # 能先禁用控制就禁用（如果节点还活着），再动手 kill ros2 param set /servo_node enable_control false >/dev/null 2>&1 || true - # Kill launch parent first, then executables (SIGTERM then SIGKILL) + # 先杀 launch 父进程，再杀可执行文件（先 SIGTERM 再 SIGKILL） local patterns=( 'visual_servo.launch.py' '/lib/arm_application/servo_node' @@ -115,7 +115,7 @@ n.destroy_node(); rclpy.shutdown() PY } -# Strong settle via controller ACTION (not topic — matches JTC reliably) +# 用控制器的 ACTION 做强力稳定（不走话题 —— 这样能可靠地匹配 JTC） settle_hold_action() { local pose="$1" python3 - "$pose" <<'PY' @@ -160,7 +160,7 @@ def spin_until(pred, timeout=3.0): spin_until(lambda: js_box[0] is not None, 3.0) -# Two-point: current -> target (or target hold), BOTH with zero velocity +# 两点轨迹：当前位姿 -> 目标位姿（或保持在目标位姿），两点速度都为零 q_now = list(target) if js_box[0] is not None: m = dict(zip(js_box[0].name, js_box[0].position)) @@ -172,14 +172,14 @@ if js_box[0] is not None: def send_hold(q, T=0.40): traj = JointTrajectory() traj.joint_names = list(JOINTS) - # start + # 起点 p0 = JointTrajectoryPoint() p0.positions = [float(v) for v in q] p0.velocities = [0.0] * 7 p0.accelerations = [0.0] * 7 p0.time_from_start.sec = 0 p0.time_from_start.nanosec = 50_000_000 - # end same pose, still zero vel (pin) + # 终点是同一姿势，速度仍为零（钉住） p1 = JointTrajectoryPoint() p1.positions = [float(v) for v in q] p1.velocities = [0.0] * 7 @@ -203,11 +203,11 @@ def send_hold(q, T=0.40): spin_until(lambda: rfut.done(), T + 3.0) return True -# 1) snap toward named pose with zero end vel +# 1) 先朝命名位姿快速靠拢，末端速度为零 print(f" action hold → {name} (zero vel)") send_hold(target, T=0.60) time.sleep(0.4) -# 2–4) re-pin measured pose several times (align JTC open-loop cmd to encoder) +# 2–4) 反复把实测位姿重新钉住（让 JTC 的开环指令与编码器读数对齐） for i in range(3): spin_until(lambda: js_box[0] is not None, 1.0) if js_box[0] is None: @@ -218,7 +218,7 @@ for i in range(3): send_hold(q_pin, T=0.45) time.sleep(0.25) -# Wait until |qd| small +# 等到 |qd| 足够小 print(" waiting for joints to stop ...") t0 = time.monotonic() while time.monotonic() - t0 < 5.0: @@ -270,7 +270,7 @@ sleep "${WAIT_SEC}" echo "=== settle (action, vel=0) ===" settle_hold_action "${POSE}" -# Final check: servo must still be dead +# 最后确认：servo 必须仍然是死的 if pgrep -f '/lib/arm_application/servo_node' >/dev/null 2>&1; then echo "WARNING: servo_node came back — killing again" kill_streamers diff --git a/scripts/install_rdk_offline.sh b/scripts/install_rdk_offline.sh index 404e9dd..0a4b2ee 100755 --- a/scripts/install_rdk_offline.sh +++ b/scripts/install_rdk_offline.sh @@ -1,6 +1,6 @@ #!/usr/bin/env bash -# Offline-friendly Flexiv RDK v1.9.1 install for ROS (humble-v1 / flexiv_hardware). -# CMakeLists already supports FLEXIV_RDK_OFFLINE_LIB / vendor/rdk_libs/. +# 面向离线场景的 Flexiv RDK v1.9.1 安装脚本（供 ROS 的 humble-v1 / `flexiv_hardware` 使用）。 +# CMakeLists 已经支持 `FLEXIV_RDK_OFFLINE_LIB` / `vendor/rdk_libs/`。 set -euo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" @@ -11,7 +11,7 @@ EXPECTED_HASH="$(tr -d '[:space:]' < "${RDK_SRC}/lib/${LIB_NAME}.sha256")" VENDOR_LIB="${ROOT}/vendor/rdk_libs/${LIB_NAME}" JOBS="${JOBS:-$(nproc)}" -# Prefer conda ros_flexiv toolchain (cmake/g++ often only there) +# 优先用 conda 的 ros_flexiv 工具链（cmake/g++ 往往只在那里有） if ! command -v cmake >/dev/null 2>&1; then # shellcheck disable=SC1091 source "${HOME}/anaconda3/etc/profile.d/conda.sh" 2>/dev/null \ diff --git a/scripts/lib/export_site_env.py b/scripts/lib/export_site_env.py index 5b26234..17ec653 100644 --- a/scripts/lib/export_site_env.py +++ b/scripts/lib/export_site_env.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 -"""Load ~/ros2/config/site.yaml and print bash export lines or JSON. +"""读取 ``~/ros2/config/site.yaml``，打印 bash 的 export 行或 JSON。 -Usage: +用法: eval "$(python3 ~/ros2/scripts/lib/export_site_env.py)" python3 ~/ros2/scripts/lib/export_site_env.py --json python3 ~/ros2/scripts/lib/export_site_env.py --print @@ -35,18 +35,18 @@ def site_path() -> Path: def load_site(path: Path | None = None) -> dict: path = path or site_path() if not path.is_file(): - raise FileNotFoundError(f"site config not found: {path}") + raise FileNotFoundError(f"找不到 site 配置: {path}") text = path.read_text(encoding="utf-8") if yaml is None: - raise RuntimeError("PyYAML required: pip/conda install pyyaml") + raise RuntimeError("需要 PyYAML：pip/conda install pyyaml") data = yaml.safe_load(text) or {} if not isinstance(data, dict): - raise ValueError(f"invalid site.yaml: {path}") + raise ValueError(f"无效的 site.yaml: {path}") return data def list_robots(data: dict) -> list[dict]: - """Catalog entries from robot.robots (list or SN-keyed map).""" + """从 robot.robots 取清单条目（列表或按 SN 索引的映射）。""" raw = (data.get("robot") or {}).get("robots") or [] out: list[dict] = [] if isinstance(raw, dict): @@ -90,7 +90,7 @@ def _deep_merge(base: dict, overlay: dict) -> dict: def normalize_robot_sn(data: dict, want: str) -> str: - """Map 89/91/left/right/label → full Flexiv SN.""" + """把 89/91/left/right/label 映射成完整的 Flexiv SN。""" w = (want or "").strip() if not w: return w @@ -109,7 +109,7 @@ def normalize_robot_sn(data: dict, want: str) -> str: def resolve_robot(data: dict, sn: str | None = None) -> dict: - """Active arm: ROBOT_SN env overrides robot.sn; catalog fills ip/iface/frames.""" + """当前生效的机械臂：环境变量 ``ROBOT_SN`` 覆盖 robot.sn；清单补齐 ip/iface/frames。""" robot = dict(data.get("robot") or {}) catalog = list_robots(data) want = normalize_robot_sn( @@ -146,7 +146,7 @@ def resolve_robot(data: dict, sn: str | None = None) -> dict: def apply_active_arm(data: dict, sn: str | None = None) -> tuple[dict, dict]: - """Merge robots[] camera/gripper/paths/servo onto site defaults for the active SN.""" + """把 robots[] 里的 camera/gripper/paths/servo 合并到当前 SN 对应的 site 默认值上。""" robot = resolve_robot(data, sn) merged = dict(data) for key in _ARM_SECTION_KEYS: @@ -157,14 +157,14 @@ def apply_active_arm(data: dict, sn: str | None = None) -> tuple[dict, dict]: def camera_ns(data: dict) -> str: - """Topic namespace for the active wrist camera (d405_left / d405_right).""" + """当前腕部相机（d405_left / d405_right）的话题命名空间。""" cam = data.get("camera") or {} name = str(cam.get("name") or "d405").strip().strip("/") return name or "d405" def camera_frames(data: dict) -> tuple[str, str, str]: - """(namespace, optical_frame, depth_topic) for the active arm.""" + """当前机械臂的（命名空间, 光学坐标系, 深度话题）。""" ns = camera_ns(data) yolo = data.get("yolo") or {} frame = str(yolo.get("camera_frame") or "").strip() @@ -177,7 +177,7 @@ def camera_frames(data: dict) -> tuple[str, str, str]: def flatten(data: dict) -> dict: - """Flat SITE_* env map for bash scripts.""" + """供 bash 脚本使用的扁平 ``SITE_*`` 环境变量映射。""" data, robot = apply_active_arm(data) paths = data.get("paths") or {} cam = data.get("camera") or {} @@ -341,7 +341,7 @@ def flatten(data: dict) -> dict: def gripper_overrides(data: dict) -> dict: - """ROS parameter dict for dh_pgea_node (DH Modbus, not Flexiv gripper).""" + """供 dh_pgea_node 使用的 ROS 参数字典（DH Modbus，不是 Flexiv 夹爪）。""" data, _robot = apply_active_arm(data) g = data.get("gripper") or {} return { @@ -362,7 +362,7 @@ def gripper_overrides(data: dict) -> dict: def perception_overrides(data: dict) -> dict: - """ROS parameter dict for perception_node.""" + """供 perception_node 使用的 ROS 参数字典。""" data, _robot = apply_active_arm(data) paths = data.get("paths") or {} yolo = data.get("yolo") or {} @@ -415,7 +415,7 @@ def perception_overrides(data: dict) -> dict: def servo_overrides(data: dict) -> dict: - """ROS parameter dict for servo_node.""" + """供 servo_node 使用的 ROS 参数字典。""" data, robot = apply_active_arm(data) paths = data.get("paths") or {} yolo = data.get("yolo") or {} @@ -431,7 +431,7 @@ def servo_overrides(data: dict) -> dict: "arm_label": str(robot.get("label") or robot.get("arm") or ""), "z_ref_m": float(servo.get("z_ref_m") or yolo.get("z_ref_m") or 0.40), } - # Pass through remaining servo keys that match node params + # 透传其余与节点参数同名的 servo 键 keys = [ "control_hz", "watchdog_timeout_sec", "lost_timeout_sec", "stale_hold_sec", "max_extrapolate_sec", "predict_enable", "predict_sec", @@ -460,7 +460,7 @@ def main() -> int: ap.add_argument("--json", action="store_true") ap.add_argument("--print", action="store_true", dest="do_print") ap.add_argument("--config", default="") - ap.add_argument("--sn", "--arm", dest="sn", default="", help="89|91|left|right|full SN") + ap.add_argument("--sn", "--arm", dest="sn", default="", help="89|91|left|right|完整 SN") args = ap.parse_args() if args.sn: os.environ["ROBOT_SN"] = str(args.sn) @@ -471,35 +471,35 @@ def main() -> int: print(json.dumps({"site": data, "env": flat}, ensure_ascii=False, indent=2)) return 0 if args.do_print: - print(f"site config: {path}") + print(f"site 配置: {path}") print( - f" robot SN : {flat['SITE_ROBOT_SN']}" + f" 机械臂 SN : {flat['SITE_ROBOT_SN']}" f"{(' (' + flat['SITE_ROBOT_LABEL'] + ')') if flat.get('SITE_ROBOT_LABEL') else ''}" f" arm={flat.get('SITE_ROBOT_ARM') or '?'}" ) - print(f" catalog : {flat.get('SITE_ROBOT_CATALOG') or '(none)'}") - print(f" robot IP : {flat['SITE_ROBOT_IP']} pc={flat['SITE_PC_IP']} iface={flat.get('SITE_NET_IFACE') or '?'}") - print(f" frames : prefix={flat['SITE_JOINT_PREFIX']} ee={flat['SITE_EE_FRAME']}") - print(f" tool name : {flat.get('SITE_ROBOT_TOOL_NAME') or '(keep current)'}") + print(f" 清单 : {flat.get('SITE_ROBOT_CATALOG') or '（无）'}") + print(f" 机械臂 IP : {flat['SITE_ROBOT_IP']} pc={flat['SITE_PC_IP']} iface={flat.get('SITE_NET_IFACE') or '?'}") + print(f" 坐标系 : prefix={flat['SITE_JOINT_PREFIX']} ee={flat['SITE_EE_FRAME']}") + print(f" 工具名 : {flat.get('SITE_ROBOT_TOOL_NAME') or '（保持当前）'}") catalog_sns = [str(r.get("sn") or "") for r in list_robots(data)] if catalog_sns and flat["SITE_ROBOT_SN"] not in catalog_sns: print( - f" WARNING: robot.sn {flat['SITE_ROBOT_SN']!r} is not in robots[]", + f" 警告：robot.sn {flat['SITE_ROBOT_SN']!r} 不在 robots[] 里", file=sys.stderr, ) - print(f" yolo weights : {flat['SITE_YOLO_WEIGHTS']}") - print(f" resolved : {flat['SITE_YOLO_WEIGHTS_RESOLVED']}") - print(f" handeye : {flat['SITE_HANDEYE_FILE']}") - print(f" handeye wp : {flat['SITE_HANDEYE_WAYPOINTS']}") - print(f" handeye dir : {flat['SITE_HANDEYE_WORKDIR']}") - print(f" named poses : {flat.get('SITE_NAMED_POSES_FILE')}") + print(f" YOLO 权重 : {flat['SITE_YOLO_WEIGHTS']}") + print(f" 解析后 : {flat['SITE_YOLO_WEIGHTS_RESOLVED']}") + print(f" 手眼 : {flat['SITE_HANDEYE_FILE']}") + print(f" 手眼路点 : {flat['SITE_HANDEYE_WAYPOINTS']}") + print(f" 手眼目录 : {flat['SITE_HANDEYE_WORKDIR']}") + print(f" 命名位姿 : {flat.get('SITE_NAMED_POSES_FILE')}") print( - " board : " + " 标定板 : " f"{flat['SITE_HANDEYE_BOARD_COLS']}x{flat['SITE_HANDEYE_BOARD_ROWS']} " - f"inner, square={flat['SITE_HANDEYE_SQUARE_M']} m" + f"内角点, square={flat['SITE_HANDEYE_SQUARE_M']} m" ) print( - f" camera : serial={flat['SITE_CAM_SERIAL'] or '(empty)'} " + f" 相机 : serial={flat['SITE_CAM_SERIAL'] or '（空）'} " f"{flat['SITE_CAM_WIDTH']}x{flat['SITE_CAM_HEIGHT']}@{flat['SITE_CAM_FPS']} " f"ae={flat.get('SITE_CAM_AUTO_EXPOSURE')} " f"exp={flat.get('SITE_CAM_EXPOSURE_US')}us " @@ -507,26 +507,26 @@ def main() -> int: f"ae_limit={flat.get('SITE_CAM_AE_LIMIT_US')}us emitter={flat.get('SITE_CAM_EMITTER')}" ) print( - f" catalog cam : leftD405={flat.get('SITE_D405_LEFT_SERIAL')} " + f" 清单相机 : leftD405={flat.get('SITE_D405_LEFT_SERIAL')} " f"rightD405={flat.get('SITE_D405_RIGHT_SERIAL')} " f"D435={flat.get('SITE_D435_SERIAL')}" ) print( - f" gripper : {flat.get('SITE_GRIP_MODEL')} port={flat.get('SITE_GRIP_PORT')}" + f" 夹爪 : {flat.get('SITE_GRIP_MODEL')} port={flat.get('SITE_GRIP_PORT')}" ) print( f" hole_detect : backend={flat.get('SITE_HOLE_BACKEND')} " f"weights={flat.get('SITE_HOLE_WEIGHTS')}" ) print( - f" tactile : backend={flat.get('SITE_TACTILE_BACKEND')} " + f" 触觉 : backend={flat.get('SITE_TACTILE_BACKEND')} " f"L={flat.get('SITE_TACTILE_LEFT')} R={flat.get('SITE_TACTILE_RIGHT')} " f"cfg={flat.get('SITE_TACTILE_CONFIG')}" ) print(f" go_home : pose={flat['SITE_GO_HOME_POSE']} speed={flat['SITE_GO_HOME_SPEED']}") he_path = Path(flat["SITE_HANDEYE_FILE"]).expanduser() if not he_path.is_file(): - print(f" WARNING: hand-eye file missing: {he_path}", file=sys.stderr) + print(f" 警告：手眼文件缺失: {he_path}", file=sys.stderr) else: try: he_data = yaml.safe_load(he_path.read_text(encoding="utf-8")) or {} @@ -536,28 +536,28 @@ def main() -> int: he_data.get("uncalibrated") or "T_4x4" not in he_data ): print( - f" WARNING: {he_path.name} is not calibrated for this arm " + f" 警告：{he_path.name} 没有针对该机械臂标定 " f"(run_handeye.sh teach/calibrate --arm {flat.get('SITE_ROBOT_ARM') or '91'})", file=sys.stderr, ) if "063991" in flat["SITE_ROBOT_SN"] and flat.get("SITE_CAM_SERIAL") == flat.get("SITE_D405_LEFT_SERIAL"): print( - " WARNING: 91 is using the LEFT wrist D405 serial — check robots[].camera.serial", + " 警告：91 用的是左腕 D405 的序列号 — 请检查 robots[].camera.serial", file=sys.stderr, ) if "RGI" in str(flat.get("SITE_GRIP_MODEL") or ""): print( - " NOTE: 本臂是 RGI。CLI: bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open|rotate", + " 提示：本臂是 RGI。CLI: bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open|rotate", file=sys.stderr, ) w = Path(flat["SITE_YOLO_WEIGHTS"]).expanduser() if not w.is_file(): - print(f" WARNING: weights file missing: {w}", file=sys.stderr) + print(f" 警告：权重文件缺失: {w}", file=sys.stderr) return 2 return 0 - # bash exports + # bash 导出 for k, v in flat.items(): - # escape for eval + # 为 eval 转义 vv = str(v).replace("'", "'\"'\"'") print(f"export {k}='{vv}'") return 0 diff --git a/scripts/lib/load_site_config.sh b/scripts/lib/load_site_config.sh index 355d4b3..7d16562 100755 --- a/scripts/lib/load_site_config.sh +++ b/scripts/lib/load_site_config.sh @@ -1,6 +1,6 @@ #!/usr/bin/env bash -# Load ~/ros2/config/site.yaml into SITE_* environment variables. -# Usage (from other scripts): +# 把 ~/ros2/config/site.yaml 加载成 `SITE_*` 环境变量。 +# 用法（在其它脚本里）： # source "${ROOT}/scripts/lib/load_site_config.sh" _SITE_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" @@ -13,7 +13,7 @@ else # shellcheck disable=SC1090 eval "$(python3 "${_SITE_EXPORTER}" --config "${_SITE_FILE}")" export SITE_CONFIG="${_SITE_FILE}" - # Resolve 89/91/left/right aliases to the full Flexiv SN for RDK. + # 把 89/91/left/right 别名解析成完整的 Flexiv SN，供 RDK 使用。 if [[ -n "${SITE_ROBOT_SN:-}" ]]; then export ROBOT_SN="${SITE_ROBOT_SN}" fi diff --git a/scripts/lib/parse_arm_args.sh b/scripts/lib/parse_arm_args.sh index a499b7a..a7478ae 100644 --- a/scripts/lib/parse_arm_args.sh +++ b/scripts/lib/parse_arm_args.sh @@ -1,12 +1,12 @@ #!/usr/bin/env bash -# Parse --sn / --arm from argv. Must be sourced BEFORE load_site_config.sh -# so ROBOT_SN is visible to export_site_env.py. +# 从 argv 里解析 --sn / --arm。必须在 `load_site_config.sh` 之前 source， +# 这样 `ROBOT_SN` 才能被 export_site_env.py 看到。 # # source "${ROOT}/scripts/lib/parse_arm_args.sh" # parse_arm_args "$@" # set -- "${PARSE_ARM_REST[@]}" # -# Aliases (89|91|left|right) are resolved in export_site_env.py. +# 别名（89|91|left|right）在 export_site_env.py 里解析。 parse_arm_args() { PARSE_ARM_REST=() diff --git a/scripts/list_realsense.sh b/scripts/list_realsense.sh index e914a0b..e8d7005 100755 --- a/scripts/list_realsense.sh +++ b/scripts/list_realsense.sh @@ -1,5 +1,5 @@ #!/usr/bin/env bash -# List plugged RealSense cameras (serials used in site.yaml). +# 列出已插上的 RealSense 相机（序列号在 site.yaml 里使用）。 set -eo pipefail python3 - <<'PY' import pyrealsense2 as rs diff --git a/scripts/merge_handeye_poses.py b/scripts/merge_handeye_poses.py index c071aed..4552e01 100755 --- a/scripts/merge_handeye_poses.py +++ b/scripts/merge_handeye_poses.py @@ -73,7 +73,7 @@ def format_positions_block(poses: list[list[float]], indent: str = " ") -> st def merge(yaml_path: Path, poses: list[list[float]]) -> None: text = yaml_path.read_text(encoding="utf-8").replace("\r\n", "\n").replace("\r", "\n") - # Detect indent of waypoint_positions (2 or 4 spaces typical) + # 探测 waypoint_positions 的缩进（通常 2 空格或 4 空格） m_ind = re.search(r"^([ \t]*)waypoint_positions:\s*\[", text, re.MULTILINE) indent = m_ind.group(1) if m_ind else " " block = format_positions_block(poses, indent=indent) @@ -82,7 +82,7 @@ def merge(yaml_path: Path, poses: list[list[float]]) -> None: rf"{re.escape(indent)}waypoint_positions:\s*\[[\s\S]*?{re.escape(indent)}\]", re.MULTILINE, ) - # Also match a one-line empty list: waypoint_positions: [] + # 也要匹配"单行空列表"的写法：waypoint_positions: [] one_line = re.compile( rf"{re.escape(indent)}waypoint_positions:\s*\[\s*\]", re.MULTILINE, @@ -94,7 +94,7 @@ def merge(yaml_path: Path, poses: list[list[float]]) -> None: else: raise RuntimeError("waypoint_positions block not found in YAML") - # Keep num_poses in sync when present + # 顺手把 num_poses 同步成实际点数（如果原本就有这个键） n = len(poses) if re.search(r"^\s*num_poses:\s*\d+", new_text, re.MULTILINE): new_text = re.sub( @@ -105,7 +105,7 @@ def merge(yaml_path: Path, poses: list[list[float]]) -> None: flags=re.MULTILINE, ) else: - # Insert num_poses just before waypoint_positions + # 原本没有 num_poses，就插在 waypoint_positions 前面 new_text = new_text.replace( block, f"{indent}num_poses: {n}\n{block}", diff --git a/scripts/rdk_drag_bridge.py b/scripts/rdk_drag_bridge.py index 5a70a61..7d30008 100755 --- a/scripts/rdk_drag_bridge.py +++ b/scripts/rdk_drag_bridge.py @@ -105,13 +105,13 @@ class SoftDrag: robot.SetJointImpedance(g, soft) log(f'SetJointImpedance {g} K={soft}') except Exception as exc: - log(f'SetJointImpedance group {g}: {exc}') + log(f'SetJointImpedance 组 {g}: {exc}') else: n = max(7, len(self.latest_q)) try: robot.SetJointImpedance([35.0] * n, [0.7] * n) except Exception as exc: - log(f'SetJointImpedance flat: {exc}') + log(f'SetJointImpedance flat（不分组）: {exc}') self._stop.clear() self._thread = threading.Thread(target=self._loop, name='soft-drag', daemon=True) @@ -144,9 +144,9 @@ class SoftDrag: n = len(q) robot.SendJointPosition(q, [0.0] * n, [1.5] * n, [2.0] * n) except Exception as exc: - log(f'soft loop: {exc}') + log(f'soft 循环: {exc}') time.sleep(0.15) - # ~20 Hz — keep lock released most of the time + # 约 20 Hz —— 大部分时间让锁保持释放状态 for _ in range(10): if self._stop.is_set(): return @@ -158,7 +158,7 @@ class SoftDrag: if th is not None: th.join(timeout=4.0) if th.is_alive(): - log('soft loop still alive after join (will idle anyway)') + log('join 之后 soft 循环仍然存活（无论如何都会切到 IDLE）') self._thread = None @@ -168,7 +168,7 @@ def _ensure_operational(robot) -> Optional[str]: def _idle_async(robot, lock: threading.Lock, flexivrdk, timeout: float = 4.0) -> bool: - """Switch to IDLE without blocking the stdin loop forever.""" + """切到 IDLE，同时避免把 stdin 循环永久阻塞住。""" done = threading.Event() def run() -> None: @@ -193,7 +193,7 @@ def _idle_async(robot, lock: threading.Lock, flexivrdk, timeout: float = 4.0) -> threading.Thread(target=run, name='idle', daemon=True).start() ok = done.wait(timeout) if not ok: - log('IDLE/Stop timed out — stdin loop continues') + log('IDLE/Stop 超时 —— stdin 循环继续运行') return ok @@ -213,7 +213,7 @@ def main() -> int: try: msg = json.loads(line) except Exception as exc: # noqa: BLE001 - reply({'ok': False, 'error': f'bad json: {exc}'}) + reply({'ok': False, 'error': f'JSON 解析失败: {exc}'}) continue cmd = str(msg.get('cmd', '')).strip() @@ -233,17 +233,17 @@ def main() -> int: for args in attempts: try: robot = flexivrdk.Robot(*args) - log(f'connected {sn} args={args!r}') + log(f'已连接 {sn} args={args!r}') last_err = None break except TypeError: continue except Exception as exc: last_err = exc - log(f'connect {args!r}: {exc}') + log(f'连接 {args!r}: {exc}') robot = None if robot is None: - reply({'ok': False, 'error': str(last_err or 'connect failed')}) + reply({'ok': False, 'error': str(last_err or '连接失败')}) continue err = _ensure_operational(robot) if err: @@ -256,7 +256,7 @@ def main() -> int: elif cmd == 'drag_on': if robot is None: - reply({'ok': False, 'error': 'not connected'}) + reply({'ok': False, 'error': '尚未连接'}) continue err = _ensure_operational(robot) if err: @@ -274,7 +274,7 @@ def main() -> int: reply({'ok': True, 'drag': True, 'mode': 'floating', 'primitive': name}) continue except Exception as exc: - log(f'Floating failed, fallback soft: {exc}') + log(f'Floating 失败，退回 soft 模式: {exc}') try: if soft is not None: @@ -284,18 +284,18 @@ def main() -> int: if soft.latest_q: cached_q = list(soft.latest_q) drag_on = True - log('soft drag ON') + log('软拖拽已开启') reply({ 'ok': True, 'drag': True, 'mode': 'soft', - 'hint': 'Push the arm gently; low joint impedance. Enter=photo (s=hold).', + 'hint': '轻推机械臂即可，关节阻抗已调低。Enter=拍照（s=保持）。', }) except Exception as exc: - reply({'ok': False, 'error': f'soft drag failed: {exc}'}) + reply({'ok': False, 'error': f'软拖拽失败: {exc}'}) elif cmd == 'drag_off': - log('drag_off requested') + log('收到 drag_off 请求') if soft is not None: if soft.latest_q: cached_q = list(soft.latest_q) @@ -304,7 +304,7 @@ def main() -> int: if robot is not None: _idle_async(robot, rlock, flexivrdk, timeout=4.0) drag_on = False - log('drag_off done') + log('drag_off 已完成') reply({'ok': True, 'drag': False, 'q': cached_q}) elif cmd == 'joints': @@ -314,14 +314,14 @@ def main() -> int: elif len(cached_q) >= 7: q = list(cached_q) if q is None or len(q) < 7: - reply({'ok': False, 'error': 'no joint cache'}) + reply({'ok': False, 'error': '没有关节缓存'}) continue cached_q = q reply({'ok': True, 'q': q, 'drag': drag_on}) elif cmd == 'enable': if robot is None: - reply({'ok': False, 'error': '尚未 connect'}) + reply({'ok': False, 'error': '尚未连接'}) continue err = ensure_operational(robot) reply({ @@ -332,7 +332,7 @@ def main() -> int: elif cmd == 'clear_fault': if robot is None: - reply({'ok': False, 'error': '尚未 connect'}) + reply({'ok': False, 'error': '尚未连接'}) continue err = rdk_clear_fault(robot) if err: @@ -349,7 +349,7 @@ def main() -> int: elif cmd == 'switch_tool': if robot is None: - reply({'ok': False, 'error': '尚未 connect'}) + reply({'ok': False, 'error': '尚未连接'}) continue want = str(msg.get('name', '')).strip() @@ -428,9 +428,9 @@ def main() -> int: return 0 else: - reply({'ok': False, 'error': f'unknown cmd {cmd}'}) + reply({'ok': False, 'error': f'未知命令 {cmd}'}) except Exception as exc: # noqa: BLE001 - log(f'cmd={cmd} error: {exc}') + log(f'cmd={cmd} 出错: {exc}') reply({'ok': False, 'error': str(exc)}) return 0 diff --git a/scripts/rdk_gripper.py b/scripts/rdk_gripper.py index 81d420f..38696dd 100755 --- a/scripts/rdk_gripper.py +++ b/scripts/rdk_gripper.py @@ -1,10 +1,10 @@ #!/usr/bin/env python3 -"""Control Studio device GripperDahuan / GripperDahuanModbus via Flexiv RDK. +"""通过 Flexiv RDK 控制 Studio 设备 GripperDahuan / GripperDahuanModbus。 -Must run under RDK Python (rdk_py310). Uses a *lite* RDK instance so it can -share the robot with run_real.sh (ros2_control holds the normal RDK). +必须在 RDK 的 Python（rdk_py310）下运行。使用 *lite*（轻量）RDK 实例， +以便和 run_real.sh 共用机器人（常规 RDK 被 ros2_control 占用）。 -The controller talks RS485 itself (its own /dev/ttyUSB0), not the IPC serial. +控制器自己走 RS485（用自带的 /dev/ttyUSB0），不走工控机的串口。 /home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py --list /home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py status @@ -65,28 +65,28 @@ def _move(grip, params, width: float, vel: float, force: float) -> None: vmax = float(params.max_vel) if vmax <= 0: vmax = 0.05 - # Move velocity cannot be 0 (RDK). + # 移动速度不能为 0（RDK 限制）。 vel = _clamp(vel, max(vmin, 1e-4), vmax) force = _clamp(force, float(params.min_force), float(params.max_force)) width = _clamp(width, float(params.min_width), float(params.max_width)) - print(f"Move width={width:.4f} m vel={vel:.4f} m/s force={force:.1f} N") + print(f"移动 width={width:.4f} m vel={vel:.4f} m/s force={force:.1f} N") grip.Move(width, vel, force) def main() -> int: - ap = argparse.ArgumentParser(description="Flexiv RDK gripper (GripperDahuanModbus)") + ap = argparse.ArgumentParser(description="Flexiv RDK 夹爪（GripperDahuanModbus）") ap.add_argument("--sn", default="") ap.add_argument("--iface", default="") - ap.add_argument("--device", default="", help="Studio Device name") - ap.add_argument("--force-n", type=float, default=0.0, help="force limit [N]; 0=mid") - ap.add_argument("--vel", type=float, default=0.0, help="speed [m/s]; 0=half max") + ap.add_argument("--device", default="", help="Studio 设备名") + ap.add_argument("--force-n", type=float, default=0.0, help="力上限 [N]；0=取中值") + ap.add_argument("--vel", type=float, default=0.0, help="速度 [m/s]；0=最大速度的一半") ap.add_argument( "cmd", nargs="?", default="status", choices=["list", "status", "init", "open", "close", "pos", "enable"], ) - ap.add_argument("value", nargs="?", default="", help="pos: meters or 0–1000") + ap.add_argument("value", nargs="?", default="", help="pos: 米，或 0–1000") args = ap.parse_args() sys.path.insert(0, str(ROOT / "scripts" / "lib")) @@ -99,18 +99,18 @@ def main() -> int: iface = (args.iface or robot_cfg.get("iface") or "").strip() device = (args.device or g.get("device_name") or "GripperDahuanModbus").strip() if not sn: - print("ERROR: robot SN missing", file=sys.stderr) + print("错误：没拿到机械臂序列号（SN）", file=sys.stderr) return 2 import flexivrdk - print(f"=== RDK lite gripper SN={sn} device={device} iface={iface or 'auto'} ===") + print(f"=== RDK lite 夹爪 SN={sn} device={device} iface={iface or 'auto'} ===") robot = connect_lite(sn, iface) dev = flexivrdk.Device(robot) names = list(dev.list() or []) if args.cmd == "list": - print("Studio devices:") + print("Studio 设备列表:") for n in names: try: en = dev.enabled(n) @@ -123,7 +123,7 @@ def main() -> int: if device not in names: print( - f"ERROR: device {device!r} not in Studio pool.\n available: {', '.join(names)}", + f"错误：设备 {device!r} 不在 Studio 设备池里。\n 可用: {', '.join(names)}", file=sys.stderr, ) return 1 @@ -133,7 +133,7 @@ def main() -> int: ) if not dev.connected(device): print( - "WARN: connected=False — 控制器自己的 /dev/ttyUSB0 没接到爪。\n" + "警告：connected=False —— 控制器自己的 /dev/ttyUSB0 没接到夹爪。\n" " USB–RS485 要插 Flexiv 机箱 USB（不是工控机），A/B + 24V 到 PGEA。\n" " 或用法兰工具口 485（腕部白绿+/绿-）。" ) @@ -142,14 +142,14 @@ def main() -> int: if args.cmd in ("enable", "status", "init", "open", "close", "pos"): try: grip.Enable(device) - print(f"Enable [{device}] OK") + print(f"使能 [{device}] 成功") except Exception as exc: - print(f"ERROR: Enable failed: {exc}", file=sys.stderr) + print(f"错误：使能失败: {exc}", file=sys.stderr) return 1 params = grip.params() - print(" params:", _fmt_params(params)) - print(" states:", _fmt_states(grip.states())) + print(" 参数:", _fmt_params(params)) + print(" 状态:", _fmt_states(grip.states())) force = float(args.force_n or g.get("force_n") or 0.0) if force <= 0: @@ -164,27 +164,27 @@ def main() -> int: return 0 if args.cmd == "init": - print("Init (fingers home, ~10 s) …") + print("初始化（手指回零，约 10 秒）…") grip.Init() time.sleep(2.0) - print(" states:", _fmt_states(grip.states())) + print(" 状态:", _fmt_states(grip.states())) return 0 if args.cmd == "open": _move(grip, params, float(params.max_width), vel, force) time.sleep(0.4) - print(" states:", _fmt_states(grip.states())) + print(" 状态:", _fmt_states(grip.states())) return 0 if args.cmd == "close": _move(grip, params, float(params.min_width), vel, force) time.sleep(0.4) - print(" states:", _fmt_states(grip.states())) + print(" 状态:", _fmt_states(grip.states())) return 0 if args.cmd == "pos": if args.value == "": - print("usage: pos ", file=sys.stderr) + print("用法: pos <米|0-1000>", file=sys.stderr) return 2 raw = float(args.value) if raw > 1.5: @@ -194,7 +194,7 @@ def main() -> int: width = raw _move(grip, params, width, vel, force) time.sleep(0.4) - print(" states:", _fmt_states(grip.states())) + print(" 状态:", _fmt_states(grip.states())) return 0 return 0 @@ -203,5 +203,5 @@ if __name__ == "__main__": try: raise SystemExit(main()) except Exception as exc: - print(f"ERROR: {exc}", file=sys.stderr) + print(f"错误：{exc}", file=sys.stderr) raise SystemExit(1) diff --git a/scripts/rdk_switch_tool.py b/scripts/rdk_switch_tool.py index dbbb41c..bb14e17 100755 --- a/scripts/rdk_switch_tool.py +++ b/scripts/rdk_switch_tool.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 -"""Switch the Flexiv active tool (TCP + payload) to a Studio-configured name. +"""把 Flexiv 当前工具（TCP + 负载）切换成 Studio 里配置好的名字。 -Must run under RDK Python (rdk_py310). Robot must be IDLE; stop run_real.sh first -if ros2_control already owns the RDK connection. +必须在 RDK 的 Python（rdk_py310）下运行。机器人必须处于 IDLE；如果 +ros2_control 已经占住了 RDK 连接，请先停掉 run_real.sh。 /home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_switch_tool.py --list /home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_switch_tool.py --name Flange @@ -17,7 +17,7 @@ import time def _single_arm_groups(robot): - """RDK 1.9 Tool API is single-arm (no JointGroup). Older builds use groups.""" + """RDK 1.9 的 Tool API 是单臂的（没有 JointGroup）。老版本用 groups。""" info = robot.info() groups = getattr(info, "single_arm_groups", None) or {} if isinstance(groups, dict): @@ -33,7 +33,7 @@ def _single_arm_groups(robot): return [flexivrdk.JointGroup.ARM_1] except Exception: pass - return [None] # sentinel: call Tool.name()/Switch() without a group + return [None] # 哨兵：调用 Tool.name()/Switch() 时不带 group def _tool_name(tool, group): @@ -57,7 +57,7 @@ def _tool_switch(tool, group, target: str) -> None: def _group_name(group) -> str: if group is None: - return "arm" + return "单臂" try: import flexivrdk @@ -91,7 +91,7 @@ def _ensure_idle(robot, mode) -> None: time.sleep(0.25) if not robot.operational(): raise RuntimeError( - "robot not operational — AUTO Remote + Enable, release E-stop" + "机器人未处于 operational 状态 —— 请在示教器切 AUTO Remote 并使能，松开急停" ) robot.SwitchMode(mode.IDLE) @@ -108,13 +108,13 @@ def _resolve_name(tool, want: str) -> str: if hit: return hit raise SystemExit( - "tool %r not found in Studio pool.\n available: %s" - % (want, ", ".join(names) or "(empty)") + "工具 %r 不在 Studio 工具池里。\n 可选: %s" + % (want, ", ".join(names) or "（空）") ) def _iface_ipv4s(name: str) -> list: - """FastDDS whitelist wants IPv4 addresses, not NIC names like enp4s0.""" + """FastDDS 白名单要的是 IPv4 地址，而不是 enp4s0 这类网卡名。""" import socket import fcntl import struct @@ -147,7 +147,7 @@ def _iface_ipv4s(name: str) -> list: addrs.append(cidr.split("/")[0]) except Exception: pass - # unique, keep order + # 去重，保持原有顺序 seen = set() uniq = [] for a in addrs: @@ -170,7 +170,7 @@ def _rdk_whitelist(iface: str) -> list[list[str]]: robot_lan = [a for a in _iface_ipv4s(iface) if a.startswith("192.168.2.")] if robot_lan: lists.append(robot_lan) - # unique + # 去重 seen: set[tuple[str, ...]] = set() out: list[list[str]] = [] for item in lists: @@ -185,7 +185,7 @@ def connect_rdk(sn: str, iface: str = ""): import flexivrdk for listen in _rdk_whitelist(iface): - print("RDK whitelist:", ", ".join(listen)) + print("RDK 白名单:", ", ".join(listen)) for args in ((sn, listen), (sn, listen, True), (sn, listen, True, False)): try: return flexivrdk.Robot(*args) @@ -194,51 +194,51 @@ def connect_rdk(sn: str, iface: str = ""): except RuntimeError as exc: msg = str(exc) if "whitelist" in msg.lower() or "filtered out" in msg.lower(): - print(" whitelist rejected, try next") + print(" 白名单被拒，试下一个") break if "Failed to connect" in msg or "failed to connect" in msg.lower(): - print(" search timeout with this whitelist") + print(" 用这个白名单搜索超时") break raise - print("retry without iface filter") + print("不带 iface 过滤再试一次") return flexivrdk.Robot(sn) def main() -> int: - ap = argparse.ArgumentParser(description="Switch Flexiv Studio tool / TCP") - ap.add_argument("--sn", default="", help="robot serial (default: site.yaml)") - ap.add_argument("--iface", default="", help="NIC whitelist for RDK (e.g. enp4s0)") + ap = argparse.ArgumentParser(description="切换 Flexiv Studio 工具 / TCP") + ap.add_argument("--sn", default="", help="机械臂序列号（默认取 site.yaml）") + ap.add_argument("--iface", default="", help="RDK 用的网卡白名单（如 enp4s0）") ap.add_argument( "--name", default="", - help="Studio tool name (Flange = no tool). Empty = site.yaml robot.tool_name", + help="Studio 工具名（Flange = 不装工具）。留空 = 用 site.yaml 的 robot.tool_name", ) - ap.add_argument("--list", action="store_true", help="list tools and exit") + ap.add_argument("--list", action="store_true", help="列出工具后退出") args = ap.parse_args() sn = (args.sn or "").strip() want = (args.name or "").strip() if not sn: - print("ERROR: --sn is required (run via run_real.sh / rdk_switch_tool.sh)", file=sys.stderr) + print("错误：必须提供 --sn（请通过 run_real.sh / rdk_switch_tool.sh 运行）", file=sys.stderr) return 2 import flexivrdk mode = flexivrdk.Mode iface = (args.iface or "").strip() - print(f"=== RDK tool SN={sn} iface={iface or 'auto'} ===") + print(f"=== RDK 工具 SN={sn} iface={iface or 'auto'} ===") robot = None try: robot = connect_rdk(sn, iface) if robot.fault(): - print("fault present, ClearFault ...") + print("检测到故障，正在 ClearFault ...") if not robot.ClearFault(): - print("ERROR: fault cannot be cleared", file=sys.stderr) + print("错误：故障无法清除", file=sys.stderr) return 1 tool = flexivrdk.Tool(robot) names = list(tool.list() or []) groups = _single_arm_groups(robot) - print("Studio tools:", ", ".join(names) or "(none)") + print("Studio 工具:", ", ".join(names) or "（无）") for g in groups: cur = _tool_name(tool, g) extra = "" @@ -246,23 +246,23 @@ def main() -> int: extra = " " + _fmt_params(_tool_params(tool, g)) except Exception: pass - print(f" active [{_group_name(g)}]: {cur}{extra}") + print(f" 当前 [{_group_name(g)}]: {cur}{extra}") if args.list: return 0 if not want: - print("tool_name empty — keep current (set robot.tool_name in site.yaml to switch)") - print("RDK OK") + print("tool_name 为空 —— 保持当前工具（要切换请在 site.yaml 里设置 robot.tool_name）") + print("RDK 正常") return 0 target = _resolve_name(tool, want) already = all(_tool_name(tool, g) == target for g in groups) if already: - print(f"already on [{target}]") - print("RDK OK") + print(f"已经处于 [{target}]") + print("RDK 正常") return 0 - print(f"switching → [{target}] (requires IDLE)") + print(f"正在切换到 → [{target}]（需要 IDLE）") _ensure_idle(robot, mode) for g in groups: _tool_switch(tool, g, target) @@ -273,16 +273,16 @@ def main() -> int: extra = " " + _fmt_params(_tool_params(tool, g)) except Exception: pass - print(f" now [{_group_name(g)}]: {cur}{extra}") + print(f" 现在 [{_group_name(g)}]: {cur}{extra}") if cur != target: - print(f"ERROR: switch failed, still {cur!r}", file=sys.stderr) + print(f"错误：切换失败，当前仍是 {cur!r}", file=sys.stderr) return 1 - print("RDK OK") + print("RDK 正常") return 0 except Exception as exc: - print(f"ERROR: {exc}", file=sys.stderr) - print(" Stop run_real.sh first (RDK is exclusive).", file=sys.stderr) - print(" Pendant: AUTO Remote + Enable Ethernet/RDK.", file=sys.stderr) + print(f"错误：{exc}", file=sys.stderr) + print(" 请先停掉 run_real.sh（RDK 是独占的）。", file=sys.stderr) + print(" 示教器：切 AUTO Remote，并使能 Ethernet/RDK。", file=sys.stderr) return 1 finally: robot = None diff --git a/scripts/rdk_switch_tool.sh b/scripts/rdk_switch_tool.sh index 9a0f370..8462c30 100755 --- a/scripts/rdk_switch_tool.sh +++ b/scripts/rdk_switch_tool.sh @@ -1,11 +1,11 @@ #!/usr/bin/env bash -# Switch / list Flexiv Studio tools via RDK (reads name from site.yaml). +# 通过 RDK 切换 / 列出 Flexiv Studio 工具（工具名从 site.yaml 读取）。 # -# bash ~/ros2/scripts/rdk_switch_tool.sh # switch to robot.tool_name -# bash ~/ros2/scripts/rdk_switch_tool.sh --list # print Studio tool pool +# bash ~/ros2/scripts/rdk_switch_tool.sh # 切换到 robot.tool_name +# bash ~/ros2/scripts/rdk_switch_tool.sh --list # 打印 Studio 工具池 # bash ~/ros2/scripts/rdk_switch_tool.sh --name Flange # -# Stop run_real.sh first — RDK allows only one client. +# 先停掉 `run_real.sh` —— RDK 只允许一个客户端。 set -eo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" diff --git a/scripts/run_camera_d405.sh b/scripts/run_camera_d405.sh index 5399849..6f6fd90 100755 --- a/scripts/run_camera_d405.sh +++ b/scripts/run_camera_d405.sh @@ -1,17 +1,17 @@ #!/usr/bin/env bash -# Terminal: D405 via official pyrealsense2 SDK — color + depth + stereo IR topics. -# With --preview the Qt window runs in a SEPARATE viewer process (realsense_view) -# so painting never slows the camera stream. Closing the window ≠ stop camera. -# Do NOT start view_realsense_depth.sh at the same time (USB is exclusive). +# 终端方式运行 D405：走官方 pyrealsense2 SDK —— 发布彩色 + 深度 + 双目 IR 话题。 +# 带 --preview 时，Qt 窗口跑在**独立的**查看器进程（`realsense_view`）里， +# 这样绘制界面永远不会拖慢相机数据流。关掉窗口 ≠ 停止相机。 +# 不要同时启动 `view_realsense_depth.sh`（USB 是独占的）。 # -# Usage: +# 用法： # bash ~/ros2/scripts/run_camera_d405.sh # bash ~/ros2/scripts/run_camera_d405.sh --preview # bash ~/ros2/scripts/run_camera_d405.sh --arm 91 --preview set -eo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" -PREVIEW=-1 # -1 = use site; 0/1 = force +PREVIEW=-1 # -1 = 沿用 site；0/1 = 强制 HIGH_FPS=0 # shellcheck disable=SC1091 @@ -143,8 +143,8 @@ fi echo "=== next: bash ~/ros2/scripts/run_visual_servo_rt.sh --arm ${SITE_ROBOT_ARM:-89} [--control] ===" if [[ "${DO_PREVIEW}" == "1" ]]; then - # Camera publishes only; the Qt viewer runs in its own process so painting - # can never starve the capture loop (shared GIL cost ~70 ms/frame before). + # 相机进程只负责发布；Qt 查看器跑在自己的进程里，所以界面绘制 + # 永远不会把采集循环饿死（以前共用 GIL 时每帧要花约 70 ms）。 pkill -f 'lib/arm_application/realsense_view' 2>/dev/null || true ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false & CAM_PID=$! diff --git a/scripts/run_camera_d435.sh b/scripts/run_camera_d435.sh index ec19f92..7380ecd 100755 --- a/scripts/run_camera_d435.sh +++ b/scripts/run_camera_d435.sh @@ -1,5 +1,5 @@ #!/usr/bin/env bash -# Overhead D435 between the two arms (eye-to-hand). Does not open either D405. +# 两臂之间的顶置 D435（`eye-to-hand`，相机固定在外）。不会打开任何一个 D405。 # # bash ~/ros2/scripts/run_camera_d435.sh --preview # diff --git a/scripts/run_display.sh b/scripts/run_display.sh index 96ad148..1594a38 100755 --- a/scripts/run_display.sh +++ b/scripts/run_display.sh @@ -1,5 +1,5 @@ #!/usr/bin/env bash -# View URDF only (joint_state_publisher_gui + RViz) +# 只看 URDF（joint_state_publisher_gui + RViz） set -euo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" # shellcheck disable=SC1091 diff --git a/scripts/run_dual_gripper.sh b/scripts/run_dual_gripper.sh index 7a05621..b31e436 100755 --- a/scripts/run_dual_gripper.sh +++ b/scripts/run_dual_gripper.sh @@ -1,8 +1,8 @@ #!/usr/bin/env bash -# Both DH grippers (left PGEA + right RGI) as one ROS 2 node. +# 把两个 DH 夹爪（左 PGEA + 右 RGI）作为同一个 ROS 2 节点启动。 # bash ~/ros2/scripts/run_dual_gripper.sh -# Topics: /dual_gripper_driver/left|right/{command,status} -# Services: /dual_gripper_driver/left|right/execute (arm_msgs/srv/GripperExecute) +# 话题：/dual_gripper_driver/left|right/{command,status} +# 服务：/dual_gripper_driver/left|right/execute (arm_msgs/srv/GripperExecute) set -eo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" diff --git a/scripts/run_gripper.sh b/scripts/run_gripper.sh index 0ef1c10..3d07144 100755 --- a/scripts/run_gripper.sh +++ b/scripts/run_gripper.sh @@ -1,8 +1,8 @@ #!/usr/bin/env bash -# DH grippers on the IPC (Modbus RTU). Not Flexiv Studio / RDK. +# 工控机上的 DH 夹爪（Modbus RTU）。不是 Flexiv Studio / RDK 那一套。 # -# Left 89 PGEA parallel /dev/ttyUSB0 -# Right 91 RGI rotary /dev/ttyUSB1 +# 左臂 89 PGEA 平行夹爪 /dev/ttyUSB0 +# 右臂 91 RGI 旋转夹爪 /dev/ttyUSB1 # # bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli probe # bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli init @@ -13,9 +13,9 @@ # bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli close # bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli rotate 90 # bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli revolutions 2 -# bash ~/ros2/scripts/run_gripper.sh --arm 89 # ROS node PGEA -# bash ~/ros2/scripts/run_gripper.sh --arm 91 # ROS node RGI -# bash ~/ros2/scripts/run_dual_gripper.sh # both ROS nodes +# bash ~/ros2/scripts/run_gripper.sh --arm 89 # ROS 节点 PGEA +# bash ~/ros2/scripts/run_gripper.sh --arm 91 # ROS 节点 RGI +# bash ~/ros2/scripts/run_dual_gripper.sh # 两个 ROS 节点都起 set -eo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" diff --git a/scripts/run_handeye.sh b/scripts/run_handeye.sh index 56b4ca1..0eb4adb 100755 --- a/scripts/run_handeye.sh +++ b/scripts/run_handeye.sh @@ -1,14 +1,14 @@ #!/usr/bin/env bash -# Hand-eye calibration helpers. -# Scripts: ~/ros2/scripts/run_handeye.sh -# Config & results: ~/ros2/handeye/ ← 见 handeye/README.md +# 手眼标定辅助脚本。 +# 脚本：~/ros2/scripts/run_handeye.sh +# 配置与结果：~/ros2/handeye/ ← 见 handeye/README.md # poses/<臂号>_<模式>.yaml 采点（输入） # results/<臂号>_<模式>.yaml 标定结果（输出） # calibrate.yaml 板尺寸 / 运动 / 稳定性参数 # tmp/ 临时产物 -# Site overrides: ~/ros2/config/site.yaml → handeye: +# 站点覆盖项：~/ros2/config/site.yaml → handeye: # -# Usage: +# 用法： # bash ~/ros2/scripts/run_handeye.sh calibrate # bash ~/ros2/scripts/run_handeye.sh waypoints # bash ~/ros2/scripts/run_handeye.sh teach # RDK 软拖拽（推荐） @@ -61,7 +61,7 @@ source "${ROOT}/scripts/env.sh" # shellcheck disable=SC1091 source "${ROOT}/scripts/lib/load_site_config.sh" -# Prefer package source tree (editable); fall back to install share +# 优先用包的源码树（editable 模式）；退而用 install 里的 share APP_CFG="${ROOT}/src/arm_application/config" if [[ ! -d "${APP_CFG}" ]]; then APP_CFG="$(ros2 pkg prefix arm_application 2>/dev/null)/share/arm_application/config" || true diff --git a/scripts/run_hole_detect.sh b/scripts/run_hole_detect.sh index 54c5ece..f4d2915 100755 --- a/scripts/run_hole_detect.sh +++ b/scripts/run_hole_detect.sh @@ -1,13 +1,13 @@ #!/usr/bin/env bash -# Hole detection on the current-arm wrist D405. Detect only — no insert motion. +# 用当前臂腕部 D405 做孔检测。只检测 —— 不做插入动作。 # # bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview # bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview # -# backend from site.yaml hole_detect.backend: -# two_stage YOLO workpiece bbox + HoughCircles (camera_test hole_detect.py) -# yolo YOLO boxes treated as holes -# hough full-image HoughCircles +# 后端取自 site.yaml 的 hole_detect.backend: +# two_stage YOLO 工件 bbox + HoughCircles（camera_test 的 hole_detect.py） +# yolo 把 YOLO 框直接当作孔 +# hough 整图 HoughCircles set -eo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" diff --git a/scripts/run_real.sh b/scripts/run_real.sh index b2a4cea..2377679 100755 --- a/scripts/run_real.sh +++ b/scripts/run_real.sh @@ -1,6 +1,6 @@ #!/usr/bin/env bash -# Real Rizon4 bringup — sets RDK runtime libs + launches stack backend:=real -# Note: no `set -u` — conda/RoboStack hooks reference unset vars (CONDA_BUILD, COLCON_TRACE) +# 真机 Rizon4 启动入口 —— 设置 RDK 运行时库，然后以 backend:=real 启动整栈 +# 注意：这里不加 `set -u` —— conda/RoboStack 的钩子会引用未绑定的变量（CONDA_BUILD、COLCON_TRACE） # # bash ~/ros2/scripts/run_real.sh # bash ~/ros2/scripts/run_real.sh --arm 91 @@ -31,10 +31,10 @@ source "${ROOT}/install/setup.bash" # shellcheck disable=SC1091 source "${ROOT}/scripts/lib/load_site_config.sh" -# flexiv_hardware must load spdlog/RBDyn from flexiv_install (not conda fmt v11) +# `flexiv_hardware` 必须从 flexiv_install 加载 spdlog/RBDyn（不能用 conda 的 fmt v11） export LD_LIBRARY_PATH="${FLEXIV_PREFIX}/lib:${LD_LIBRARY_PATH:-}" -# SITE_ROBOT_SN is the resolved Flexiv serial (89/91 aliases already mapped). +# `SITE_ROBOT_SN` 是解析后的 Flexiv 序列号（89/91 别名已经映射好了）。 ROBOT_SN="${SITE_ROBOT_SN:-${ROBOT_SN:-Rizon4-063989}}" ROBOT_IP="${ROBOT_IP:-${SITE_ROBOT_IP:-192.168.100.1}}" NET_IFACE="${SITE_NET_IFACE:-}" diff --git a/scripts/run_sim.sh b/scripts/run_sim.sh index 774b098..4541eaa 100755 --- a/scripts/run_sim.sh +++ b/scripts/run_sim.sh @@ -1,5 +1,5 @@ #!/usr/bin/env bash -# Quick-start: mock arm + RViz +# 快速启动：mock 机械臂 + RViz set -euo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" # shellcheck disable=SC1091 diff --git a/scripts/run_tactile.sh b/scripts/run_tactile.sh index db7e3ba..ab95945 100755 --- a/scripts/run_tactile.sh +++ b/scripts/run_tactile.sh @@ -1,8 +1,8 @@ #!/usr/bin/env bash -# Orisys visuo-tactile (finger cameras, not D405). Config: site.yaml tactile: +# Orisys 视触觉（指端相机，不是 D405）。配置：site.yaml 的 tactile: # # bash ~/ros2/scripts/run_tactile.sh --cli -# bash ~/ros2/scripts/run_tactile.sh # ROS node /tactile/left|right/status +# bash ~/ros2/scripts/run_tactile.sh # ROS 节点 /tactile/left|right/status set -eo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" diff --git a/scripts/run_visual_servo_rt.sh b/scripts/run_visual_servo_rt.sh index b058993..ec416e5 100755 --- a/scripts/run_visual_servo_rt.sh +++ b/scripts/run_visual_servo_rt.sh @@ -1,14 +1,14 @@ #!/usr/bin/env bash -# Real-time visual servo (reads ALL tunables from ~/ros2/config/site.yaml). +# 实时视觉伺服（所有可调参数全部从 ~/ros2/config/site.yaml 读取）。 # # bash ~/ros2/scripts/run_visual_servo_rt.sh # bash ~/ros2/scripts/run_visual_servo_rt.sh --control # bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 91 --control # -# --control starts tracking from the current pose (no lift, no return-to-home). +# --control 从当前位姿直接开始跟踪（不抬升，也不回 home）。 # 89/91 手眼与相机各自独立：必须与 run_real.sh 同一 --arm。 # -# Switch YOLO weights: edit paths.yolo_weights in site.yaml +# 切换 YOLO 权重：改 site.yaml 里的 paths.yolo_weights set -eo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" @@ -42,9 +42,9 @@ fi if [[ "${CONTROL}" == true ]]; then echo "=== waiting for rizon_arm_controller ACTIVE (T1: run_real.sh --arm ${SITE_ROBOT_ARM:-?}) ===" - # NOTE: action-list can show zombie entries from a crashed ros2_control, - # and `ros2 control` CLI is broken in this env (xmlrpc Fault). A raw - # service call only answers when the controller manager is actually alive. + # 注意：action-list 可能显示出崩溃过的 ros2_control 留下的僵尸条目， + # 而且这个环境里的 `ros2 control` CLI 是坏的（xmlrpc Fault）。直接发 + # 服务请求只有控制器管理器真的活着时才会应答。 ready=false for _i in $(seq 1 40); do if timeout 5 ros2 service call /controller_manager/list_controllers \ diff --git a/scripts/show_site_config.sh b/scripts/show_site_config.sh index d0bd984..5b5e624 100755 --- a/scripts/show_site_config.sh +++ b/scripts/show_site_config.sh @@ -1,5 +1,5 @@ #!/usr/bin/env bash -# Show / validate the master site config. +# 显示 / 校验主站点配置。 # bash ~/ros2/scripts/show_site_config.sh # SITE_CONFIG=/path/to/other.yaml bash ~/ros2/scripts/show_site_config.sh diff --git a/scripts/test_rdk_python.sh b/scripts/test_rdk_python.sh index 47b3866..f4631d7 100755 --- a/scripts/test_rdk_python.sh +++ b/scripts/test_rdk_python.sh @@ -1,5 +1,5 @@ #!/usr/bin/env bash -# Smoke-test Flexiv RDK Python against site robot (no ROS / no GitHub). +# 针对站点机器人对 Flexiv RDK Python 做冒烟测试（不需要 ROS / 不访问 GitHub）。 set -euo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" # shellcheck disable=SC1091 diff --git a/scripts/use_local_flexiv.sh b/scripts/use_local_flexiv.sh index 9a5fdda..d3082f7 100755 --- a/scripts/use_local_flexiv.sh +++ b/scripts/use_local_flexiv.sh @@ -1,6 +1,6 @@ #!/usr/bin/env bash -# Wire local Flexiv trees under ~/文档/Flexiv_Rizon4 into this workspace. -# Avoids re-cloning RDK / sim plugin from GitHub. +# 把 ~/文档/Flexiv_Rizon4 下的本地 Flexiv 代码树接入本工作区。 +# 免得再从 GitHub 重新克隆 RDK / 仿真插件。 set -euo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" @@ -30,7 +30,7 @@ if [[ -d "${LOCAL_FLEXIV}/flexiv_sim_plugin" ]]; then echo "Linked thirdparty/flexiv_sim_plugin -> ${LOCAL_FLEXIV}/flexiv_sim_plugin" fi -# humble-v1 ROS hardware needs RDK 1.9.x (kept separately if present) +# humble-v1 的 ROS 硬件需要 RDK 1.9.x（如果存在，会单独保留一份） if [[ -d "${SRC}/flexiv_rdk.bak.1787646725" ]]; then ln -sfn "${SRC}/flexiv_rdk.bak.1787646725" "${SRC}/flexiv_rdk_v1" touch "${SRC}/flexiv_rdk.bak.1787646725/COLCON_IGNORE" diff --git a/src/arm_application/arm_application/__init__.py b/src/arm_application/arm_application/__init__.py index 662ddd8..3f4946f 100644 --- a/src/arm_application/arm_application/__init__.py +++ b/src/arm_application/arm_application/__init__.py @@ -1,32 +1,32 @@ -"""arm_application — field nodes for Flexiv Rizon + D405 + DH grippers. +"""arm_application —— Flexiv Rizon + D405 + DH 夹爪的现场节点包。 -Public ROS executables (see setup.py console_scripts) ----------------------------------------------------- -Camera: realsense_color_pub -Perception: perception_node -Servo: servo_node -Hand-eye: handeye_teach, handeye_waypoints, handeye_calibrate -Gripper: dh_pgea, dh_pgea_cli, dual_gripper, dual_gripper_cli -Hole detect: hole_detect -Tactile: tactile, tactile_cli -Helpers: goto_ee_plane, demo_named_poses, run_joint_trajectory +对外可直接运行的 ROS 程序（见 ``setup.py`` 的 ``console_scripts``） +------------------------------------------------------------------ +相机： realsense_color_pub +感知： perception_node +伺服： servo_node +手眼： handeye_teach、handeye_waypoints、handeye_calibrate +夹爪： dh_pgea、dh_pgea_cli、dual_gripper、dual_gripper_cli +入孔检测： hole_detect +触觉： tactile、tactile_cli +辅助： goto_ee_plane、demo_named_poses、run_joint_trajectory -Reusable Python modules (import from your own nodes) ----------------------------------------------------- -yolo_detector YOLO / TensorRT inference + pick_best -box_tracker pixel-space constant-velocity box coast -plane_feature stereo plane centroid inside a YOLO box -dh_pgea official Modbus RTU client for DH PGEA -dh_official DH Modbus client used by dual_gripper -dual_gripper PGEA left + RGI right -hole_detector yolo / hough / two_stage hole centers -orisys_tactile Orisys visuo-tactile adapter -geom_ik Rizon geometric Jacobian / DLS / hand-eye load -handeye_lib chessboard + OpenCV calibrateHandEye -handeye_realsense topic / serial helpers for D405 -pid_controller PID with integral separation -kalman_filter 3D constant-velocity KF +可复用的 Python 模块（可以在你自己的节点里 import） +------------------------------------------------------------------ +``yolo_detector`` YOLO / TensorRT 推理 + ``pick_best`` 挑最优框 +``box_tracker`` 像素平面内的匀速模型框预测（丢帧时"滑行"用） +``plane_feature`` 在 YOLO 框内取立体平面的质心 +``dh_pgea`` 大寰 PGEA 的官方 Modbus RTU 客户端 +``dh_official`` ``dual_gripper`` 用的 DH Modbus 客户端 +``dual_gripper`` 左 PGEA + 右 RGI +``hole_detector`` yolo / hough / two_stage 三种孔心检测 +``orisys_tactile`` Orisys 视触觉适配器 +``geom_ik`` Rizon 几何雅可比 / DLS / 手眼加载 +``handeye_lib`` 棋盘格 + OpenCV ``calibrateHandEye`` +``handeye_realsense`` D405 的话题 / 序列号辅助 +``pid_controller`` 带积分分离的 PID +``kalman_filter`` 三维匀速模型卡尔曼滤波 -Operators should use ``scripts/*.sh`` and ``config/site.yaml``, not these -imports. See ``docs/INTERFACES.md``. +现场操作请走 ``scripts/*.sh`` 和 ``config/site.yaml``，不要直接 import 这些模块。 +接口契约见 ``docs/INTERFACES.md``。 """ diff --git a/src/arm_application/arm_application/box_tracker.py b/src/arm_application/arm_application/box_tracker.py index ac10b30..daf8786 100644 --- a/src/arm_application/arm_application/box_tracker.py +++ b/src/arm_application/arm_application/box_tracker.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 -"""Constant-velocity tracker for a YOLO xyxy box (pixel space). +"""针对 YOLO ``xyxy`` 框（像素坐标系）的匀速跟踪器。 -Used so fast workpiece motion does not drop the lock: the next-frame gate -is the *predicted* box, and brief YOLO misses coast the overlay. +用于避免工件快速运动时丢失锁定：下一帧的闸门使用*预测*框， +短暂的 YOLO 漏检则由惯性滑行（coast）维持叠加显示。 """ from __future__ import annotations @@ -38,7 +38,7 @@ class BoxTracker: self.v_lpf = float(np.clip(v_lpf, 0.05, 1.0)) self.max_dt = float(max_dt) self.xyxy: Optional[np.ndarray] = None - self.v = np.zeros(2, dtype=float) # center px/s + self.v = np.zeros(2, dtype=float) # 中心点 px/s self.t: Optional[float] = None def reset(self) -> None: diff --git a/src/arm_application/arm_application/demo_named_poses.py b/src/arm_application/arm_application/demo_named_poses.py index 7bc08bc..c679623 100644 --- a/src/arm_application/arm_application/demo_named_poses.py +++ b/src/arm_application/arm_application/demo_named_poses.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Demo: cycle named poses via /set_named_pose (works for any backend).""" +"""演示：通过 ``/set_named_pose`` 循环切换命名位姿（对任何后端都适用）。""" from __future__ import annotations @@ -31,10 +31,10 @@ class DemoNamedPoses(Node): def run(self) -> int: if not self._cli.wait_for_service(timeout_sec=15.0): - self.get_logger().error('/set_named_pose not available') + self.get_logger().error('``/set_named_pose`` 服务不可用') return 1 for loop in range(max(self.loops, 1)): - self.get_logger().info(f'loop {loop + 1}/{self.loops}') + self.get_logger().info(f'第 {loop + 1}/{self.loops} 轮') for pose in self.poses: req = SetNamedPose.Request() req.pose_name = pose @@ -45,17 +45,17 @@ class DemoNamedPoses(Node): while rclpy.ok() and not fut.done(): rclpy.spin_once(self, timeout_sec=0.05) if time.monotonic() > deadline: - self.get_logger().error(f'timeout on {pose}') + self.get_logger().error(f'{pose} 调用超时') return 2 res = fut.result() if res is None or not res.success: self.get_logger().error( - f'{pose} failed: {res.message if res else "no response"}' + f'{pose} 执行失败：{res.message if res else "无响应"}' ) return 3 - self.get_logger().info(f'{pose}: {res.message}') + self.get_logger().info(f'{pose}：{res.message}') time.sleep(self.dwell_sec) - self.get_logger().info('demo done') + self.get_logger().info('演示结束') return 0 diff --git a/src/arm_application/arm_application/dh_official.py b/src/arm_application/arm_application/dh_official.py index 493a85e..451d8b6 100644 --- a/src/arm_application/arm_application/dh_official.py +++ b/src/arm_application/arm_application/dh_official.py @@ -20,34 +20,34 @@ from typing import Iterable # ============================================================================= -# DH-Robotics Modbus register map +# DH-Robotics Modbus 寄存器映射 # ============================================================================= -# Basic control registers (holding registers, function code 03/06/10) -REG_INIT = 0x0100 # 0x01 initialize, 0xA5 fully initialize -REG_FORCE = 0x0101 # gripper closing force, 20~100 % -REG_POSITION = 0x0103 # finger reference position, 0~1000 per-mille -REG_SPEED = 0x0104 # finger open/close speed, 1~100 % +# 基础控制寄存器（保持寄存器，功能码 03/06/10） +REG_INIT = 0x0100 # 0x01 初始化，0xA5 完全初始化 +REG_FORCE = 0x0101 # 夹爪闭合力，20~100 % +REG_POSITION = 0x0103 # 手指参考位置，0~1000 千分比 +REG_SPEED = 0x0104 # 手指开合速度，1~100 % -# RGI rotation control registers -REG_ROTATION_ANGLE = 0x0105 # target rotation angle, signed degree value -REG_ROTATION_SPEED = 0x0107 # rotation speed, 1~100 % -REG_ROTATION_FORCE = 0x0108 # rotation force/torque, 20~100 % +# RGI 旋转控制寄存器 +REG_ROTATION_ANGLE = 0x0105 # 目标旋转角度，有符号角度值 +REG_ROTATION_SPEED = 0x0107 # 旋转速度，1~100 % +REG_ROTATION_FORCE = 0x0108 # 旋转力/力矩，20~100 % -# Feedback registers -REG_INIT_STATE = 0x0200 # 0 uninitialized, 1 initialized, 2 initializing -REG_GRIPPER_STATE = 0x0201 # 0 moving, 1 reached, 2 object caught, 3 object dropped -REG_CURRENT_POSITION = 0x0202 # current finger position, per-mille -REG_ROTATION_ANGLE_FB = 0x0208 # current rotation angle, signed degree value -REG_ROTATION_INIT_STATE = 0x020A # 0 uninitialized, 1 initialized, 2 initializing -REG_ROTATION_STATE = 0x020B # 0 moving, 1 reached, 2 blocking, 3 had been blocked +# 反馈寄存器 +REG_INIT_STATE = 0x0200 # 0 未初始化，1 已初始化，2 初始化中 +REG_GRIPPER_STATE = 0x0201 # 0 运动中，1 已到位，2 已夹到物体，3 物体掉落 +REG_CURRENT_POSITION = 0x0202 # 当前手指位置，千分比 +REG_ROTATION_ANGLE_FB = 0x0208 # 当前旋转角度，有符号角度值 +REG_ROTATION_INIT_STATE = 0x020A # 0 未初始化，1 已初始化，2 初始化中 +REG_ROTATION_STATE = 0x020B # 0 运动中，1 已到位，2 受阻，3 曾经受阻 -# Configuration registers -REG_SAVE_PARAMS = 0x0300 # write 1 to save all parameters to flash +# 配置寄存器 +REG_SAVE_PARAMS = 0x0300 # 写入 1 可把所有参数保存到 flash class DHModbusError(RuntimeError): - """DH Modbus communication or protocol error.""" + """DH Modbus 通信或协议错误。""" def _clamp_int(value: float | int, lo: int, hi: int) -> int: @@ -55,9 +55,9 @@ def _clamp_int(value: float | int, lo: int, hi: int) -> int: def _encode_signed(value: float | int) -> int: - """Encode signed degree value as DH one's-complement register value. + """把有符号角度值编码为 DH 的反码（one's complement）寄存器值。 - RGI manual example: +360 -> 0x0168, -360 -> 0xFE97. + RGI 手册示例：+360 -> 0x0168，-360 -> 0xFE97。 """ value = _clamp_int(value, -32767, 32767) if value >= 0: @@ -66,7 +66,7 @@ def _encode_signed(value: float | int) -> int: def _decode_signed(raw: int) -> int: - """Decode DH one's-complement register value to signed integer.""" + """把 DH 的反码（one's complement）寄存器值解码为有符号整数。""" raw = int(raw) & 0xFFFF if raw & 0x8000: mag = (~raw) & 0x7FFF @@ -75,7 +75,7 @@ def _decode_signed(raw: int) -> int: class DHModbusClient: - """Minimal thread-safe Modbus RTU client for DH-Robotics grippers.""" + """面向 DH-Robotics 夹爪的最小线程安全 Modbus RTU 客户端。""" def __init__( self, @@ -91,7 +91,7 @@ class DHModbusClient: try: import minimalmodbus import serial - except ImportError as exc: # pragma: no cover - depends on runtime env + except ImportError as exc: # pragma: no cover - 取决于运行时环境 raise DHModbusError( "缺少 minimalmodbus/pyserial。请先安装：pip install minimalmodbus pyserial" ) from exc @@ -124,7 +124,7 @@ class DHModbusClient: pass # ------------------------------------------------------------------ - # Low-level register access + # 底层寄存器读写 # ------------------------------------------------------------------ def write_register(self, address: int, value: int) -> None: value = _clamp_int(value, 0, 0xFFFF) @@ -170,10 +170,10 @@ class DHModbusClient: return _decode_signed(self.read_register(address)) # ------------------------------------------------------------------ - # Common PGE/PGEA/RGI control + # PGE/PGEA/RGI 通用控制 # ------------------------------------------------------------------ def initialize(self, *, full: bool = False, rotation: bool = False, timeout: float = 10.0) -> None: - """Initialize/homing the gripper and wait until ready.""" + """初始化/回零夹爪，并等待其就绪。""" self.write_register(REG_INIT, 0xA5 if full else 0x01) start = time.monotonic() while time.monotonic() - start < timeout: @@ -229,11 +229,11 @@ class DHModbusClient: return self.read_register(REG_INIT_STATE) def hold_current_position(self) -> None: - """Best-effort stop/hold for DH grippers without a dedicated stop register.""" + """在没有专用停止寄存器的 DH 夹爪上尽力停止/保持。""" self.set_position_per_mille(self.get_position_per_mille()) # ------------------------------------------------------------------ - # RGI rotation control + # RGI 旋转控制 # ------------------------------------------------------------------ def set_rotation_speed_pct(self, speed_pct: float | int) -> int: value = _clamp_int(speed_pct, 1, 100) @@ -263,7 +263,7 @@ class DHModbusClient: return self.read_register(REG_ROTATION_STATE) def hold_current_rotation(self) -> None: - """Stop rotation by re-commanding the current feedback angle.""" + """重新下发当前反馈角度来停止旋转。""" self.write_register(REG_ROTATION_ANGLE, self.get_rotation_angle_raw()) def hold_current_all(self) -> None: diff --git a/src/arm_application/arm_application/dh_pgea.py b/src/arm_application/arm_application/dh_pgea.py index 3a9ccd7..2653230 100644 --- a/src/arm_application/arm_application/dh_pgea.py +++ b/src/arm_application/arm_application/dh_pgea.py @@ -1,21 +1,21 @@ #!/usr/bin/env python3 -"""DH Robotics PGE / PGEA Modbus RTU client. +"""DH Robotics PGE / PGEA Modbus RTU 客户端。 -Port of the official C++ driver: +移植自官方 C++ 驱动： DH-Robotics/dh_gripper_ros → DH_Modbus_Gripper - (factory maps model names containing ``PGE`` / ``PGEA`` onto this class) + （工厂会按型号名里是否包含 ``PGE`` / ``PGEA`` 映射到本类） -Flexiv RDK ``Gripper`` cannot drive this claw: it is not a Studio Device. -Talk RS485 (USB–UART) with the same registers the DH ROS driver uses. +Flexiv RDK 的 ``Gripper`` 驱动不了这个夹爪：它不是 Studio Device。 +改用 RS485（USB–UART）通信，寄存器与 DH 的 ROS 驱动完全一致。 -Registers (holding, FC03 read / FC06 write, CRC-16/Modbus LE): - 0x0100 init write 0xA5 - 0x0101 force 20–100 % - 0x0103 position 0–1000 ‰ - 0x0104 speed 1–100 % - 0x0200 init state 0=need 1=done 2=doing - 0x0201 grip state 0=moving 1=arrived 2=caught 3=dropped - 0x0202 current position +寄存器（保持寄存器，FC03 读 / FC06 写，CRC-16/Modbus 小端）： + 0x0100 初始化，写 0xA5 + 0x0101 力 20–100 % + 0x0103 位置 0–1000 ‰ + 0x0104 速度 1–100 % + 0x0200 初始化状态 0=需要 1=完成 2=进行中 + 0x0201 夹持状态 0=运动中 1=已到位 2=已夹住 3=已掉落 + 0x0202 当前位置 """ from __future__ import annotations @@ -31,11 +31,11 @@ try: import serial except ImportError as exc: # pragma: no cover raise SystemExit( - "pyserial missing. In conda ros_flexiv: pip install pyserial" + "缺少 pyserial。在 conda 环境 ros_flexiv 里执行：pip install pyserial" ) from exc -# Official CRC-16/Modbus (same table as dh_modbus_gripper.cpp). +# 官方 CRC-16/Modbus（与 dh_modbus_gripper.cpp 使用同一张表）。 _CRC_TABLE = ( 0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241, 0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440, @@ -105,7 +105,7 @@ INIT_STATE_NAME = { def crc16_modbus(data: bytes) -> int: - """Official DH CRC16 (Modbus, little-endian on the wire).""" + """官方 DH CRC16（Modbus，线路上按小端发送）。""" crc = 0xFFFF for byte in data: crc = (crc >> 8) ^ _CRC_TABLE[(crc ^ byte) & 0xFF] @@ -176,7 +176,7 @@ WIRING_HINT = ( class DhPgeaGripper: - """Thread-safe Modbus RTU client for PGE / PGEA (official register map).""" + """面向 PGE / PGEA 的线程安全 Modbus RTU 客户端（官方寄存器映射）。""" def __init__( self, @@ -196,7 +196,7 @@ class DhPgeaGripper: if mode in ("", "off", "none", "false", "0"): mode = "" if mode not in ("", "tx", "tx_inv"): - raise DhPgeaError(f"rs485_rts must be '', tx, or tx_inv, got {rs485_rts!r}") + raise DhPgeaError(f"rs485_rts 只能是 ''、tx 或 tx_inv，实际为 {rs485_rts!r}") self.rs485_rts = mode self._ser: Optional[serial.Serial] = None self._lock = threading.Lock() @@ -220,8 +220,8 @@ class DhPgeaGripper: ) except Exception as exc: raise DhPgeaError( - f"cannot open {self.port}: {exc}. " - "Need 24 V DC + RS485, and user in group dialout." + f"无法打开 {self.port}：{exc}。" + "需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。" ) from exc try: self._ser.dtr = True @@ -248,7 +248,7 @@ class DhPgeaGripper: self.close() def write_register(self, index: int, value: int, retries: int = 3) -> bool: - """FC06 write, wait for echo (official WriteRegisterFunc).""" + """FC06 写，等待回显（对应官方 WriteRegisterFunc）。""" frame = _frame(self.slave_id, 0x06, index, int(value) & 0xFFFF) with self._lock: for _ in range(max(1, retries)): @@ -258,7 +258,7 @@ class DhPgeaGripper: return False def read_register(self, index: int, retries: int = 8) -> int: - """FC03 read one holding register (official ReadRegisterFunc).""" + """FC03 读一个保持寄存器（对应官方 ReadRegisterFunc）。""" req = _frame(self.slave_id, 0x03, index, 1) last: Optional[bytes] = None with self._lock: @@ -275,13 +275,13 @@ class DhPgeaGripper: return (rsp[3] << 8) | (rsp[4] & 0xFF) hint = f" last={last.hex() if last else '(empty)'}" raise DhPgeaError( - f"Modbus read 0x{index:04X} failed on {self.port} " - f"(no valid reply{hint}). baud={self.baudrate} slave={self.slave_id} " + f"在 {self.port} 上 Modbus 读 0x{index:04X} 失败 " + f"（无有效应答{hint}）。baud={self.baudrate} slave={self.slave_id} " f"rts={self.rs485_rts or 'off'}" ) def probe(self) -> int: - """One short read of init-state. Raises if the claw is silent.""" + """短读一次初始化状态。夹爪无应答时会抛异常。""" return self.read_register(REG_INIT_STATE, retries=2) def _set_tx(self, tx: bool) -> None: @@ -297,7 +297,7 @@ class DhPgeaGripper: def _transact(self, frame: bytes, expect: int) -> Optional[bytes]: if self._ser is None or not self._ser.is_open: - raise DhPgeaError("serial not open") + raise DhPgeaError("串口未打开") bit_s = 10.0 / max(1, self.baudrate) self._set_tx(True) if self.rs485_rts: @@ -320,12 +320,12 @@ class DhPgeaGripper: return self.write_register(REG_INIT, INIT_CMD) def initialize(self, timeout_s: float = 20.0, poll_s: float = 0.2) -> bool: - """Write 0xA5 then wait until init state == finished (official bringup).""" + """写 0xA5，然后等到初始化状态 == 完成（对应官方上电流程）。""" state = self.get_init_state() if state == S_INIT_FINISHED: return True if not self.initialization(): - raise DhPgeaError("init write 0x0100=0xA5 failed (no Modbus echo)") + raise DhPgeaError("初始化写 0x0100=0xA5 失败（无 Modbus 回显）") deadline = time.monotonic() + float(timeout_s) while time.monotonic() < deadline: state = self.get_init_state() @@ -333,8 +333,8 @@ class DhPgeaGripper: return True time.sleep(poll_s) raise DhPgeaError( - f"init timeout after {timeout_s:.0f}s (state={state}). " - "Fingers should home once on first power-up." + f"初始化在 {timeout_s:.0f}s 后超时（state={state}）。" + "首次上电时手指本应先回零一次。" ) def set_target_force(self, force_pct: int) -> bool: @@ -427,11 +427,11 @@ class DhPgeaGripper: def _selfcheck_crc() -> None: - """Official short-manual example: set position 500.""" + """官方简短手册示例：设置位置 500。""" frame = _frame(1, 0x06, 0x0103, 500) expect = bytes([0x01, 0x06, 0x01, 0x03, 0x01, 0xF4, 0x78, 0x21]) if frame != expect: - raise AssertionError(f"CRC mismatch: {frame.hex()} != {expect.hex()}") + raise AssertionError(f"CRC 不匹配：{frame.hex()} != {expect.hex()}") def _site_defaults() -> dict: @@ -465,11 +465,11 @@ def _site_defaults() -> dict: def _scan_bus(port: str) -> int: - """Try baud × slave × RTS until one combination answers.""" + """遍历波特率 × 从站号 × RTS，直到某个组合有应答。""" bauds = (115200, 57600, 38400, 19200, 9600) slaves = (1, 2, 3) rts_modes = ("", "tx", "tx_inv") - print(f"scan {port} (stop the ROS node first — port is exclusive)") + print(f"扫描 {port}（请先停掉 ROS 节点 —— 该串口是独占的）") for baud in bauds: for slave in slaves: for rts in rts_modes: @@ -492,7 +492,7 @@ def _scan_bus(port: str) -> int: print(f" miss {label}") finally: g.close() - print("no reply on any combination.", file=sys.stderr) + print("所有组合都无应答。", file=sys.stderr) print(WIRING_HINT, file=sys.stderr) return 1 @@ -500,24 +500,24 @@ def _scan_bus(port: str) -> int: def _cli(argv: Optional[list[str]] = None) -> int: site = _site_defaults() ap = argparse.ArgumentParser( - description="DH PGEA Modbus CLI (official register map, not Flexiv RDK)" + description="DH PGEA Modbus 命令行工具（官方寄存器映射，不用 Flexiv RDK）" ) ap.add_argument("--port", default=site["port"]) ap.add_argument("--baud", type=int, default=site["baudrate"]) ap.add_argument("--slave", type=int, default=site["slave_id"]) ap.add_argument("--force", type=int, default=site["force"]) ap.add_argument("--speed", type=int, default=site["speed"]) - ap.add_argument("--rts", default=site.get("rs485_rts") or "", help="RS485 DE: empty | tx | tx_inv") + ap.add_argument("--rts", default=site.get("rs485_rts") or "", help="RS485 DE：空 | tx | tx_inv") ap.add_argument( "cmd", choices=["probe", "init", "status", "open", "close", "pos", "selfcheck", "scan"], ) - ap.add_argument("value", nargs="?", default="", help="position 0–1000 for 'pos'") + ap.add_argument("value", nargs="?", default="", help="'pos' 命令的位置，0–1000") args = ap.parse_args(argv) if args.cmd == "selfcheck": _selfcheck_crc() - print("CRC self-check OK (01 06 01 03 01 F4 78 21)") + print("CRC 自检通过（01 06 01 03 01 F4 78 21）") return 0 if args.cmd == "scan": return _scan_bus(args.port) @@ -532,7 +532,7 @@ def _cli(argv: Optional[list[str]] = None) -> int: try: grip.open() except DhPgeaError as exc: - print(f"ERROR: {exc}", file=sys.stderr) + print(f"错误：{exc}", file=sys.stderr) return 1 try: @@ -541,7 +541,7 @@ def _cli(argv: Optional[list[str]] = None) -> int: print(st.summary()) return 0 if args.cmd == "init": - print("initializing (fingers home)…") + print("正在初始化（手指回零）…") grip.initialize() print(grip.get_run_states().summary()) return 0 @@ -550,25 +550,25 @@ def _cli(argv: Optional[list[str]] = None) -> int: return 0 if args.cmd == "open": if not grip.open_jaw(args.force, args.speed): - raise DhPgeaError("open write failed") + raise DhPgeaError("张开写入失败") print(grip.wait_settle(1000).summary()) return 0 if args.cmd == "close": if not grip.close_jaw(args.force, args.speed): - raise DhPgeaError("close write failed") + raise DhPgeaError("闭合写入失败") print(grip.wait_settle(0).summary()) return 0 if args.cmd == "pos": if args.value == "": - print("usage: pos <0-1000>", file=sys.stderr) + print("用法：pos <0-1000>", file=sys.stderr) return 2 pos = int(float(args.value)) if not grip.move(pos, args.force, args.speed): - raise DhPgeaError("position write failed") + raise DhPgeaError("位置写入失败") print(grip.wait_settle(pos).summary()) return 0 except DhPgeaError as exc: - print(f"ERROR: {exc}", file=sys.stderr) + print(f"错误：{exc}", file=sys.stderr) print(WIRING_HINT, file=sys.stderr) return 1 finally: diff --git a/src/arm_application/arm_application/dh_pgea_node.py b/src/arm_application/arm_application/dh_pgea_node.py index 2ce345c..76fc055 100644 --- a/src/arm_application/arm_application/dh_pgea_node.py +++ b/src/arm_application/arm_application/dh_pgea_node.py @@ -1,18 +1,18 @@ #!/usr/bin/env python3 -"""ROS 2 node for DH PGEA (official Modbus RTU, not Flexiv gripper). +"""DH PGEA 的 ROS 2 节点（官方 Modbus RTU，不用 Flexiv 夹爪）。 -Topics (layout matches DH-Robotics/dh_gripper_ros): +话题（布局与 DH-Robotics/dh_gripper_ros 一致）： /gripper/ctrl std_msgs/Float32MultiArray data: [initialize, position, force, speed] - initialize>0.5 → write 0xA5; else set force/speed/position + initialize>0.5 → 写 0xA5；否则设置力/速度/位置 /gripper/states std_msgs/Float32MultiArray data: [initialized, grip_state, position, target_pos, target_force] /gripper/joint_states sensor_msgs/JointState -Services (std_srvs/Trigger): +服务（std_srvs/Trigger）： /gripper/initialize /gripper/open /gripper/close -Do not set Flexiv bringup load_gripper:=true — that is Flexiv-GN01 only. +不要设置 Flexiv bringup 的 load_gripper:=true —— 那只是 Flexiv-GN01 用的。 """ from __future__ import annotations @@ -36,7 +36,7 @@ from arm_application.dh_pgea import ( def _param_as_str(node: Node, name: str, default: str = "") -> str: - """String param that also accepts numeric CLI overrides (e.g. SN 26610799).""" + """字符串参数，同时允许命令行用数字覆盖（例如 SN 26610799）。""" node.declare_parameter(name, default, ParameterDescriptor(dynamic_typing=True)) val = node.get_parameter(name).value if val is None or val == "": @@ -105,7 +105,7 @@ class DhPgeaNode(Node): self.get_logger().info( f"DH {self._model} SN={self._sn or '?'} " f"{self._port} @{self._baud} slave={self._slave} rts={self._rts or 'off'} " - f"(official Modbus RTU; Flexiv RDK gripper API unused)" + f"（官方 Modbus RTU；未使用 Flexiv RDK 夹爪 API）" ) try: self._grip.open() @@ -121,14 +121,14 @@ class DhPgeaNode(Node): self.get_logger().error(str(exc)) for line in WIRING_HINT.splitlines(): self.get_logger().error(line) - self.get_logger().error("will keep retrying; open/close will fail until the bus answers") + self.get_logger().error("会持续重试；在总线恢复应答前 open/close 都会失败") if self._auto_init and self._online: try: self._grip.initialize() - self.get_logger().info("gripper initialized") + self.get_logger().info("夹爪已初始化") except DhPgeaError as exc: - self.get_logger().error(f"auto_init failed: {exc}") + self.get_logger().error(f"auto_init 失败：{exc}") self._online = False period = 1.0 / max(1.0, hz) @@ -150,7 +150,7 @@ class DhPgeaNode(Node): if speed is not None: self._speed = s if not self._grip.move(int(position), f, s): - raise DhPgeaError("move write failed") + raise DhPgeaError("移动写入失败") return f"pos={int(position)} force={f}% speed={s}%" def _on_ctrl(self, msg: Float32MultiArray) -> None: @@ -158,7 +158,7 @@ class DhPgeaNode(Node): try: if initialize > 0.5: self._grip.initialize() - self.get_logger().info("initialize via /gripper/ctrl") + self.get_logger().info("通过 /gripper/ctrl 触发初始化") return f = int(force) if force > 0 else self._force s = int(speed) if speed > 0 else self._speed @@ -210,7 +210,7 @@ class DhPgeaNode(Node): self._state = st if not self._online: self._online = True - self.get_logger().info("gripper bus recovered") + self.get_logger().info("夹爪总线已恢复") except DhPgeaError as exc: self._online = False self.get_logger().warn(str(exc), throttle_duration_sec=5.0) @@ -227,7 +227,7 @@ class DhPgeaNode(Node): js = JointState() js.header.stamp = self.get_clock().now().to_msg() js.name = ["gripper_finger1_joint"] - # Official ROS driver maps 0–1000 → ~0–0.637 rad; we publish mm as well. + # 官方 ROS 驱动把 0–1000 映射到约 0–0.637 rad；我们这里也发布 mm。 js.position = [float(st.width_mm) * 0.001] js.effort = [float(st.target_force)] self._pub_js.publish(js) diff --git a/src/arm_application/arm_application/dual_gripper.py b/src/arm_application/arm_application/dual_gripper.py index 08695e5..4c7708b 100644 --- a/src/arm_application/arm_application/dual_gripper.py +++ b/src/arm_application/arm_application/dual_gripper.py @@ -750,7 +750,7 @@ def load_dual_arm_gripper_config(path: str | Path) -> DualArmGripperConfig: def load_from_site(data: dict) -> DualArmGripperConfig: - """Build DualArmGripperConfig from ~/ros2/config/site.yaml.""" + """从 ``~/ros2/config/site.yaml`` 构建 ``DualArmGripperConfig``。""" robots = list((data.get("robot") or {}).get("robots") or []) left = next((r for r in robots if str(r.get("arm") or "").lower() == "left"), {}) right = next((r for r in robots if str(r.get("arm") or "").lower() == "right"), {}) diff --git a/src/arm_application/arm_application/dual_gripper_cli.py b/src/arm_application/arm_application/dual_gripper_cli.py index 5fab4e9..7269f6d 100644 --- a/src/arm_application/arm_application/dual_gripper_cli.py +++ b/src/arm_application/arm_application/dual_gripper_cli.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""CLI for DH PGEA (left) and RGI (right). Loads ports from site.yaml.""" +"""DH PGEA（左臂）与 RGI（右臂）的命令行工具。从 site.yaml 读取串口。""" from __future__ import annotations @@ -39,67 +39,67 @@ def _normalize_arm(raw: str) -> str: return "right" if t in ("both", "dual", "all"): return "both" - raise SystemExit(f"unknown --arm {raw!r} (use 89|91|left|right|both)") + raise SystemExit(f"未知的 --arm {raw!r}（可用 89|91|left|right|both）") def test_parallel(manager: DualArmGripperManager, args: argparse.Namespace) -> None: gripper = manager.left - print(f"=== PGEA parallel ({gripper.cfg.port}) ===") + print(f"=== PGEA 平行夹爪（{gripper.cfg.port}）===") gripper.connect() - _print_status("connected", gripper.get_status()) + _print_status("已连接", gripper.get_status()) if args.test in ("init", "probe", "all"): if args.test != "probe": gripper.initialize(full=args.init_full) - _print_status("init", gripper.get_status()) + _print_status("初始化", gripper.get_status()) if args.test in ("open", "all"): - _print_status("open", gripper.open(force=args.force, speed=args.speed)) + _print_status("张开", gripper.open(force=args.force, speed=args.speed)) if args.test in ("close", "all"): - _print_status("close", gripper.close(force=args.force, speed=args.speed)) + _print_status("闭合", gripper.close(force=args.force, speed=args.speed)) if args.test in ("move", "pos") and args.pos is not None: - _print_status("move", gripper.move_to(args.pos, force=args.force, speed=args.speed)) + _print_status("移动", gripper.move_to(args.pos, force=args.force, speed=args.speed)) if args.test in ("status", "probe"): - _print_status("status", gripper.get_status()) + _print_status("状态", gripper.get_status()) def test_rotary(manager: DualArmGripperManager, args: argparse.Namespace) -> None: gripper = manager.right - print(f"=== RGI rotary ({gripper.cfg.port}) ===") + print(f"=== RGI 旋转夹爪（{gripper.cfg.port}）===") gripper.connect() - _print_status("connected", gripper.get_status()) + _print_status("已连接", gripper.get_status()) if args.test in ("init", "all"): gripper.initialize(full=args.init_full) - _print_status("init", gripper.get_status()) + _print_status("初始化", gripper.get_status()) if args.test in ("open", "all"): - _print_status("open", gripper.open_fingers(force=args.force, speed=args.speed)) + _print_status("张开", gripper.open_fingers(force=args.force, speed=args.speed)) if args.test in ("close", "all"): - _print_status("close", gripper.close_fingers(force=args.force, speed=args.speed)) + _print_status("闭合", gripper.close_fingers(force=args.force, speed=args.speed)) if args.test in ("move", "pos") and args.pos is not None: - _print_status("fingers", gripper.move_fingers(int(args.pos), force=args.force, speed=args.speed)) + _print_status("手指", gripper.move_fingers(int(args.pos), force=args.force, speed=args.speed)) if args.test in ("rotate", "rotate_to", "all"): if args.revolutions is not None: _print_status( - "revolutions", + "圈数", gripper.rotate_revolutions(args.revolutions, speed=args.speed, torque=args.force), ) else: angle = args.angle if args.angle is not None else 90.0 - _print_status("rotate", gripper.rotate_to(angle, speed=args.speed, torque=args.force)) + _print_status("旋转", gripper.rotate_to(angle, speed=args.speed, torque=args.force)) if args.test == "rotate_by": delta = args.angle if args.angle is not None else 45.0 - _print_status("rotate_by", gripper.rotate_by(delta, speed=args.speed, torque=args.force)) + _print_status("相对旋转", gripper.rotate_by(delta, speed=args.speed, torque=args.force)) if args.test in ("status", "probe"): - _print_status("status", gripper.get_status()) + _print_status("状态", gripper.get_status()) def main() -> int: - parser = argparse.ArgumentParser(description="DH PGEA / RGI CLI (site.yaml ports)") + parser = argparse.ArgumentParser(description="DH PGEA / RGI 命令行工具（串口来自 site.yaml）") parser.add_argument("--arm", default=os.environ.get("ROBOT_ARM") or os.environ.get("ROBOT_SN") or "89") parser.add_argument( "--test", default="status", help="probe|init|open|close|move|pos|rotate|rotate_by|status", ) - parser.add_argument("--pos", type=float, default=None, help="PGEA mm or RGI finger 0-1000") + parser.add_argument("--pos", type=float, default=None, help="PGEA 为 mm；RGI 为手指 0-1000") parser.add_argument("--angle", type=float, default=None) parser.add_argument("--revolutions", type=float, default=None) parser.add_argument("--speed", type=int, default=25) @@ -110,7 +110,7 @@ def main() -> int: site = Path(args.site).expanduser() if args.site else _site_path() if not site.is_file(): - print(f"ERROR: site.yaml missing: {site}", file=sys.stderr) + print(f"错误：site.yaml 缺失：{site}", file=sys.stderr) return 1 data = yaml.safe_load(site.read_text(encoding="utf-8")) or {} manager = DualArmGripperManager(load_from_site(data)) @@ -126,11 +126,11 @@ def main() -> int: test_parallel(manager, args) test_rotary(manager, args) except KeyboardInterrupt: - print("interrupt — stop") + print("收到中断 —— 停止") manager.emergency_stop() return 130 except Exception as exc: - print(f"ERROR: {exc}", file=sys.stderr) + print(f"错误：{exc}", file=sys.stderr) return 1 finally: manager.disconnect_all() diff --git a/src/arm_application/arm_application/dual_gripper_node.py b/src/arm_application/arm_application/dual_gripper_node.py index 64b211c..17c4987 100644 --- a/src/arm_application/arm_application/dual_gripper_node.py +++ b/src/arm_application/arm_application/dual_gripper_node.py @@ -1,4 +1,4 @@ -"""ROS 2 dual DH gripper node (PGEA left + RGI right). Uses arm_msgs, not Flexiv RDK.""" +"""ROS 2 双臂 DH 夹爪节点（左臂 PGEA + 右臂 RGI）。使用 arm_msgs，而非 Flexiv RDK。""" from __future__ import annotations @@ -61,7 +61,7 @@ class DualGripperNode(Node): site = _site_path() data = yaml.safe_load(site.read_text(encoding="utf-8")) or {} self._manager = DualArmGripperManager(load_from_site(data)) - self.get_logger().info(f"gripper config from site.yaml: {site}") + self.get_logger().info(f"夹爪配置来自 site.yaml：{site}") self._connect_errors: dict[str, str] = {} if _as_bool(self.get_parameter("auto_connect").value, True): @@ -112,12 +112,12 @@ class DualGripperNode(Node): continue try: gripper.connect() - self.get_logger().info(f"{side} gripper connected: {gripper.cfg.port}") + self.get_logger().info(f"{side} 夹爪已连接：{gripper.cfg.port}") except Exception as exc: self._connect_errors[side] = str(exc) - self.get_logger().error(f"{side} gripper connect failed: {exc}") + self.get_logger().error(f"{side} 夹爪连接失败：{exc}") if self._connect_errors and not _as_bool(self.get_parameter("allow_offline").value, True): - raise RuntimeError(f"gripper connect failed: {self._connect_errors}") + raise RuntimeError(f"夹爪连接失败：{self._connect_errors}") def _gripper(self, side: str): return self._manager.left if side == "left" else self._manager.right @@ -245,14 +245,14 @@ class DualGripperNode(Node): status = gripper.get_status() status.state = GripperState.ERROR - status.error_msg = f"unsupported command {command!r} (side={side})" + status.error_msg = f"不支持的命令 {command!r}（side={side}）" return status def _on_command(self, side: str, msg: GripperCommandMsg) -> None: with self._locks[side]: status = self._execute(side, msg) if status.state == GripperState.ERROR: - self.get_logger().warn(f"{side} command {msg.command!r} failed: {status.error_msg}") + self.get_logger().warn(f"{side} 命令 {msg.command!r} 失败：{status.error_msg}") def _on_execute(self, side: str, request: GripperExecute.Request, response: GripperExecute.Response): with self._locks[side]: diff --git a/src/arm_application/arm_application/goto_ee_plane.py b/src/arm_application/arm_application/goto_ee_plane.py index 6d51553..d3dd39e 100644 --- a/src/arm_application/arm_application/goto_ee_plane.py +++ b/src/arm_application/arm_application/goto_ee_plane.py @@ -1,12 +1,12 @@ #!/usr/bin/env python3 -"""Startup return-to-initial before perception/servo starts. +"""感知/伺服启动前的开机回初始位流程。 -Sequence: - [A] if the flange is below servo.safe_z_m, lift it STRAIGHT UP first - (closed-loop Z hops) so the home move can't sweep walls/fixtures; - [B] call /set_named_pose (Flexiv named pose, default "home") — the arm - returns to the Flexiv initial pose with Flexiv's own planner; - [C] hold (pin) so the servo takes over from a clean, stationary pose. +流程： + [A] 若法兰低于 ``servo.safe_z_m``，先让它竖直上升（闭环 Z 小步跳）， + 以免回 home 时扫到墙面/工装； + [B] 调用 ``/set_named_pose``（Flexiv 命名位姿，默认 "home"）—— 由 Flexiv + 自己的规划器把机械臂送回 Flexiv 初始位姿； + [C] 保持（pin）住，让伺服从一个干净、静止的位姿接手。 """ from __future__ import annotations @@ -49,13 +49,13 @@ def _load_ee_cfg() -> dict: "base": str(robot.get("base_frame") or "Rizon4-063989_base_link"), "ee": str(robot.get("ee_frame") or "Rizon4-063989_flange"), "z": ee_z, - # Lift straight up to this Z before the home move (avoid hitting - # walls/fixtures); then return to the Flexiv named initial pose. + # 回 home 前先竖直升到这个 Z（避免撞到墙面/工装）； + # 然后再回到 Flexiv 的命名初始位姿。 "safe_z": float(servo.get("safe_z_m") or max(0.40, ee_z + 0.10)), "home_pose": str(servo.get("home_pose_name") or "home"), "home_speed": float(servo.get("home_pose_speed") or 0.30), "v_z": float(servo.get("v_max_z") or 0.04), - # only used by the (currently unused) verticalize planner path + # 仅用于（目前未使用的）verticalize 规划路径 "kp": float(servo.get("ee_hold_kp") or 2.0), "w_max": float(servo.get("ee_w_max") or 0.50), } @@ -170,7 +170,7 @@ class GotoEePlane(Node): self._spin(0.05) handle = fut.result() if fut.done() else None if handle is None or not handle.accepted: - print(" ERROR: JTC rejected the height trajectory", flush=True) + print(" 错误：JTC 拒绝了抬升轨迹", flush=True) return False res = handle.get_result_async() t0 = time.monotonic() @@ -183,13 +183,13 @@ class GotoEePlane(Node): if Tee is not None: p = Tee[:3, 3] print( - f" moving ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f}) " + f" 运动中 ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f}) " f"z_err={(self.cfg['z'] - p[2]) * 1000:+.0f}mm", flush=True, ) last = now if not res.done(): - print(" WARN: JTC result timeout — checking TF anyway", flush=True) + print(" 警告：JTC 结果超时 —— 仍然继续检查 TF", flush=True) return False return True @@ -203,25 +203,24 @@ class GotoEePlane(Node): max_seg: float = 0.08, v: float = 0.05, ) -> bool: - """Plan and execute ONE short cartesian segment (≤ max_seg) toward - target. Long moves are handled by the caller in closed-loop hops, - because the Jacobian is frozen at the current pose — over >10 cm the - linearization error makes single-shot plans land far off target.""" + """规划并执行一段朝向 target 的短笛卡尔位移（≤ max_seg）。 + 长距离移动由调用方以闭环小步跳的方式完成，因为雅可比是在当前位姿 + 处冻结的 —— 超过 10 cm 后线性化误差会让一次性规划严重偏离目标。""" p = Tee[:3, 3].copy() R = Tee[:3, :3] dp, ang, _ = self._target_err(p, R, target, verticalize) dist = float(np.linalg.norm(dp)) - if dist > max_seg: # clamp this hop; caller loops with fresh TF + if dist > max_seg: # 限制本次小步跳的长度；调用方会用最新的 TF 循环执行 dp = dp * (max_seg / dist) dist = max_seg print( - f" plan {tag} Δxyz=({dp[0]*1000:+.0f},{dp[1]*1000:+.0f},{dp[2]*1000:+.0f})mm " - f"tilt={ang * 180 / np.pi:.1f}deg", + f" 规划 {tag} Δxyz=({dp[0]*1000:+.0f},{dp[1]*1000:+.0f},{dp[2]*1000:+.0f})mm " + f"倾角={ang * 180 / np.pi:.1f}deg", flush=True, ) J = self._jacobian(p) if J is None: - print("ERROR: missing link TF for Jacobian", flush=True) + print("错误：缺少用于雅可比的 link TF", flush=True) return False q = q0.copy() @@ -253,14 +252,14 @@ class GotoEePlane(Node): planned = dist - float(np.linalg.norm(remain)) if len(waypoints) < 2 or planned < 0.4 * dist: print( - f" WARN {tag}: planner stalled " - f"(planned {planned*1000:.0f}/{dist*1000:.0f}mm) — skip hop", + f" 警告 {tag}：规划器停滞 " + f"（已规划 {planned*1000:.0f}/{dist*1000:.0f}mm）—— 跳过本次小步跳", flush=True, ) return False - # joint-excursion sanity gate: refuse wild plans + # 关节偏移量的合理性闸门：拒绝过于夸张的规划 if float(np.max(np.abs(waypoints[-1] - q0))) > 1.0: - print(f" ERROR {tag}: joint excursion >1 rad — abort hop", flush=True) + print(f" 错误 {tag}：关节偏移 >1 rad —— 放弃本次小步跳", flush=True) return False duration = max(1.6, dist / max(min(v, 0.06), 1e-3)) @@ -278,10 +277,10 @@ class GotoEePlane(Node): ) qdot = np.clip(qdot, -0.40, 0.40) traj.points.append(_pt(qi, qdot, t)) - # force zero velocity at the last point + # 强制最后一个点的速度为零 traj.points[-1].velocities = [0.0] * 7 print( - f" send JTC action {tag} {n} steps T={duration:.1f}s", + f" 发送 JTC action {tag}，{n} 步 T={duration:.1f}s", flush=True, ) return self._send_action(traj, wait_s=duration + 6.0) @@ -298,9 +297,8 @@ class GotoEePlane(Node): v: float, max_hops: int = 8, ) -> Tuple[bool, np.ndarray, np.ndarray]: - """Closed-loop point-to-point: plan ≤8 cm hop → execute → re-measure - TF → repeat, until within tolerance. Robust to Jacobian linearization - error over long distances.""" + """闭环点到点：规划 ≤8 cm 的小步跳 → 执行 → 重新测量 TF → 重复， + 直到落在容差内。这样对长距离下的雅可比线性化误差更鲁棒。""" for hop in range(max_hops): p = Tee[:3, 3] dp, ang, _ = self._target_err(p, Tee[:3, :3], target, verticalize) @@ -308,8 +306,8 @@ class GotoEePlane(Node): if dist < tol_m and ((not verticalize) or ang < tol_tilt): return True, q, Tee print( - f" {tag} hop{hop + 1} remain={dist*1000:.0f}mm " - f"tilt={ang * 180 / np.pi:.1f}deg", + f" {tag} 第 {hop + 1} 跳 remain={dist*1000:.0f}mm " + f"倾角={ang * 180 / np.pi:.1f}deg", flush=True, ) if not self._plan_and_send(q, Tee, target, verticalize, tag, v=v): @@ -319,7 +317,7 @@ class GotoEePlane(Node): self._spin(0.05) q2, Tee2 = self._wait_state(2.0) if q2 is None or Tee2 is None: - print("ERROR: lost state during move", flush=True) + print("错误：移动过程中丢失状态", flush=True) return False, q, Tee q, Tee = q2, Tee2 p = Tee[:3, 3] @@ -340,10 +338,10 @@ class GotoEePlane(Node): self._send_action(traj, wait_s=3.0) def _named_pose(self, name: str, speed: float) -> bool: - """Call Flexiv /set_named_pose and wait until the arm is stationary.""" + """调用 Flexiv 的 ``/set_named_pose``，并等待机械臂静止。""" cli = self.create_client(SetNamedPose, "/set_named_pose") if not cli.wait_for_server(timeout_sec=8.0): - print("ERROR: /set_named_pose unavailable — start run_real.sh", flush=True) + print("错误：``/set_named_pose`` 不可用 —— 请启动 run_real.sh", flush=True) return False req = SetNamedPose.Request() req.pose_name = name @@ -355,16 +353,16 @@ class GotoEePlane(Node): while not fut.done() and time.monotonic() - t0 < 90.0: self._spin(0.1) if not fut.done(): - print("ERROR: set_named_pose timeout", flush=True) + print("错误：set_named_pose 超时", flush=True) return False resp = fut.result() print( - f" [B] resp success={resp.success} msg={resp.message}", + f" [B] 响应 success={resp.success} msg={resp.message}", flush=True, ) if not resp.success: return False - # wait until the flange stops moving (Δp < 1 mm over ~0.8 s) + # 等待法兰停止运动（约 0.8 s 内 Δp < 1 mm） last = None still = 0.0 t0 = time.monotonic() @@ -380,57 +378,57 @@ class GotoEePlane(Node): else: still = 0.0 last = p.copy() - print(" WARN: still-moving wait timeout — continue anyway", flush=True) + print(" 警告：等待静止超时 —— 仍然继续", flush=True) return True def run(self, timeout_s: float = 25.0) -> int: cfg = self.cfg q, Tee = self._wait_state(4.0) if q is None or Tee is None: - print("ERROR: no joint_states / TF — start run_real.sh", flush=True) + print("错误：没有 joint_states / TF —— 请启动 run_real.sh", flush=True) return 2 if not self._cli.wait_for_server(timeout_sec=8.0): - print("ERROR: JTC action unavailable — start run_real.sh", flush=True) + print("错误：JTC action 不可用 —— 请启动 run_real.sh", flush=True) return 3 p = Tee[:3, 3] safe_z = float(cfg["safe_z"]) print( - f"=== return to Flexiv initial pose '{cfg['home_pose']}' " + f"=== 回到 Flexiv 初始位姿 '{cfg['home_pose']}' " f"safe_z={safe_z*1000:.0f}mm ===", flush=True, ) print( - f" now ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})", + f" 当前 ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})", flush=True, ) - # --- Phase A: pure-Z lift to the safe height before the home move --- + # --- 阶段 A：回 home 前先沿纯 Z 方向升到安全高度 --- if p[2] < safe_z - 0.01: lift_tgt = np.array([p[0], p[1], safe_z], dtype=float) - print(f" [A] lift Z {p[2]*1000:.0f} → {safe_z*1000:.0f}mm", flush=True) + print(f" [A] 抬升 Z {p[2]*1000:.0f} → {safe_z*1000:.0f}mm", flush=True) ok, q, Tee = self._goto( q, Tee, lift_tgt, False, 0.010, 0.0, "[A]lift", v=0.05 ) if not ok: - print("ERROR: lift phase failed — pin here", flush=True) + print("错误：抬升阶段失败 —— 就地 pin 住", flush=True) self._pin(q) return 4 - # --- Phase B: Flexiv named pose → real initial pose, good posture --- + # --- 阶段 B：Flexiv 命名位姿 → 真正的初始位姿，姿态良好 --- if not self._named_pose(cfg["home_pose"], cfg["home_speed"]): - print("ERROR: named-pose return failed — pin here", flush=True) + print("错误：回命名位姿失败 —— 就地 pin 住", flush=True) self._pin(q) return 4 - # --- Phase C: hold so the servo takes over from a stationary pose --- + # --- 阶段 C：保持住，让伺服从一个静止位姿接手 --- q, Tee = self._wait_state(2.0) if q is not None: self._pin(q) if Tee is not None: p = Tee[:3, 3] print( - f" at initial pose ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})", + f" 已到初始位姿 ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})", flush=True, ) return 0 diff --git a/src/arm_application/arm_application/handeye_calibrate.py b/src/arm_application/arm_application/handeye_calibrate.py index 2e985a4..d4fe70b 100644 --- a/src/arm_application/arm_application/handeye_calibrate.py +++ b/src/arm_application/arm_application/handeye_calibrate.py @@ -1,14 +1,14 @@ #!/usr/bin/env python3 -"""Full hand-eye pipeline: move -> stabilize chessboard -> capture -> solve -> cleanup. +"""完整手眼标定流水线：移动 → 棋盘稳定判定 → 采集 → 求解 → 清理。 -Supports eye_in_hand / eye_to_hand. +支持 eye_in_hand / eye_to_hand。 -Camera intrinsics priority: - 1) ROS CameraInfo topic (if publishing) - 2) RealSense D405/D435 EEPROM via pyrealsense2 (camera_source) - 3) Manual camera_fx/fy/cx/cy in YAML +相机内参优先级： + 1) ROS 的 CameraInfo 话题（若正在发布） + 2) 通过 pyrealsense2 读取 RealSense D405/D435 的 EEPROM（camera_source） + 3) YAML 中手写的 camera_fx/fy/cx/cy -Chessboard size (board_cols / board_rows / square_size_m) is user-configurable. +棋盘尺寸（board_cols / board_rows / square_size_m）可由用户配置。 """ from __future__ import annotations @@ -68,7 +68,7 @@ def _image_to_bgr(msg: Image) -> np.ndarray: img = data.reshape(h, w, 4) bgr = img[:, :, :3] return bgr[:, :, ::-1].copy() if enc == 'rgba8' else bgr - raise RuntimeError(f'Unsupported image encoding: {msg.encoding!r}') + raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}') def _camera_info_to_intrinsics(msg: CameraInfo) -> CameraIntrinsics: @@ -93,7 +93,7 @@ class HandeyeCalibrate(Node): self.declare_parameter('waypoint_positions', [0.0] * (12 * 7)) self.declare_parameter('min_poses', 5) - # Camera: manual | d405 | d435 + # 相机来源：manual | d405 | d435 self.declare_parameter('camera_source', 'manual') self.declare_parameter('camera_serial', '') self.declare_parameter('camera_namespace', '') @@ -107,12 +107,12 @@ class HandeyeCalibrate(Node): self.declare_parameter('camera_cy', 0.0) self.declare_parameter('camera_dist', [0.0, 0.0, 0.0, 0.0, 0.0]) - # Chessboard (customizable) + # 棋盘（可自定义） # 默认值必须与 site.yaml 的 handeye.board_* 及结果文件头里的实际板子一致。 # 真实使用的板：内角点 8x11，方格 15mm（见 handeye/results/*.yaml 的头部注释）。 # 这里原先写的是 9x6 / 0.025（另一张早期的板），一旦 site.yaml 漏配就会被 # 静默启用 —— 症状是"标定能顺利跑完，但结果明显不对"，极难排查。 - self.declare_parameter('board_cols', 8) # inner corners + self.declare_parameter('board_cols', 8) # 内角点 self.declare_parameter('board_rows', 11) self.declare_parameter('square_size_m', 0.015) @@ -205,18 +205,18 @@ class HandeyeCalibrate(Node): if not img_topic: self.get_logger().error( - 'image_topic empty. Set camera_source:=d405|d435 or fill image_topic.' + 'image_topic 为空。请设置 camera_source:=d405|d435，或填写 image_topic。' ) else: self.create_subscription( Image, img_topic, self._on_image, qos_profile_sensor_data ) - self.get_logger().info(f'Subscribed image: {img_topic} (BEST_EFFORT)') + self.get_logger().info(f'已订阅图像：{img_topic}（BEST_EFFORT）') if info_topic: self.create_subscription( CameraInfo, info_topic, self._on_info, qos_profile_sensor_data ) - self.get_logger().info(f'Subscribed camera_info: {info_topic} (BEST_EFFORT)') + self.get_logger().info(f'已订阅 camera_info：{info_topic}（BEST_EFFORT）') def _on_js(self, msg: JointState) -> None: self._latest_js = msg @@ -230,20 +230,20 @@ class HandeyeCalibrate(Node): def run(self) -> int: if self.mode not in ('eye_in_hand', 'eye_to_hand'): self.get_logger().error( - f'Unknown mode={self.mode!r}; use eye_in_hand or eye_to_hand' + f'未知的 mode={self.mode!r}；请使用 eye_in_hand 或 eye_to_hand' ) return 1 poses = self._load_poses() if len(poses) < self.min_poses: self.get_logger().error( - f'Only {len(poses)} filled poses (need >= {self.min_poses}). ' - 'Teach with: bash ~/ros2/scripts/run_handeye.sh teach && merge.' + f'只有 {len(poses)} 个已填写位姿（需要 >= {self.min_poses}）。' + '示教命令：bash ~/ros2/scripts/run_handeye.sh teach && merge。' ) return 1 if self.start_index >= len(poses): self.get_logger().error( - f'start_index {self.start_index} >= filled poses {len(poses)}' + f'start_index {self.start_index} >= 已填写位姿数 {len(poses)}' ) return 1 @@ -252,8 +252,8 @@ class HandeyeCalibrate(Node): if not self._client.wait_for_server(timeout_sec=15.0): self.get_logger().error( - f'Action [{self.action_name}] unavailable. ' - 'Start: bash ~/ros2/scripts/run_real.sh' + f'Action [{self.action_name}] 不可用。' + '请启动：bash ~/ros2/scripts/run_real.sh' ) return 2 @@ -262,11 +262,11 @@ class HandeyeCalibrate(Node): eye_to_hand = self.mode == 'eye_to_hand' self.get_logger().info( - f'Hand-eye [{self.mode}] camera_source={self.camera_source}: ' - f'{len(poses)} poses, board={self.board.cols}x{self.board.rows} ' - f'square={self.board.square_size}m, v={self.velocity_scale}' + f'手眼标定 [{self.mode}] camera_source={self.camera_source}：' + f'{len(poses)} 个位姿，棋盘={self.board.cols}x{self.board.rows} ' + f'方格={self.board.square_size}m，v={self.velocity_scale}' ) - self.get_logger().warn('Clear workspace; E-Stop ready. Ctrl+C aborts.') + self.get_logger().warn('清理工作空间；确认急停就绪。Ctrl+C 可中止。') Rg: List[np.ndarray] = [] tg: List[np.ndarray] = [] @@ -277,17 +277,17 @@ class HandeyeCalibrate(Node): try: for i in range(self.start_index, len(poses)): q = poses[i] - self._log_progress(i, len(poses), 'moving...') + self._log_progress(i, len(poses), '移动中...') code = self._move_one(q, self.move_time_sec) if code != 0: return code if not self._verify_joints(q, tol=0.08): self.get_logger().error( - f'[{i + 1}] arm did not reach target joints ' - '(controller reported OK but joints unchanged). ' - 'Restart run_real.sh; pendant AUTO Remote; clear fault. ' - 'Do NOT trust this calibration.' + f'[{i + 1}] 机械臂未到达目标关节位置 ' + '（控制器报告成功，但关节没有变化）。' + '请重启 run_real.sh；示教器切 AUTO Remote；清除故障。' + '不要相信这次标定结果。' ) return 10 @@ -301,16 +301,16 @@ class HandeyeCalibrate(Node): ok, std, corners, pattern, bgr = self._wait_stable() if not ok or corners is None or bgr is None: - msg = f'[{i + 1}] chessboard not stable (std={std:.3f})' + msg = f'[{i + 1}] 棋盘不稳定（std={std:.3f}）' if self.skip_unstable: - self.get_logger().warn(f'{msg} — skip') + self.get_logger().warn(f'{msg} —— 跳过') continue - self.get_logger().error(f'{msg}. Abort.') + self.get_logger().error(f'{msg}。中止。') return 7 ee = self._read_ee_pose() if ee is None: - self.get_logger().error('Failed to read ee_pose') + self.get_logger().error('读取 ee_pose 失败') return 8 assert self._intrinsics is not None @@ -349,20 +349,20 @@ class HandeyeCalibrate(Node): } ) self.get_logger().info( - f'[{i + 1}/{len(poses)}] CAPTURED ' + f'[{i + 1}/{len(poses)}] 已采集 ' f'(corner_std={std:.3f}px) -> {img_path.name}' ) except KeyboardInterrupt: - self.get_logger().info('Interrupted') + self.get_logger().info('已中断') return 0 if len(Rg) < self.min_poses: self.get_logger().error( - f'Only {len(Rg)} captures; need >= {self.min_poses} to solve' + f'只采集到 {len(Rg)} 次；求解至少需要 {self.min_poses} 次' ) return 9 - self.get_logger().info(f'Solving hand-eye with {len(Rg)} views...') + self.get_logger().info(f'用 {len(Rg)} 个视角求解手眼标定...') R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=eye_to_hand) name = ( 'T_cam_to_base (eye_to_hand)' @@ -371,7 +371,7 @@ class HandeyeCalibrate(Node): ) yaml_txt = format_transform_yaml(R, t, name) header = ( - f'# Hand-eye result\n' + f'# 手眼标定结果\n' f'# mode: {self.mode}\n' f'# camera_source: {self.camera_source}\n' f'# views: {len(Rg)}\n' @@ -385,17 +385,17 @@ class HandeyeCalibrate(Node): json.dumps(meta, indent=2), encoding='utf-8' ) - self.get_logger().info('======== Hand-eye result ========') + self.get_logger().info('======== 手眼标定结果 ========') for line in (header + yaml_txt).strip().splitlines(): self.get_logger().info(line) - self.get_logger().info(f'Saved -> {self.result_file}') + self.get_logger().info(f'已保存 -> {self.result_file}') if self.cleanup_images: if self.images_dir.exists(): shutil.rmtree(self.images_dir, ignore_errors=True) - self.get_logger().info('Calibration images deleted') + self.get_logger().info('标定图像已删除') else: - self.get_logger().info(f'Images kept in {self.images_dir}') + self.get_logger().info(f'图像保留在 {self.images_dir}') return 0 def _load_poses(self) -> List[List[float]]: @@ -407,8 +407,8 @@ class HandeyeCalibrate(Node): need = n_cfg * n_j if len(flat) < need: self.get_logger().warn( - f'waypoint_positions length {len(flat)} < num_poses*{n_j}={need}; ' - 'padding with zeros' + f'waypoint_positions 长度 {len(flat)} < num_poses*{n_j}={need}；' + '用零补齐' ) flat = flat + [0.0] * (need - len(flat)) poses: List[List[float]] = [] @@ -417,7 +417,7 @@ class HandeyeCalibrate(Node): if is_pose_filled(q): poses.append(q) else: - self.get_logger().warn(f'Pose slot {i} empty (all-zero) — skipped') + self.get_logger().warn(f'位姿槽 {i} 为空（全零）—— 已跳过') return poses def _resolve_intrinsics(self) -> bool: @@ -430,7 +430,7 @@ class HandeyeCalibrate(Node): self._intrinsics = _camera_info_to_intrinsics(self._latest_info) K = self._intrinsics.K self.get_logger().info( - f'Intrinsics from CameraInfo ({self._info_topic}): ' + f'内参来自 CameraInfo（{self._info_topic}）：' f'fx={K[0, 0]:.2f} fy={K[1, 1]:.2f} cx={K[0, 2]:.2f} cy={K[1, 2]:.2f}' ) return True @@ -445,19 +445,19 @@ class HandeyeCalibrate(Node): height=self.rs_height, ) except Exception as exc: # noqa: BLE001 - self.get_logger().error(f'RealSense intrinsics query failed: {exc}') + self.get_logger().error(f'RealSense 内参查询失败：{exc}') else: self._intrinsics = CameraIntrinsics(K=intr.as_K(), dist=intr.as_dist()) self.get_logger().info( - f'Intrinsics from RealSense {intr.model.upper()} ' - f'serial={intr.serial} {intr.width}x{intr.height}: ' + f'内参来自 RealSense {intr.model.upper()} ' + f'serial={intr.serial} {intr.width}x{intr.height}：' f'fx={intr.fx:.2f} fy={intr.fy:.2f} ' f'cx={intr.cx:.2f} cy={intr.cy:.2f}' ) if (intr.width, intr.height) != (self.rs_width, self.rs_height): self.get_logger().warn( - f'Requested {self.rs_width}x{self.rs_height}, ' - f'got {intr.width}x{intr.height} (closest profile)' + f'请求 {self.rs_width}x{self.rs_height}，' + f'实际得到 {intr.width}x{intr.height}（最接近的 profile）' ) return True @@ -468,8 +468,8 @@ class HandeyeCalibrate(Node): cy = float(self.get_parameter('camera_cy').value) if fx <= 0 or fy <= 0: self.get_logger().error( - 'No intrinsics. Set camera_source:=d405|d435, ' - 'or provide camera_info_topic, or fill camera_fx/fy/cx/cy.' + '没有内参。请设置 camera_source:=d405|d435，' + '或提供 camera_info_topic，或填写 camera_fx/fy/cx/cy。' ) return False dist = np.array( @@ -478,7 +478,7 @@ class HandeyeCalibrate(Node): K = np.array([[fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]], dtype=float) self._intrinsics = CameraIntrinsics(K=K, dist=dist) self.get_logger().info( - f'Intrinsics from YAML: fx={fx:.2f} fy={fy:.2f} cx={cx:.2f} cy={cy:.2f}' + f'内参来自 YAML：fx={fx:.2f} fy={fy:.2f} cx={cx:.2f} cy={cy:.2f}' ) return True @@ -561,7 +561,7 @@ class HandeyeCalibrate(Node): try: bgr = _image_to_bgr(self._latest_image) except Exception as exc: # noqa: BLE001 - self.get_logger().error(f'Image convert failed: {exc}') + self.get_logger().error(f'图像转换失败：{exc}') return False, last_std, None, None, None detected = detect_chessboard(bgr, self.board) corners = detected[0] if detected is not None else None @@ -574,9 +574,9 @@ class HandeyeCalibrate(Node): last_bgr = bgr now = time.monotonic() if now - last_log > 1.0: - seen = 'yes' if corners is not None else 'NO' + seen = '有' if corners is not None else '无' self.get_logger().info( - f' board={seen} corner_std=' + f' 棋盘={seen} 角点标准差=' f'{std if std < 1e8 else float("nan"):.3f}px' ) last_log = now @@ -586,7 +586,12 @@ class HandeyeCalibrate(Node): def _verify_joints(self, target: List[float], tol: float = 0.08) -> bool: - """Confirm joint_states reached target (detect fake trajectory success).""" + """确认 ``joint_states`` 真的走到了目标位置。 + + 用来识破"假的成功"：有些执行接口会立刻返回成功，但关节其实没动 + （或只动了一半）。这里在 2 秒内轮询关节角，逐轴比对容差 ``tol``（弧度）， + 只有全部落在容差内才算真到位。 + """ deadline = time.monotonic() + 2.0 while rclpy.ok() and time.monotonic() < deadline: rclpy.spin_once(self, timeout_sec=0.05) @@ -602,10 +607,10 @@ class HandeyeCalibrate(Node): continue err = float(np.sqrt(np.mean([(a - b) ** 2 for a, b in zip(actual, target)]))) self.get_logger().info( - f' joint RMSE vs target: {err:.4f} rad (tol={tol})' + f' 关节相对目标的 RMSE：{err:.4f} rad（tol={tol}）' ) return err <= tol - self.get_logger().error('No joint_states while verifying motion') + self.get_logger().error('校验运动时没有 joint_states') return False def _move_one(self, positions: List[float], duration: float) -> int: @@ -617,18 +622,18 @@ class HandeyeCalibrate(Node): goal.time_from_start = [float(duration)] send = self._client.send_goal_async(goal) - if not self._wait_future(send, 10.0, 'send goal'): + if not self._wait_future(send, 10.0, '发送目标'): return 3 handle = send.result() if handle is None or not handle.accepted: - self.get_logger().error('Goal rejected') + self.get_logger().error('目标被拒绝') return 4 res_fut = handle.get_result_async() - if not self._wait_future(res_fut, max(60.0, duration * 3.0), 'result'): + if not self._wait_future(res_fut, max(60.0, duration * 3.0), '结果'): return 5 result = res_fut.result().result if not result.success: - self.get_logger().error(f'Move failed: {result.message}') + self.get_logger().error(f'移动失败：{result.message}') return 6 return 0 @@ -637,7 +642,7 @@ class HandeyeCalibrate(Node): while rclpy.ok() and not fut.done(): rclpy.spin_once(self, timeout_sec=0.05) if time.monotonic() > deadline: - self.get_logger().error(f'Timeout waiting for {label}') + self.get_logger().error(f'等待 {label} 超时') return False return fut.done() @@ -656,7 +661,7 @@ class HandeyeCalibrate(Node): import cv2 if not cv2.imwrite(str(path), bgr): - raise RuntimeError(f'cv2.imwrite failed: {path}') + raise RuntimeError(f'cv2.imwrite 失败：{path}') def main(args=None) -> None: diff --git a/src/arm_application/arm_application/handeye_lib.py b/src/arm_application/arm_application/handeye_lib.py index 09fc3e7..ecf23d4 100644 --- a/src/arm_application/arm_application/handeye_lib.py +++ b/src/arm_application/arm_application/handeye_lib.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 -"""Hand-eye helpers: chessboard stabilize + AX=XB solve. +"""手眼标定辅助工具：棋盘稳定判定 + AX=XB 求解。 -Uses ``cv2.calibrateHandEye`` when present; otherwise Tsai-Lenz (NumPy). -Chessboard detection always needs OpenCV. +有 ``cv2.calibrateHandEye`` 时用它，否则用 Tsai-Lenz（NumPy 实现）。 +棋盘检测始终依赖 OpenCV。 """ from __future__ import annotations @@ -21,9 +21,9 @@ except Exception: # noqa: BLE001 @dataclass class BoardConfig: - cols: int # inner corners (width) - rows: int # inner corners (height) - square_size: float # meters + cols: int # 内角点数（宽） + rows: int # 内角点数（高） + square_size: float # 米 @dataclass @@ -78,9 +78,9 @@ def board_object_points(cfg: BoardConfig) -> np.ndarray: def detect_chessboard( image_bgr: np.ndarray, cfg: BoardConfig ) -> Optional[Tuple[np.ndarray, Tuple[int, int]]]: - """Return (corners, pattern_size) or None. pattern is (cols, rows) used.""" + """返回 (角点, pattern_size) 或 None。pattern 为实际使用的 (cols, rows)。""" if cv2 is None: - raise RuntimeError('OpenCV (cv2) is required for chessboard detection') + raise RuntimeError('棋盘检测需要 OpenCV（cv2）') gray = cv2.cvtColor(image_bgr, cv2.COLOR_BGR2GRAY) pattern = (int(cfg.cols), int(cfg.rows)) flags = cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_NORMALIZE_IMAGE @@ -105,7 +105,7 @@ def solve_board_pose( pattern: Optional[Tuple[int, int]] = None, ) -> Tuple[np.ndarray, np.ndarray]: if cv2 is None: - raise RuntimeError('OpenCV (cv2) is required for solvePnP') + raise RuntimeError('solvePnP 需要 OpenCV（cv2）') if pattern is None: pattern = (cfg.cols, cfg.rows) board = BoardConfig(pattern[0], pattern[1], cfg.square_size) @@ -114,7 +114,7 @@ def solve_board_pose( obj, corners, cam.K, cam.dist, flags=cv2.SOLVEPNP_ITERATIVE ) if not ok: - raise RuntimeError('solvePnP failed') + raise RuntimeError('solvePnP 失败') R, _ = cv2.Rodrigues(rvec) return R.astype(float), np.asarray(tvec, dtype=float).reshape(3, 1) @@ -172,10 +172,10 @@ def calibrate_hand_eye_tsai( R_target2cam: List[np.ndarray], t_target2cam: List[np.ndarray], ) -> Tuple[np.ndarray, np.ndarray]: - """Tsai-Lenz. Returns R_cam2gripper, t_cam2gripper.""" + """Tsai-Lenz 方法。返回 R_cam2gripper、t_cam2gripper。""" n = len(R_gripper2base) if n < 3: - raise ValueError('Need >= 3 poses') + raise ValueError('至少需要 3 个位姿') Hg = [Rt_to_T(R, t) for R, t in zip(R_gripper2base, t_gripper2base)] Hc = [Rt_to_T(R, t) for R, t in zip(R_target2cam, t_target2cam)] @@ -220,8 +220,8 @@ def calibrate_hand_eye( eye_to_hand: bool = False, ) -> Tuple[np.ndarray, np.ndarray]: """ - eye_in_hand -> X = cam -> gripper (mounted on flange) - eye_to_hand -> X = cam -> base (fixed camera) + eye_in_hand -> X = cam -> gripper（相机装在法兰上） + eye_to_hand -> X = cam -> base（相机固定安装） """ Rg = list(R_gripper2base) tg = list(t_gripper2base) @@ -240,7 +240,7 @@ def calibrate_hand_eye( ) return np.asarray(R, float), np.asarray(t, float).reshape(3, 1) except Exception: - pass # fall back to NumPy Tsai + pass # 退回 NumPy 版 Tsai return calibrate_hand_eye_tsai(Rg, tg, R_target2cam, t_target2cam) @@ -257,7 +257,7 @@ def format_transform_yaml(R: np.ndarray, t: np.ndarray, name: str) -> str: def is_pose_filled(joints: Sequence[float]) -> bool: - """False if all zeros / NaN placeholder.""" + """若全为零 / NaN 占位则返回 False。""" arr = np.asarray(joints, dtype=float) if arr.size != 7: return False diff --git a/src/arm_application/arm_application/handeye_realsense.py b/src/arm_application/arm_application/handeye_realsense.py index bebdfef..3d072e0 100644 --- a/src/arm_application/arm_application/handeye_realsense.py +++ b/src/arm_application/arm_application/handeye_realsense.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Query RealSense (D405 / D435) factory color intrinsics via pyrealsense2.""" +"""通过 pyrealsense2 查询 RealSense（D405 / D435）的出厂彩色内参。""" from __future__ import annotations @@ -19,7 +19,7 @@ class RealsenseIntrinsics: fy: float cx: float cy: float - dist: List[float] # OpenCV order, length 5 + dist: List[float] # OpenCV 顺序，长度 5 def as_K(self) -> np.ndarray: return np.array( @@ -32,7 +32,7 @@ class RealsenseIntrinsics: return np.array(d[:5], dtype=float).reshape(-1, 1) -# Default ROS topic namespaces when camera_source is d405/d435. +# camera_source 为 d405/d435 时的默认 ROS 话题命名空间。 REALSENSE_TOPIC_DEFAULTS = { 'd405': { 'image_topic': '/d405/color/image_raw', @@ -55,13 +55,13 @@ def _norm_model(model: str) -> str: def list_realsense_devices() -> List[Tuple[str, str]]: - """Return [(name, serial), ...].""" + """返回 [(name, serial), ...]。""" try: import pyrealsense2 as rs except Exception as exc: # noqa: BLE001 raise RuntimeError( - 'pyrealsense2 not installed. ' - 'Install in ros_flexiv: pip/conda install pyrealsense2' + '未安装 pyrealsense2。' + '请在 ros_flexiv 环境中安装：pip/conda install pyrealsense2' ) from exc out: List[Tuple[str, str]] = [] for d in rs.context().devices: @@ -79,27 +79,27 @@ def query_realsense_color_intrinsics( height: int = 720, ) -> RealsenseIntrinsics: """ - Read on-device color intrinsics for D405/D435 at the given resolution. + 在给定分辨率下读取 D405/D435 设备上的彩色内参。 - Does not start a streaming pipeline (uses profile metadata). + 不会启动流管道（只读取 profile 元数据）。 """ try: import pyrealsense2 as rs except Exception as exc: # noqa: BLE001 raise RuntimeError( - 'pyrealsense2 not installed. ' - 'Install in ros_flexiv: pip/conda install pyrealsense2' + '未安装 pyrealsense2。' + '请在 ros_flexiv 环境中安装：pip/conda install pyrealsense2' ) from exc want = _norm_model(model) if want not in ('d405', 'd435'): - raise ValueError(f'Unsupported model {model!r}; use d405 or d435') + raise ValueError(f'不支持的型号 {model!r}；请使用 d405 或 d435') hint = want.upper() # D405 / D435 serial = (serial or '').strip() devices = list(rs.context().devices) if not devices: - raise RuntimeError('No RealSense device connected') + raise RuntimeError('未连接任何 RealSense 设备') chosen = None for d in devices: @@ -119,14 +119,14 @@ def query_realsense_color_intrinsics( for d in devices ) raise RuntimeError( - f'No RealSense matching model={want!r} serial={serial!r}. ' - f'Connected: {avail or "none"}' + f'没有匹配 model={want!r} serial={serial!r} 的 RealSense。' + f'已连接的设备：{avail or "无"}' ) name = chosen.get_info(rs.camera_info.name) sn = chosen.get_info(rs.camera_info.serial_number) - best = None # (score, width, height, fx, fy, cx, cy, coeffs) + best = None # (评分, width, height, fx, fy, cx, cy, 畸变系数) for s in chosen.sensors: for p in s.profiles: if p.stream_type() != rs.stream.color: @@ -155,7 +155,7 @@ def query_realsense_color_intrinsics( best = cand if best is None: - raise RuntimeError(f'No color stream profile on {name} ({sn})') + raise RuntimeError(f'{name} ({sn}) 上没有彩色流 profile') _, w, h, fx, fy, cx, cy, coeffs = best while len(coeffs) < 5: @@ -174,7 +174,7 @@ def query_realsense_color_intrinsics( def is_realsense_source(name: str) -> bool: - """True for d405 / d435 and namespaced aliases (d405_left, d405_right).""" + """对 d405 / d435 及带命名空间的别名（d405_left、d405_right）返回 True。""" return _norm_model(name or "") in ("d405", "d435") @@ -185,9 +185,9 @@ def resolve_realsense_topics( image_topic: str = '', camera_info_topic: str = '', ) -> Tuple[str, str]: - """Fill default image/camera_info topics for d405/d435 if empty. + """为空时，为 d405/d435 填充默认的 image/camera_info 话题。 - ``model`` may be a topic namespace such as ``d405_left`` / ``d405_right``. + ``model`` 可以是 ``d405_left`` / ``d405_right`` 这类话题命名空间。 """ ns = (camera_namespace or model or 'd405').strip().strip('/') img = (image_topic or '').strip() @@ -210,7 +210,7 @@ def apply_realsense_exposure( gain: float | None = None, log=None, ) -> list[str]: - """Cap AE / kill projector. Wrist D405 overexposure is usually long AE + emitter.""" + """限制自动曝光 / 关闭投影器。腕部 D405 过曝通常是自动曝光过长 + 发射器导致。""" import pyrealsense2 as rs notes: list[str] = [] @@ -230,7 +230,7 @@ def apply_realsense_exposure( laser_opt = getattr(rs.option, "laser_power", None) if laser_opt is not None and not emitter_enabled: _set(sensor, laser_opt, float(laser_power)) - # Must disable AE *before* writing exposure; D405 firmware ignores exposure while AE is on. + # 必须在写曝光*之前*关闭自动曝光；D405 固件在自动曝光开启时会忽略曝光设置。 if _set(sensor, rs.option.enable_auto_exposure, 1.0 if auto_exposure else 0.0): notes.append(f"ae={int(bool(auto_exposure))}") pri = getattr(rs.option, "auto_exposure_priority", None) @@ -247,5 +247,5 @@ def apply_realsense_exposure( if gain is not None and _set(sensor, rs.option.gain, float(gain)): notes.append(f"gain={gain}") if log and notes: - log("RealSense exposure: " + ", ".join(dict.fromkeys(notes))) + log("RealSense 曝光设置：" + ", ".join(dict.fromkeys(notes))) return notes diff --git a/src/arm_application/arm_application/handeye_teach.py b/src/arm_application/arm_application/handeye_teach.py index fb323cf..245682c 100644 --- a/src/arm_application/arm_application/handeye_teach.py +++ b/src/arm_application/arm_application/handeye_teach.py @@ -1,26 +1,26 @@ #!/usr/bin/env python3 -"""Hand-eye teach: live preview + Free Drive + Enter to photo/record joints. +"""手眼示教：实时预览 + Free Drive + 按 Enter 拍照/记录关节。 -Two backends +两种后端 ------------ -ros (default, stack running): - - Joints from /get_arm_state - - Drag: teach pendant Free Drive + hold enable - Keep: bash ~/ros2/scripts/run_real.sh +ros（默认，整套栈正在运行）： + - 关节来自 ``/get_arm_state`` + - 拖拽：示教器 Free Drive + 按住使能键 + 需保持：bash ~/ros2/scripts/run_real.sh -rdk (software Free Drive): - - Stop run_real.sh first (RDK exclusive) - - Press 'd' to enable FloatingJoint drag via RDK - - Joints from RDK robot.states +rdk（软件 Free Drive）： + - 先停掉 run_real.sh（RDK 独占） + - 按 'd' 通过 RDK 启用 FloatingJoint 拖拽 + - 关节来自 RDK 的 robot.states source ~/ros2/scripts/env.sh ros2 run arm_application handeye_teach --ros-args -p backend:=rdk -UI keys (preview window) - Enter / Space photo + record joints - d enable Free Drive (rdk) / show pendant tip (ros) - s stop Free Drive (rdk) / hold - q / Esc finish +界面按键（预览窗口） + Enter / Space 拍照 + 记录关节 + d 启用 Free Drive（rdk）/ 显示示教器提示（ros） + s 停止 Free Drive（rdk）/ 保持 + q / Esc 结束 """ from __future__ import annotations @@ -40,21 +40,21 @@ from rclpy.node import Node HELP = """ ================================================ - Hand-eye TEACH: preview -> drag -> photo+pose + 手眼示教 TEACH：预览 -> 拖拽 -> 拍照+位姿 ================================================ - Window keys: - Enter / Space = take photo + save joints - d = Free Drive ON - s = Free Drive OFF / hold - q / Esc = finish - Tip: make board clear & fill ~30-70% of image - Prefer 8-12 poses with varied angles + 窗口按键： + Enter / Space = 拍照 + 保存关节 + d = 开启 Free Drive + s = 关闭 Free Drive / 保持 + q / Esc = 结束 + 提示：让棋盘清晰，并占到画面的约 30-70% + 建议采 8-12 个角度有变化的位姿 ================================================ """ def _setup_matplotlib_font(): - """Avoid Chinese mojibake: prefer CJK font if present.""" + """避免中文乱码：优先选用存在的中日韩（CJK）字体。""" import matplotlib matplotlib.use('QtAgg') @@ -77,7 +77,7 @@ def _setup_matplotlib_font(): chosen = name break if chosen is None: - # try by file path + # 按文件路径再试一次 for path in ( '/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc', '/usr/share/fonts/opentype/noto/NotoSansCJK-Medium.ttc', @@ -104,7 +104,7 @@ def _param_as_str(node: Node, name: str, default: str = '') -> str: class RdkBridge: - """Talk to Flexiv RDK via rdk_py310 subprocess (Python 3.10 ABI).""" + """通过 rdk_py310 子进程（Python 3.10 ABI）与 Flexiv RDK 通信。""" def __init__(self, robot_sn: str, logger=None) -> None: import json @@ -120,9 +120,9 @@ class RdkBridge: if not bridge.is_file(): bridge = Path('/home/cc/ros2/scripts/rdk_drag_bridge.py') if not bridge.is_file(): - raise FileNotFoundError(f'rdk_drag_bridge.py not found: {bridge}') + raise FileNotFoundError(f'找不到 rdk_drag_bridge.py：{bridge}') - # Only Flexiv libs — do not inherit conda LD_LIBRARY_PATH (can hang RDK). + # 只保留 Flexiv 的库 —— 不要继承 conda 的 LD_LIBRARY_PATH（会让 RDK 卡死）。 flexiv_lib = os.path.expanduser('~/flexiv_install/lib') env = { 'HOME': os.environ.get('HOME', ''), @@ -130,9 +130,9 @@ class RdkBridge: 'LD_LIBRARY_PATH': flexiv_lib, 'LANG': 'C.UTF-8', } - # Do NOT inherit conda PYTHONPATH. - # IMPORTANT: do not use PIPE for stderr — RDK logs fill the pipe and - # deadlock the bridge (stdout never replies). Log to a file instead. + # 不要继承 conda 的 PYTHONPATH。 + # 重要：stderr 不能用 PIPE —— RDK 日志会填满管道并让桥接进程死锁 + # （stdout 永远不返回）。改成写入日志文件。 log_path = Path('/tmp/rdk_drag_bridge.log') self._err_file = open(log_path, 'w', encoding='utf-8') self._proc = subprocess.Popen( @@ -145,14 +145,14 @@ class RdkBridge: env=env, ) if logger: - logger.info(f'RDK bridge started; log → {log_path}') + logger.info(f'RDK 桥接已启动；日志 → {log_path}') pong = self._ask({'cmd': 'ping'}, timeout=8.0) if not pong.get('ok'): raise RuntimeError( - f'RDK bridge ping failed: {pong}. See {log_path}' + f'RDK 桥接 ping 失败：{pong}。参见 {log_path}' ) if logger: - logger.info('RDK bridge ping OK, connecting to robot...') + logger.info('RDK 桥接 ping 正常，正在连接机器人...') conn = self._ask( { 'cmd': 'connect', @@ -163,11 +163,11 @@ class RdkBridge: ) if not conn.get('ok'): raise RuntimeError( - f'RDK connect failed: {conn.get("error")}. ' - f'Stop run_real.sh first; AUTO Remote + clear fault. See {log_path}' + f'RDK 连接失败：{conn.get("error")}。' + f'请先停掉 run_real.sh；切 AUTO Remote 并清除故障。参见 {log_path}' ) if logger: - logger.info(f'RDK bridge OK (py={pong.get("py")}) sn={robot_sn}') + logger.info(f'RDK 桥接就绪（py={pong.get("py")}）sn={robot_sn}') q0 = conn.get('q') or [] if len(q0) >= 7: self._last_q = [float(v) for v in q0[:7]] @@ -175,9 +175,9 @@ class RdkBridge: def _ask(self, msg: dict, timeout: float = 10.0) -> dict: import select - # One in-flight command: camera/GUI must never share this pipe. + # 同一时刻只允许一条在途命令：相机/GUI 绝不能共用这个管道。 if not self._io.acquire(timeout=max(timeout, 0.2)): - return {'ok': False, 'error': 'bridge busy'} + return {'ok': False, 'error': '桥接忙'} try: assert self._proc.stdin and self._proc.stdout self._proc.stdin.write(self._json.dumps(msg) + '\n') @@ -188,7 +188,7 @@ class RdkBridge: tip = '/tmp/rdk_drag_bridge.log' return { 'ok': False, - 'error': f'bridge exited rc={self._proc.returncode}; see {tip}', + 'error': f'桥接已退出 rc={self._proc.returncode}；参见 {tip}', } ready, _, _ = select.select([self._proc.stdout], [], [], 0.05) if not ready: @@ -202,8 +202,8 @@ class RdkBridge: try: return self._json.loads(line) except Exception as exc: # noqa: BLE001 - return {'ok': False, 'error': f'bad reply: {exc} {line!r}'} - return {'ok': False, 'error': 'bridge timeout'} + return {'ok': False, 'error': f'应答异常：{exc} {line!r}'} + return {'ok': False, 'error': '桥接超时'} finally: self._io.release() @@ -296,14 +296,14 @@ class HandeyeTeach(Node): self._start_gui() print(HELP) if self.backend == 'rdk': - print('[backend=rdk] Press d = soft-drag (works in AUTO Remote, no Floating license)') - print(' run_real.sh must be STOPPED.') + print('[backend=rdk] 按 d = 软拖拽（在 AUTO Remote 下可用，无需 Floating 许可）') + print(' run_real.sh 必须已停止。') print(' 拖着就能按 Enter 拍照，不必先按 s') else: - print('[backend=ros] Drag with pendant Free Drive + hold enable.') - print(' Or stop run_real.sh and use: -p backend:=rdk') + print('[backend=ros] 用示教器 Free Drive + 按住使能键来拖拽。') + print(' 或停掉 run_real.sh 后改用：-p backend:=rdk') if font: - self.get_logger().info(f'UI font: {font}') + self.get_logger().info(f'界面字体：{font}') self.create_timer(1.0 / max(1, self._fps), self._tick) def _init_ros_state(self) -> None: @@ -312,15 +312,15 @@ class HandeyeTeach(Node): self._get_state = self.create_client(GetArmState, 'get_arm_state') if not self._get_state.wait_for_service(timeout_sec=10.0): raise RuntimeError( - 'get_arm_state unavailable. Start run_real.sh, ' - 'or use: -p backend:=rdk (after stopping the stack)' + 'get_arm_state 不可用。请启动 run_real.sh，' + '或改用：-p backend:=rdk（需先停掉整套栈）' ) def _init_rdk(self) -> None: - self.get_logger().info(f'Connecting RDK bridge to {self.robot_sn} ...') + self.get_logger().info(f'正在把 RDK 桥接连到 {self.robot_sn} ...') self._rdk = RdkBridge(self.robot_sn, logger=self.get_logger()) - self._robot = True # marker: rdk backend ready - self.get_logger().info('RDK connected. Press d for Free Drive.') + self._robot = True # 标记：RDK 后端已就绪 + self.get_logger().info('RDK 已连接。按 d 启用 Free Drive。') def _start_camera(self) -> None: import pyrealsense2 as rs @@ -342,7 +342,7 @@ class HandeyeTeach(Node): cfg.enable_stream(rs.stream.color, ww, hh, rs.format.bgr8, ff) profile = pipe.start(cfg) self._fps = ff - self.get_logger().info(f'Camera {ww}x{hh}@{ff}Hz') + self.get_logger().info(f'相机 {ww}x{hh}@{ff}Hz') break except Exception as exc: # noqa: BLE001 last_err = exc @@ -353,7 +353,7 @@ class HandeyeTeach(Node): pipe = rs.pipeline() if profile is None: raise RuntimeError( - f'Cannot open D405 — stop realsense_color_pub first. ({last_err})' + f'无法打开 D405 —— 请先停掉 realsense_color_pub。（{last_err}）' ) self._pipe = pipe try: @@ -371,7 +371,7 @@ class HandeyeTeach(Node): log=self.get_logger().info, ) except Exception as exc: # noqa: BLE001 - self.get_logger().warn(f'exposure apply: {exc}') + self.get_logger().warn(f'曝光设置应用失败：{exc}') self._cam_thread = threading.Thread(target=self._cam_loop, name='d405', daemon=True) self._cam_thread.start() @@ -395,7 +395,7 @@ class HandeyeTeach(Node): self._plt = plt self._fig, self._ax = plt.subplots(1, 1, figsize=(11, 7)) self._fig.canvas.manager.set_window_title( - 'handeye_teach | Enter=photo d=drag s=stop q=quit' + 'handeye_teach | Enter=拍照 d=拖拽 s=停止 q=退出' ) self._im = None self._status = self._ax.text( @@ -415,21 +415,21 @@ class HandeyeTeach(Node): plt.show(block=False) def _status_text(self) -> str: - drag = 'DRAG ON' if self._drag_on else 'drag off' + drag = '拖拽开启' if self._drag_on else '拖拽关闭' if self.backend == 'rdk': - tip = 'd=soft-drag s=hold' + tip = 'd=软拖拽 s=保持' else: - tip = 'Pendant FreeDrive+Enable' + tip = '示教器 FreeDrive+使能' return ( - f'captured:{len(self._captures)} [{drag}] ' - f'{tip} | Enter=photo q=quit' + f'已采集:{len(self._captures)} [{drag}] ' + f'{tip} | Enter=拍照 q=退出' ) def _on_key(self, event) -> None: key = (event.key or '').lower() if key in ('q', 'escape'): self._quit = True - self.get_logger().info('Quit requested') + self.get_logger().info('收到退出请求') return if self._cmd_pending: return @@ -446,7 +446,7 @@ class HandeyeTeach(Node): elif key == 's': self._disable_drag() except Exception as exc: # noqa: BLE001 - self.get_logger().error(f'key {key}: {exc}') + self.get_logger().error(f'按键 {key}：{exc}') finally: self._cmd_pending = False @@ -454,34 +454,34 @@ class HandeyeTeach(Node): if self.backend != 'rdk': self._drag_on = True self.get_logger().warn( - 'ROS backend: use teach pendant Free Drive + hold enable button. ' - 'For software drag: stop run_real.sh, then ' + 'ROS 后端：请用示教器的 Free Drive + 按住使能键。' + '想用软件拖拽：先停掉 run_real.sh，然后执行 ' 'ros2 run arm_application handeye_teach --ros-args -p backend:=rdk' ) self._status.set_text(self._status_text()) return - self.get_logger().info('starting soft-drag (preview stays live) ...') - # soft = joint impedance drag under AUTO Remote (no Floating license) + self.get_logger().info('正在启动软拖拽（预览保持实时刷新）...') + # soft = AUTO Remote 模式下的关节阻抗拖拽（不需要 Floating 许可） res = self._rdk.drag_on(self.drag_primitive, mode='soft') if res.get('ok'): self._drag_on = True self.get_logger().info( - f'Drag ON mode={res.get("mode")} — push the arm, Enter=photo (s=hold optional)' + f'拖拽已开启 mode={res.get("mode")} —— 推着机械臂走，Enter=拍照（s=保持，可选）' ) if res.get('hint'): self.get_logger().info(str(res.get('hint'))) else: self._drag_on = False - self.get_logger().error(f'Drag failed: {res.get("error")}') - # status text is refreshed by _tick on the GUI thread + self.get_logger().error(f'拖拽失败：{res.get("error")}') + # 状态文字由 GUI 线程里的 _tick 刷新 def _disable_drag(self) -> None: if self.backend == 'rdk' and getattr(self, '_rdk', None) is not None: res = self._rdk.drag_off() if not res.get('ok'): - self.get_logger().warn(f'Stop drag: {res.get("error")}') + self.get_logger().warn(f'停止拖拽：{res.get("error")}') self._drag_on = False - self.get_logger().info('Free Drive OFF / hold') + self.get_logger().info('Free Drive 已关闭 / 保持') def _tick(self) -> None: if self._quit: @@ -510,7 +510,7 @@ class HandeyeTeach(Node): if self.backend == 'rdk' and getattr(self, '_rdk', None) is not None: q = self._rdk.joints() if q is None: - self.get_logger().error('RDK joints failed') + self.get_logger().error('读取 RDK 关节失败') return q from arm_msgs.srv import GetArmState @@ -532,11 +532,11 @@ class HandeyeTeach(Node): with self._frame_lock: bgr = None if self._latest_bgr is None else self._latest_bgr.copy() if bgr is None: - self.get_logger().warn('No camera frame yet') + self.get_logger().warn('还没有相机帧') return q = self._read_joints() if q is None: - self.get_logger().error('Failed to read joints') + self.get_logger().error('读取关节失败') return idx = len(self._captures) @@ -569,7 +569,7 @@ class HandeyeTeach(Node): if target > 0: if n < target: self.get_logger().info( - f'[{n}/{target}] saved photo+joints —— 还差 {target - n} 个点' + f'[{n}/{target}] 已保存照片+关节 —— 还差 {target - n} 个点' ) elif n == target: self.get_logger().info( @@ -579,18 +579,18 @@ class HandeyeTeach(Node): ) else: self.get_logger().info( - f'[{n}/{target}] saved photo+joints —— 已超出目标 ' + f'[{n}/{target}] 已保存照片+关节 —— 已超出目标 ' f'{n - target} 个点（会用全部点求解）' ) else: - self.get_logger().info(f'[{n}] saved photo+joints') + self.get_logger().info(f'[{n}] 已保存照片+关节') self._write_yaml_snippet() finally: self._busy = False def _write_yaml_snippet(self) -> None: lines = [ - '# Paste into handeye_calibrate.yaml -> waypoint_positions', + '# 粘贴到 handeye_calibrate.yaml -> waypoint_positions', f'# num_poses: {len(self._captures)}', 'waypoint_positions: [', ] @@ -612,17 +612,17 @@ class HandeyeTeach(Node): except Exception: pass n = len(self._captures) - print('\n========== TEACH DONE ==========') - print(f'poses: {n}') - print(f'joints: {self.poses_file}') + print('\n========== 示教完成 ==========') + print(f'位姿: {n}') + print(f'关节: {self.poses_file}') print(f'yaml: {self.yaml_file}') - print(f'images: {self.images_dir}') + print(f'图像: {self.images_dir}') if n >= 1: - print('\n--- paste into handeye_calibrate.yaml ---\n') + print('\n--- 粘贴到 handeye_calibrate.yaml ---\n') print(self.yaml_file.read_text(encoding='utf-8')) - print('Also set num_poses:', n) + print('同时设置 num_poses:', n) if n < 5: - self.get_logger().warn(f'Only {n} poses; need >=5 (prefer 8-12).') + self.get_logger().warn(f'只有 {n} 个位姿；至少需要 5 个（建议 8-12 个）。') def main(args=None) -> None: diff --git a/src/arm_application/arm_application/handeye_waypoints.py b/src/arm_application/arm_application/handeye_waypoints.py index 2d6dc6a..ab1bd1e 100644 --- a/src/arm_application/arm_application/handeye_waypoints.py +++ b/src/arm_application/arm_application/handeye_waypoints.py @@ -1,17 +1,16 @@ #!/usr/bin/env python3 -"""Hand-eye calibration waypoint runner. +"""手眼标定路点执行器。 -Moves to each joint waypoint one-by-one, waits for arrival, then dwells so a -camera (or operator) can capture a calibration image. +逐个移动到每个关节路点，等到位后停留一段时间， +让相机（或操作者）采集一张标定图像。 capture_mode: - auto — dwell_sec then next pose - keypress — terminal Enter to continue - preview — OpenCV live camera + Enter/Space confirm (recommended for teaching) + auto — 停留 dwell_sec 后进入下一个位姿 + keypress — 在终端按 Enter 继续 + preview — OpenCV 实时相机 + Enter/Space 确认（示教时推荐） -During ``preview`` mode, use the teach pendant **Free Drive** to drag-adjust -while the preview window is open (ros2_control keeps AUTO Remote; drag is -via pendant, not RDK). +处于 ``preview`` 模式时，可在预览窗口打开期间用示教器的 **Free Drive** +拖拽微调（ros2_control 保持 AUTO Remote；拖拽经由示教器，而非 RDK）。 """ from __future__ import annotations @@ -106,7 +105,7 @@ def _image_to_bgr(msg: Image) -> np.ndarray: img = data.reshape(h, w, 4) bgr = img[:, :, :3] return bgr[:, :, ::-1].copy() if enc == 'rgba8' else bgr - raise RuntimeError(f'Unsupported image encoding: {msg.encoding!r}') + raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}') class HandeyeWaypoints(Node): @@ -165,14 +164,14 @@ class HandeyeWaypoints(Node): self._cv_gui = _cv_gui_available() and self.preview_window if self.capture_mode == 'preview' and not self._cv_gui: self.get_logger().warn( - 'OpenCV has no GUI (headless). Preview on /handeye/preview_image; ' - 'confirm with Enter in this terminal.' + 'OpenCV 没有 GUI（无头环境）。请在 /handeye/preview_image 上预览，' + '在本终端按 Enter 确认。' ) if self.image_topic and self.capture_mode == 'preview': self.create_subscription( Image, self.image_topic, self._on_image, qos_profile_sensor_data ) - self.get_logger().info(f'Preview image: {self.image_topic}') + self.get_logger().info(f'预览图像：{self.image_topic}') def _on_image(self, msg: Image) -> None: try: @@ -183,10 +182,10 @@ class HandeyeWaypoints(Node): def run(self) -> int: if self.capture_mode == 'preview': if cv2 is None: - self.get_logger().error('preview mode needs OpenCV (cv2)') + self.get_logger().error('preview 模式需要 OpenCV（cv2）') return 1 if not self.image_topic: - self.get_logger().error('preview mode needs image_topic or camera_source') + self.get_logger().error('preview 模式需要 image_topic 或 camera_source') return 1 times = list(self.get_parameter('waypoint_times').value) @@ -194,23 +193,23 @@ class HandeyeWaypoints(Node): n_j = len(self.joint_names) n_pts = len(times) if n_pts < 1: - self.get_logger().error('waypoint_times is empty') + self.get_logger().error('waypoint_times 为空') return 1 if len(positions) != n_pts * n_j: self.get_logger().error( - f'waypoint_positions length {len(positions)} != {n_pts}*{n_j}' + f'waypoint_positions 长度 {len(positions)} != {n_pts}*{n_j}' ) return 1 if self.start_index >= n_pts: self.get_logger().error( - f'start_index {self.start_index} >= num waypoints {n_pts}' + f'start_index {self.start_index} >= 路点总数 {n_pts}' ) return 1 if not self._client.wait_for_server(timeout_sec=15.0): self.get_logger().error( - f'Action [{self.action_name}] not available. ' - 'Start stack first: bash scripts/run_real.sh' + f'Action [{self.action_name}] 不可用。' + '请先启动整套栈：bash scripts/run_real.sh' ) return 2 @@ -226,18 +225,18 @@ class HandeyeWaypoints(Node): + [f'target_{n}' for n in self.joint_names] + [f'measured_{n}' for n in self.joint_names] ) - self.get_logger().info(f'Logging captures → {path}') + self.get_logger().info(f'正在记录采集数据 → {path}') if self.capture_mode == 'preview': print(PREVIEW_HELP_GUI if self._cv_gui else PREVIEW_HELP_HEADLESS) self.get_logger().info( - f'Hand-eye: {n_pts} poses, start={self.start_index}, ' - f'v_scale={self.velocity_scale}, dwell={self.dwell_sec}s, ' + f'手眼标定：{n_pts} 个位姿，start={self.start_index}，' + f'v_scale={self.velocity_scale}，dwell={self.dwell_sec}s，' f'mode={self.capture_mode}' ) self.get_logger().warn( - 'Clear workspace; keep board in FOV. Ctrl+C aborts.' + '清理工作空间；让棋盘保持在视野内。Ctrl+C 可中止。' ) win = 'handeye_preview' @@ -246,7 +245,7 @@ class HandeyeWaypoints(Node): q = positions[i * n_j : (i + 1) * n_j] move_t = max(1.0, float(times[i])) self.get_logger().info( - f'[{i + 1}/{n_pts}] moving (~{move_t:.1f}s)...' + f'[{i + 1}/{n_pts}] 移动中（约 {move_t:.1f}s）...' ) code = self._move_one(q, move_t) if code != 0: @@ -264,10 +263,10 @@ class HandeyeWaypoints(Node): csv_file.flush() if not self._wait_capture(i, n_pts): - self.get_logger().info('Aborted by user') + self.get_logger().info('用户已中止') return 0 - # After confirm in preview mode, save taught joints + # 在 preview 模式下确认后，保存示教关节 if self.capture_mode == 'preview': taught = self._read_joints() if taught: @@ -281,10 +280,10 @@ class HandeyeWaypoints(Node): except Exception: # noqa: BLE001 pass - self.get_logger().info('Hand-eye waypoint sequence done') + self.get_logger().info('手眼标定路点序列执行完毕') if self.append_poses_file and self.capture_mode == 'preview': self.get_logger().info( - f'Taught poses saved → {self.append_poses_file}' + f'示教位姿已保存 → {self.append_poses_file}' ) return 0 @@ -299,7 +298,7 @@ class HandeyeWaypoints(Node): with path.open('a', encoding='utf-8') as f: f.write(f'# pose {index}\n') f.write(line + '\n') - self.get_logger().info(f'Saved pose {index} → {path}') + self.get_logger().info(f'已保存位姿 {index} → {path}') def _move_one(self, positions: List[float], duration: float) -> int: goal = FollowJointTrajectory.Goal() @@ -310,19 +309,19 @@ class HandeyeWaypoints(Node): goal.time_from_start = [float(duration)] send = self._client.send_goal_async(goal) - if not self._wait_future(send, 10.0, 'send goal'): + if not self._wait_future(send, 10.0, '发送目标'): return 3 handle = send.result() if handle is None or not handle.accepted: - self.get_logger().error('Goal rejected') + self.get_logger().error('目标被拒绝') return 4 res_fut = handle.get_result_async() - if not self._wait_future(res_fut, max(60.0, duration * 3.0), 'result'): + if not self._wait_future(res_fut, max(60.0, duration * 3.0), '结果'): return 5 result = res_fut.result().result if not result.success: - self.get_logger().error(f'Move failed: {result.message}') + self.get_logger().error(f'移动失败：{result.message}') return 6 return 0 @@ -331,7 +330,7 @@ class HandeyeWaypoints(Node): while rclpy.ok() and not fut.done(): rclpy.spin_once(self, timeout_sec=0.05) if time.monotonic() > deadline: - self.get_logger().error(f'Timeout waiting for {label}') + self.get_logger().error(f'等待 {label} 超时') return False return fut.done() @@ -359,11 +358,11 @@ class HandeyeWaypoints(Node): q_str = ', '.join(f'{v:.4f}' for v in (measured or target)) if self.capture_mode == 'preview': self.get_logger().info( - f'[{index + 1}/{n_pts}] ARRIVED — Free Drive adjust, then Enter/Space' + f'[{index + 1}/{n_pts}] 已到位 —— 用 Free Drive 调整，然后按 Enter/Space' ) else: self.get_logger().info( - f'[{index + 1}/{n_pts}] ARRIVED — CAPTURE NOW q=[{q_str}]' + f'[{index + 1}/{n_pts}] 已到位 —— 现在采集 q=[{q_str}]' ) def _wait_capture(self, index: int, n_pts: int) -> bool: @@ -372,7 +371,7 @@ class HandeyeWaypoints(Node): if self.capture_mode in ('key', 'keypress', 'manual'): self.get_logger().info( - 'Press Enter after capturing (q+Enter to quit)...' + '采集完成后按 Enter（q+Enter 退出）...' ) for _ in range(5): rclpy.spin_once(self, timeout_sec=0.05) @@ -394,7 +393,7 @@ class HandeyeWaypoints(Node): return vis cv2.putText( vis, - f'Pose {index + 1}/{n_pts}', + f'位姿 {index + 1}/{n_pts}', (12, 28), cv2.FONT_HERSHEY_SIMPLEX, 0.7, @@ -403,7 +402,7 @@ class HandeyeWaypoints(Node): ) cv2.putText( vis, - 'Free Drive drag | Enter=next q=quit', + 'Free Drive 拖拽 | Enter=下一个 q=退出', (12, 56), cv2.FONT_HERSHEY_SIMPLEX, 0.6, @@ -420,7 +419,7 @@ class HandeyeWaypoints(Node): msg.encoding = 'bgr8' msg.is_bigendian = 0 msg.step = msg.width * 3 - msg.data = array.array('B', bgr.tobytes()) # fast path; bytes costs ~40 ms + msg.data = array.array('B', bgr.tobytes()) # 快速路径；用 bytes 会多花约 40 ms self._preview_pub.publish(msg) def _wait_preview(self, index: int, n_pts: int) -> bool: @@ -444,8 +443,8 @@ class HandeyeWaypoints(Node): confirmed.set() self.get_logger().info( - f'[{index + 1}/{n_pts}] Adjust with Free Drive, then press Enter ' - '(q+Enter to quit)' + f'[{index + 1}/{n_pts}] 用 Free Drive 调整，然后按 Enter ' + '（q+Enter 退出）' ) reader = threading.Thread(target=_stdin_reader, daemon=True) reader.start() @@ -464,7 +463,7 @@ class HandeyeWaypoints(Node): if bgr is None: if not no_frame_warn: self.get_logger().warn( - f'Waiting for {self.image_topic} ...', + f'正在等待 {self.image_topic} ...', throttle_duration_sec=3.0, ) no_frame_warn = True @@ -485,7 +484,7 @@ class HandeyeWaypoints(Node): if bgr is None: if not no_frame_warn: self.get_logger().warn( - f'Waiting for {self.image_topic} ...', + f'正在等待 {self.image_topic} ...', throttle_duration_sec=3.0, ) no_frame_warn = True diff --git a/src/arm_application/arm_application/hole_detect_node.py b/src/arm_application/arm_application/hole_detect_node.py index 4137ede..ae547fa 100644 --- a/src/arm_application/arm_application/hole_detect_node.py +++ b/src/arm_application/arm_application/hole_detect_node.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Hole detection on the current-arm D405 color topic. Detect-only (no insert motion).""" +"""在当前机械臂的 D405 彩色话题上做入孔检测。只检测，不做插入动作。""" from __future__ import annotations diff --git a/src/arm_application/arm_application/hole_detector.py b/src/arm_application/arm_application/hole_detector.py index 4cd7742..87bdc57 100644 --- a/src/arm_application/arm_application/hole_detector.py +++ b/src/arm_application/arm_application/hole_detector.py @@ -60,7 +60,10 @@ _HOLE_SITE_SKIP = {"weights", "workpiece_weights", "config"} def config_from_site(data: dict[str, Any] | None = None) -> dict[str, Any]: - """Package yaml defaults, then overlay site.yaml hole_detect: (operator source of truth).""" + """先取包内 yaml 默认值，再用 ``site.yaml`` 的 ``hole_detect:`` 覆盖。 + + 以 ``site.yaml`` 为准（operator 的唯一数据源）；``data`` 参数用于测试时直接喂字典。 + """ from arm_application.site_io import deep_merge, load_site_dict, ros2_root site = data if data is not None else load_site_dict() diff --git a/src/arm_application/arm_application/kalman_filter.py b/src/arm_application/arm_application/kalman_filter.py index 8439fa7..2de4e58 100644 --- a/src/arm_application/arm_application/kalman_filter.py +++ b/src/arm_application/arm_application/kalman_filter.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Constant-velocity Kalman filter for a 3D point (camera frame).""" +"""三维点的匀速模型卡尔曼滤波（坐标系为相机系）。""" from __future__ import annotations @@ -9,7 +9,7 @@ import numpy as np class KalmanFilter3D: - """State ``[x, y, z, vx, vy, vz]``. Measurement is position only.""" + """状态量为 ``[x, y, z, vx, vy, vz]``；观测只有位置，没有速度。""" def __init__( self, @@ -43,7 +43,11 @@ class KalmanFilter3D: return self.x[3:].copy() def advance(self, t: float) -> None: - """Predict the state forward to time ``t`` (seconds, monotonic).""" + """把状态预测到时刻 ``t``（秒，必须单调递增）。 + + 超过 0.5 秒的空档按"目标可能已经跑掉"处理：位置不动、速度清零， + 否则旧速度会把位置外推得很离谱。 + """ if not self.initialized or self._t is None: self._t = t return @@ -51,7 +55,7 @@ class KalmanFilter3D: if dt <= 1e-6: return if dt > 0.5: - # Long gap — keep position, zero velocity. + # 空档太长——位置保持不动，速度清零。 self.x[3:] = 0.0 self._t = t return @@ -59,7 +63,7 @@ class KalmanFilter3D: self._t = t def update(self, z: np.ndarray) -> None: - """Measurement update with a 3D position.""" + """用一次三维位置观测做量测更新。""" z = np.asarray(z, dtype=float).reshape(3) if not self.initialized: self.reset(z) diff --git a/src/arm_application/arm_application/move_to_pose_client.py b/src/arm_application/arm_application/move_to_pose_client.py index c3c3873..bd53285 100644 --- a/src/arm_application/arm_application/move_to_pose_client.py +++ b/src/arm_application/arm_application/move_to_pose_client.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 -"""Example client for arm_msgs/action/MoveToPose (Cartesian goal). +"""``arm_msgs/action/MoveToPose``（笛卡尔目标）的示例客户端。 -Wire this to MoveIt or your IK solver action server when available. +等你有了 MoveIt 或自己的 IK 求解 action server 后，把它接上去即可。 """ from __future__ import annotations @@ -24,8 +24,8 @@ class MoveToPoseClient(Node): def send(self, x: float, y: float, z: float) -> None: if not self._client.wait_for_server(timeout_sec=5.0): self.get_logger().error( - 'move_to_pose action not available. ' - 'Start a Cartesian planner (e.g. MoveIt) that implements it.' + 'move_to_pose action 不可用。' + '请启动一个实现了它的笛卡尔规划器（例如 MoveIt）。' ) return @@ -36,18 +36,18 @@ class MoveToPoseClient(Node): goal.target_pose.pose.position.x = x goal.target_pose.pose.position.y = y goal.target_pose.pose.position.z = z - # Identity orientation + # 单位姿态（不旋转） goal.target_pose.pose.orientation.w = 1.0 goal.velocity_scale = 0.3 goal.acceleration_scale = 0.3 goal.avoid_collisions = True - self.get_logger().info(f'Sending Cartesian goal: ({x:.3f}, {y:.3f}, {z:.3f})') + self.get_logger().info(f'发送笛卡尔目标：({x:.3f}, {y:.3f}, {z:.3f})') future = self._client.send_goal_async(goal) rclpy.spin_until_future_complete(self, future) handle = future.result() if handle is None or not handle.accepted: - self.get_logger().error('Goal rejected') + self.get_logger().error('目标被拒绝') return result_future = handle.get_result_async() @@ -59,7 +59,7 @@ class MoveToPoseClient(Node): def main(args=None) -> None: rclpy.init(args=args) node = MoveToPoseClient() - # Default demo pose; override via CLI: x y z + # 默认演示位姿；可通过命令行覆盖：x y z vals = [0.35, 0.0, 0.35] if len(sys.argv) >= 4: vals = [float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3])] diff --git a/src/arm_application/arm_application/orisys_tactile.py b/src/arm_application/arm_application/orisys_tactile.py index e25e7ed..2ff58a8 100644 --- a/src/arm_application/arm_application/orisys_tactile.py +++ b/src/arm_application/arm_application/orisys_tactile.py @@ -1,4 +1,4 @@ -"""Orisys visuo-tactile adapter. SDK is pip-installed; site.yaml holds device ids.""" +"""Orisys 视触觉适配器。SDK 通过 pip 安装；设备 id 存放在 ``site.yaml`` 里。""" from __future__ import annotations @@ -185,8 +185,8 @@ class OrisysTactile(TactileInterface): import orisys except ImportError as exc: raise RuntimeError( - "orisys SDK missing. pip install -e /path/to/orisys-sdk " - "(git.orisys-tech.cn/orisys/test). Config still comes from site.yaml tactile." + "缺少 orisys SDK。请执行 pip install -e /path/to/orisys-sdk " + "（git.orisys-tech.cn/orisys/test）。配置依旧从 ``site.yaml`` 的 ``tactile`` 段读取。" ) from exc self._sensors = [] diff --git a/src/arm_application/arm_application/perception_node.py b/src/arm_application/arm_application/perception_node.py index 469f603..01772f6 100644 --- a/src/arm_application/arm_application/perception_node.py +++ b/src/arm_application/arm_application/perception_node.py @@ -1,13 +1,13 @@ #!/usr/bin/env python3 -"""perception_node — YOLO + depth + KF (own process). +"""perception_node —— YOLO + 深度 + 卡尔曼滤波（独立进程）。 -Hard rules: -- Image callbacks ONLY store latest frames (never run YOLO in the callback) -- YOLO runs on a dedicated worker thread so MutuallyExclusive I/O is never blocked -- SensorDataQoS BEST_EFFORT keep_last(5) -- Publish filtered 3D as geometry_msgs/PointStamped (no Services) -- Dynamic object TF at vision rate only -- High-confidence detections always reacquire (no freeze on fast motion) +硬性规则： +- 图像回调**只**存最新帧（绝不在回调里跑 YOLO） +- YOLO 跑在专门的工作线程上，保证 ``MutuallyExclusive`` 的 I/O 回调永远不会被阻塞 +- 用 ``SensorDataQoS`` ``BEST_EFFORT`` ``keep_last(5)`` +- 滤波后的三维点用 ``geometry_msgs/PointStamped`` 发布（不用 Service） +- 动态物体 TF 只在视觉帧率下发布 +- 高置信度检测始终重新捕获（快速运动时不会冻结） """ from __future__ import annotations @@ -50,7 +50,7 @@ from arm_application.yolo_detector import ( def _bgr_copy(msg: Image) -> np.ndarray: - """BGR uint8 owned by caller (safe for async YOLO; ROS may drop the msg).""" + """返回调用方持有的 BGR uint8 数组（对异步 YOLO 安全；ROS 可能把原消息丢掉）。""" h, w = int(msg.height), int(msg.width) enc = (msg.encoding or "").lower() buf = np.frombuffer(msg.data, dtype=np.uint8) @@ -58,7 +58,7 @@ def _bgr_copy(msg: Image) -> np.ndarray: return buf.reshape(h, w, 3).copy() if enc == "rgb8": return buf.reshape(h, w, 3)[:, :, ::-1].copy() - raise RuntimeError(f"Unsupported encoding {msg.encoding!r}") + raise RuntimeError(f"不支持的编码 {msg.encoding!r}") class PerceptionNode(Node): @@ -66,7 +66,7 @@ class PerceptionNode(Node): super().__init__("perception_node") tid = threading.get_ident() self.get_logger().warn( - f"perception_node started pid={os.getpid()} thread_id={tid}" + f"perception_node 已启动 pid={os.getpid()} thread_id={tid}" ) self.declare_parameter("camera_source", "d405") @@ -86,30 +86,30 @@ class PerceptionNode(Node): self.declare_parameter("jump_z_m", 0.20) self.declare_parameter("min_depth_m", 0.04) self.declare_parameter("max_depth_m", 0.55) - self.declare_parameter("work_z_max_m", 0.50) # above this = hole/background + self.declare_parameter("work_z_max_m", 0.50) # 高于此值 = 孔 / 背景 self.declare_parameter("object_frame", "tracked_object") self.declare_parameter("camera_frame", "d405_color_optical_frame") self.declare_parameter("kf_q_pos", 2.0e-3) self.declare_parameter("kf_q_vel", 5.0e-2) self.declare_parameter("kf_r_pos", 8.0e-4) - self.declare_parameter("meas_blend", 0.65) # 1=raw meas, 0=pure KF + self.declare_parameter("meas_blend", 0.65) # 1 = 原始测量值，0 = 纯卡尔曼滤波 self.declare_parameter("half_precision", True) self.declare_parameter("backend", "auto") # auto|torch|tensorrt - self.declare_parameter("mp_infer", True) # YOLO in a spawn() process - # Adaptive Z LPF: alpha_z = clamp(z_alpha_min + |vel_z|*z_alpha_gain, min, max) + self.declare_parameter("mp_infer", True) # YOLO 跑在 spawn() 子进程里 + # 自适应 Z 低通滤波：alpha_z = clamp(z_alpha_min + |vel_z|*z_alpha_gain, min, max) self.declare_parameter("z_alpha_min", 0.10) self.declare_parameter("z_alpha_max", 0.60) self.declare_parameter("z_alpha_gain", 2.0) - # Faster YOLO: downscale before infer; boxes scaled back to full res + # 加速 YOLO：推理前先降采样；检测框再缩放回全分辨率 self.declare_parameter("infer_max_width", 0) - # Box EMA (1=raw YOLO every frame — tight follow; lower = smoother/lag) + # 检测框 EMA（1 = 每帧都用原始 YOLO —— 紧跟目标；更低 = 更平滑但滞后） self.declare_parameter("box_ema_alpha", 0.85) - # Brief YOLO misses: keep last box on preview; clear only after N frames + # YOLO 短暂漏检：预览里保留上一个框；只有连续 N 帧都漏检才清除 self.declare_parameter("miss_clear_frames", 12) self.declare_parameter("overlay_hold_on_miss", True) self.declare_parameter("hold_point_on_miss", True) self.declare_parameter("miss_hold_point_frames", 2) - # plane = stereo surface centroid (recommended); box = YOLO box center + # plane = 立体表面质心（推荐）；box = YOLO 检测框中心 self.declare_parameter("feature_mode", "plane") self.declare_parameter("plane_inlier_m", 0.008) self.declare_parameter("plane_band_m", 0.06) @@ -170,7 +170,7 @@ class PerceptionNode(Node): self._stop = threading.Event() self._wake = threading.Event() - # I/O group: only lightweight store-latest (never YOLO) + # I/O 回调组：只做轻量的“存最新帧”（绝不跑 YOLO） self._io_cbg = MutuallyExclusiveCallbackGroup() self._wake_cbg = MutuallyExclusiveCallbackGroup() @@ -195,7 +195,7 @@ class PerceptionNode(Node): self._yolo_proc: Optional[YoloInferProcess] = None if self.mp_infer: self._yolo_proc = YoloInferProcess(yolo_cfg) - self.detector = self._yolo_proc # duck-typed infer() + self.detector = self._yolo_proc # 鸭子类型，只要求有 infer() imgsz = self._yolo_proc.imgsz self.get_logger().info( f"YOLO worker pid={self._yolo_proc._p.pid} weights={self._yolo_proc.weights_path}" @@ -221,7 +221,7 @@ class PerceptionNode(Node): try: self.detector.warmup(half=self._yolo_half) except Exception as exc: # noqa: BLE001 - self.get_logger().warn(f"YOLO warmup failed: {exc}") + self.get_logger().warn(f"YOLO 预热失败：{exc}") imgsz = self.detector.imgsz image_qos = QoSProfile( @@ -256,8 +256,8 @@ class PerceptionNode(Node): depth_topic = f"/{cam_ns}/aligned_depth_to_color/image_raw" if not img_topic: raise RuntimeError( - f"image_topic empty (camera_source={cam_src!r} ns={ns!r}); " - "use d405 / d405_left / d405_right" + f"image_topic 为空（camera_source={cam_src!r} ns={ns!r}）；" + "请使用 d405 / d405_left / d405_right" ) self.get_logger().info( f"perception image={img_topic} depth={depth_topic} info={info_topic or '-'}" @@ -283,7 +283,7 @@ class PerceptionNode(Node): self._point_pub = self.create_publisher( PointStamped, "visual_servo/filtered_point", point_qos ) - # Reliable + keep_last(5): camera preview tolerates brief DDS gaps + # RELIABLE + keep_last(5)：相机预览能容忍短暂的 DDS 丢包间隙 det_qos = QoSProfile( history=HistoryPolicy.KEEP_LAST, depth=5, @@ -298,7 +298,7 @@ class PerceptionNode(Node): self._worker.start() self.get_logger().info( - f"perception ready vision≤{vision_hz:.0f}Hz imgsz={imgsz} " + f"perception 就绪 vision≤{vision_hz:.0f}Hz imgsz={imgsz} " f"infer_max_w={self.infer_max_width} mp={int(self.mp_infer)} " f"backend={self.backend_name} half={self._yolo_half} " f"feature={self.feature_mode} image={img_topic}" @@ -328,8 +328,8 @@ class PerceptionNode(Node): w = float(msg.width) self._fx = 654.8 if w >= 1200 else 393.9 * (w / 640.0) self._fy = 654.0 if w >= 1200 else 393.4 * (float(msg.height) / 480.0) - # Do NOT pickle the image into the YOLO queue here — that stalls the - # camera callback and looks like inference freeze. Infer thread submits. + # 千万不要在这里把图像 pickle 进 YOLO 队列 —— 那会卡住相机回调， + # 看起来就像是推理冻结。由推理线程负责提交。 self._wake.set() def _on_info(self, msg: CameraInfo) -> None: @@ -349,7 +349,7 @@ class PerceptionNode(Node): self._wake.set() def _submit_latest(self) -> None: - """Copy the newest camera frame and hand it to the GPU process.""" + """复制最新的相机帧，交给 GPU 进程。""" msg = self._latest_image seq = int(self._frame_seq) if msg is None or seq <= self._submitted_seq: @@ -361,38 +361,37 @@ class PerceptionNode(Node): self.detector.submit(small, seq) self._submitted_seq = seq except Exception as exc: # noqa: BLE001 - self.get_logger().warn(f"yolo submit: {exc}", throttle_duration_sec=2.0) + self.get_logger().warn(f"yolo 提交失败：{exc}", throttle_duration_sec=2.0) def _restart_yolo(self, why: str) -> None: - """Self-heal a hung/dead GPU worker (engine reload takes a few sec).""" + """自愈：重启卡死 / 挂掉的 GPU 工作进程（重新加载 engine 需要几秒）。""" now = time.monotonic() if now - self._last_restart_t < 5.0: return if not hasattr(self.detector, "restart"): return self._last_restart_t = now - self.get_logger().error(f"YOLO worker {why} — restarting (box coasts)") + self.get_logger().error(f"YOLO 工作进程 {why} —— 正在重启（检测框靠惯性滑行）") try: self.detector.restart() self._submitted_seq = -1 self._last_gpu_t = time.monotonic() - self.get_logger().info("YOLO worker restarted") + self.get_logger().info("YOLO 工作进程已重启") except Exception as exc: # noqa: BLE001 - self.get_logger().error(f"YOLO restart failed: {exc}") + self.get_logger().error(f"YOLO 重启失败：{exc}") def _infer_loop(self) -> None: - """Submit latest frame, collect GPU result, then stereo. + """提交最新帧，收集 GPU 结果，再做立体匹配。 - GPU must never wait on stereo: the worker drops old results if we are - briefly busy. We also submit *before* waiting so the next infer is - already queued. + GPU 绝不能等立体匹配：如果我们短暂正忙，工作进程会直接丢掉旧结果。 + 我们还会**先**提交再去等，这样下一次推理已经排进队列了。 """ while not self._stop.is_set(): self._submit_latest() try: got = self.detector.wait_result(timeout=0.030) except Exception as exc: # noqa: BLE001 - self.get_logger().error(f"infer: {exc}", throttle_duration_sec=2.0) + self.get_logger().error(f"推理：{exc}", throttle_duration_sec=2.0) if not getattr(self.detector, "alive", True): self._restart_yolo("died") time.sleep(0.02) @@ -402,13 +401,13 @@ class PerceptionNode(Node): silent = (now - self._last_gpu_t) if self._last_gpu_t else 0.0 if self._last_gpu_t is not None and silent > 0.25: self.get_logger().warn( - f"YOLO silent {silent*1000:.0f}ms " - f"(worker={'up' if getattr(self.detector, 'alive', True) else 'DEAD'})", + f"YOLO 静默 {silent*1000:.0f}ms " + f"(worker={'存活' if getattr(self.detector, 'alive', True) else '已死'})", throttle_duration_sec=1.0, ) outstanding = self._submitted_seq > self._done_seq if outstanding and self._last_gpu_t is not None and silent > 2.0: - self._restart_yolo(f"hung {silent:.1f}s") + self._restart_yolo(f"卡死 {silent:.1f}s") continue if ( self._last_yolo_t is not None @@ -429,7 +428,7 @@ class PerceptionNode(Node): try: self._apply_detections(dets, scale, t_img) except Exception as exc: # noqa: BLE001 - self.get_logger().error(f"apply: {exc}", throttle_duration_sec=2.0) + self.get_logger().error(f"应用检测结果出错：{exc}", throttle_duration_sec=2.0) self._done_seq = int(seq) self._submit_latest() dt_ms = (time.monotonic() - t0) * 1000.0 @@ -457,13 +456,13 @@ class PerceptionNode(Node): def _depth_in_box(self, xyxy) -> tuple: msg = self._latest_depth if msg is None: - return None, "no depth" + return None, "无深度" if not self._depth_fresh(msg): - return None, "stale depth" + return None, "深度过期" enc = (msg.encoding or "").lower() buf = self._decode_depth_m(msg) if buf is None: - return None, "bad depth" + return None, "深度无效" h, w = buf.shape x1, y1, x2, y2 = [float(v) for v in xyxy] img = self._latest_image @@ -471,7 +470,7 @@ class PerceptionNode(Node): sx, sy = w / float(img.width), h / float(img.height) x1, y1, x2, y2 = x1 * sx, y1 * sy, x2 * sx, y2 * sy bw, bh = max(8.0, x2 - x1), max(8.0, y2 - y1) - # Inner ROI — avoid box edges; still keep enough pixels on a holey workpiece + # 内部 ROI —— 避开检测框边缘；但对有孔的工件仍要保留足够像素 ix0 = int(np.clip(x1 + 0.20 * bw, 0, w - 1)) iy0 = int(np.clip(y1 + 0.20 * bh, 0, h - 1)) ix1 = int(np.clip(x2 - 0.20 * bw, ix0 + 2, w)) @@ -491,7 +490,7 @@ class PerceptionNode(Node): scale = 1.0 vals = meters[np.isfinite(meters) & (meters > self.min_depth) & (meters < self.max_depth)] if vals.size < 8: - # Inner ROI is often holes; retry the full box. + # 内部 ROI 经常落在孔上；改用整个检测框重试。 y0 = int(np.clip(y1, 0, h - 1)) x0 = int(np.clip(x1, 0, w - 1)) y1b = int(np.clip(y2, y0 + 2, h)) @@ -509,8 +508,8 @@ class PerceptionNode(Node): if vals.size < 8: med_m = float(np.median(meters)) if meters.size else 0.0 return None, f"ok={vals.size} scale={scale} med={med_m:.3f}" - # Near-surface: holes / table / far background must NOT pull Z out. - # Take closest 40%, then keep a 6 cm band around the 10th percentile. + # 只取近表面：孔 / 桌面 / 远处背景绝不能把 Z 拉偏。 + # 先取最近的 40%，再围绕第 10 百分位保留一个 6 cm 的带宽。 vals = np.sort(vals) n_near = max(8, int(0.40 * vals.size)) near = vals[:n_near] @@ -524,7 +523,7 @@ class PerceptionNode(Node): return z, f"ok={vals.size} near={band.size}" def _adaptive_filter_z(self, z_raw: float, now: float) -> float: - """Item 4: adaptive LPF on depth — quiet when still, responsive when moving.""" + """第 4 项：对深度做自适应低通滤波 —— 静止时更稳，运动时更灵敏。""" if self._z_filt is None or self._z_filt_t is None: self._z_filt = float(z_raw) self._z_filt_t = now @@ -563,7 +562,7 @@ class PerceptionNode(Node): ) def _publish_detection_clear(self) -> None: - """Empty detection → preview clears green box (must NOT reuse last xyxy).""" + """空检测 → 预览清掉绿框（绝不能复用上一次的 xyxy）。""" self._last_det_payload = None msg = Float32MultiArray() msg.data = [] @@ -589,7 +588,7 @@ class PerceptionNode(Node): self._det_pub.publish(msg) def _prepare_infer(self, bgr: np.ndarray) -> tuple: - """Optional downscale for YOLO; return (infer_bgr, scale) full = infer * scale.""" + """可选的 YOLO 降采样；返回 (infer_bgr, scale)，全分辨率 = 推理图 * scale。""" max_w = self.infer_max_width h, w = bgr.shape[:2] if max_w <= 0 or w <= max_w: @@ -612,7 +611,7 @@ class PerceptionNode(Node): return (int(msg.width), int(msg.height)) def _coast_box(self, note: str) -> bool: - """Keep the green box moving with last velocity; do not drive 3D.""" + """让绿框按上一次的速度继续移动；不要用它驱动三维。""" now = time.monotonic() pred = self._box_trk.predict(now, self._img_wh()) if pred is None and self._last_raw_xyxy is not None: @@ -626,7 +625,7 @@ class PerceptionNode(Node): self._publish_detection_box( pred, float(self._last_box_conf), float(self._last_box_cls), uv, z ) - self.get_logger().info(f"box coast {note}", throttle_duration_sec=0.5) + self.get_logger().info(f"检测框惯滑 {note}", throttle_duration_sec=0.5) return True def _clear_track(self) -> None: @@ -640,7 +639,7 @@ class PerceptionNode(Node): self._publish_detection_clear() def _apply_detections(self, dets, scale: float, t_img: float) -> None: - """Match YOLO, publish a *now*-predicted box, then stereo on that box.""" + """匹配 YOLO 结果，发布按当前时刻预测的检测框，再在该框上做立体匹配。""" if scale != 1.0: for d in dets: d.xyxy = d.xyxy * scale @@ -665,7 +664,7 @@ class PerceptionNode(Node): self._clear_track() self._last_yolo_t = None self.get_logger().info( - "detection miss — cleared overlay", + "检测漏检 —— 已清除叠加显示", throttle_duration_sec=1.0, ) else: @@ -691,7 +690,7 @@ class PerceptionNode(Node): self._last_box_conf = float(best.conf) self._last_box_cls = float(best.cls_id) self._box_trk.update(xyxy_raw, t_img) - # Compensate infer delay: draw where the part is *now*, not where YOLO saw it + # 补偿推理延迟：画工件**当前**所在的位置，而不是 YOLO 看到它时的位置 xyxy = self._box_trk.predict(now, self._img_wh()) if xyxy is None: xyxy = xyxy_raw @@ -705,8 +704,8 @@ class PerceptionNode(Node): xyxy, float(best.conf), float(best.cls_id), box_uv, z_preview ) - # Hover target = stereo 3D on the workpiece inside the box (not a ray - # through a hole / table, and not z_ref). Red dot = that 3D point. + # 悬停目标 = 检测框内工件表面的立体三维点（不是穿过孔 / 桌面的射线， + # 也不是 z_ref）。红点 = 该三维点。 feat = None p_cam: Optional[np.ndarray] = None note = "box" @@ -727,13 +726,13 @@ class PerceptionNode(Node): z_ok = z_meas is not None z, note = self._metric_z(z_meas, note) if z is None: - # Never had usable depth (target beyond D405 range / not in the work - # area / camera mount or handeye off). Do NOT publish a z_ref point — - # that would fake-lock the gripper over a phantom. Hold instead. + # 从未拿到过可用深度（目标超出 D405 量程 / 不在工作区内 / 相机安装或 + # 手眼标定有问题）。不要发布 z_ref 点 —— 那会在幻影位置上假装锁定夹爪。 + # 此时应保持不动。 self.get_logger().warn( - f"target has NO usable depth ({note}) — NOT tracking. " - f"Put the workpiece in view at {self.min_depth:.2f}-{self.work_z_max:.2f} m " - "from the camera, or check camera mount / handeye calibration.", + f"目标没有可用深度（{note}）—— 不跟踪。" + f"请把工件放到相机视野内 {self.min_depth:.2f}-{self.work_z_max:.2f} m 处，" + "或检查相机安装 / 手眼标定。", throttle_duration_sec=1.0, ) return @@ -754,7 +753,7 @@ class PerceptionNode(Node): if dpx > self.jump_px or (z_ok and dz > self.jump_z_m): self.kf.reset(p_cam, now) self.get_logger().info( - f"reacquire conf={best.conf:.2f} dpx={dpx:.0f}", + f"重新捕获 conf={best.conf:.2f} dpx={dpx:.0f}", throttle_duration_sec=0.5, ) else: @@ -765,7 +764,7 @@ class PerceptionNode(Node): kf_p = self.kf.position blend = self.meas_blend filt = blend * p_cam + (1.0 - blend) * kf_p - # Keep XY scale honest even if KF still has an old far-Z + # 即使卡尔曼里还残留着旧的远距离 Z，也要保证 XY 的尺度真实 z_use = float(np.clip(filt[2], self.min_depth, self.work_z_max)) if abs(z_use - float(p_cam[2])) > 0.04: filt = p_cam.copy() @@ -775,14 +774,14 @@ class PerceptionNode(Node): self._publish_point(filt, stamp) self.get_logger().info( - f"track {self.feature_mode} conf={best.conf:.2f} uv=({uv[0]:.0f},{uv[1]:.0f}) " + f"跟踪 {self.feature_mode} conf={best.conf:.2f} uv=({uv[0]:.0f},{uv[1]:.0f}) " f"z={filt[2]:.3f} z_ok={int(z_ok)} {note}", throttle_duration_sec=0.4, ) def _depth_fresh(self, msg: Image) -> bool: - """Reject stale depth — on a fast-moving part, old depth at the current - box reads the background (table at ~2.4 m) and poisons the 3D point.""" + """拒绝过期的深度 —— 对快速运动的工件，当前检测框处的旧深度会读到背景 + （约 2.4 m 处的桌面），从而污染三维点。""" try: t = rclpy.time.Time.from_msg(msg.header.stamp) age = (self.get_clock().now() - t).nanoseconds * 1e-9 @@ -816,7 +815,7 @@ class PerceptionNode(Node): band_m=self.plane_band_m, ) except Exception as exc: # noqa: BLE001 - self.get_logger().warn(f"inner3d feature: {exc}", throttle_duration_sec=2.0) + self.get_logger().warn(f"inner3d 特征计算失败：{exc}", throttle_duration_sec=2.0) return None def _plane_center(self, xyxy): @@ -846,13 +845,13 @@ class PerceptionNode(Node): band_m=self.plane_band_m, ) except Exception as exc: # noqa: BLE001 - self.get_logger().warn(f"plane feature: {exc}", throttle_duration_sec=2.0) + self.get_logger().warn(f"平面特征计算失败：{exc}", throttle_duration_sec=2.0) return None def _metric_z(self, z_meas: Optional[float], note: str) -> tuple: - """Z used for pixel→metre. Never use hole/floor depth (inflates XY error). - Returns (None, note) when this target NEVER had usable depth — publishing - a z_ref point then would fake-lock the servo onto a phantom XY.""" + """用于像素→米的 Z。绝不能用孔 / 地面的深度（会放大 XY 误差）。 + 当这个目标**从未**有过可用深度时返回 (None, note) —— 那种情况下发布 + z_ref 点会让伺服假锁到一个幻影 XY 上。""" z_ref = max(0.12, float(self.z_ref)) if z_meas is not None and self.min_depth <= z_meas <= self.work_z_max: self._last_good_z = float(z_meas) diff --git a/src/arm_application/arm_application/pid_controller.py b/src/arm_application/arm_application/pid_controller.py index bc54a4d..5819675 100644 --- a/src/arm_application/arm_application/pid_controller.py +++ b/src/arm_application/arm_application/pid_controller.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""PID with integral clamp, derivative-on-measurement, and integral separation.""" +"""PID 控制器，带积分限幅、微分作用于测量值（derivative-on-measurement）以及积分分离。""" from __future__ import annotations @@ -7,10 +7,10 @@ from typing import Optional class PidController: - """``u = Kp e + Ki ∫e + Kd (−dmeas/dt)``, then saturate. + """``u = Kp e + Ki ∫e + Kd (−dmeas/dt)``，然后再做饱和限幅。 - Integral separation: when ``|error| > i_sep_thresh``, integral is forced to 0 - and does not accumulate (P+D only for fast chase / less overshoot). + 积分分离：当 ``|error| > i_sep_thresh`` 时，积分被强制置 0 且不再累积 + （只用 P+D，以便快速追赶 / 减少超调）。 """ def __init__( @@ -50,7 +50,7 @@ class PidController: dterm = -(x - self._meas_prev) / dt self._meas_prev = x - # Integral separation — large error: clear I, do not accumulate + # 积分分离 —— 误差较大：清空 I，不再累积 if self.i_sep_thresh > 0.0 and abs(err) > self.i_sep_thresh: self._i = 0.0 else: diff --git a/src/arm_application/arm_application/plane_feature.py b/src/arm_application/arm_application/plane_feature.py index c215171..84c43ad 100644 --- a/src/arm_application/arm_application/plane_feature.py +++ b/src/arm_application/arm_application/plane_feature.py @@ -1,10 +1,9 @@ #!/usr/bin/env python3 -"""Stereo plane Z at the YOLO box center. +"""在 YOLO 检测框中心处求立体平面的 Z。 -The hover target is the box-center ray intersected with the fitted near -plane. That stays on the detection, with metric Z (30° camera: wrong Z -would shift base XY). The cloud-mean centroid is not used — holes / table -inliers pull it off the green box. +悬停目标是「检测框中心射线」与「拟合出的近平面」的交点。这样既能留在检测结果上， +又带有真实的米制 Z（30° 相机下，Z 错了会把 base 系的 XY 也带偏）。不使用点云均值 +质心 —— 孔 / 桌面的内点会把它从绿框上拉走。 """ from __future__ import annotations @@ -17,9 +16,9 @@ import numpy as np @dataclass class PlaneFeature: - uv: np.ndarray # (2,) color-pixel coords of the 3D centroid - z: float # camera-frame Z of the 3D centroid - xyz: np.ndarray # (3,) stereo-cloud centroid in camera frame + uv: np.ndarray # (2,) 三维质心对应的彩色像素坐标 + z: float # 三维质心在相机坐标系下的 Z + xyz: np.ndarray # (3,) 相机坐标系下的立体点云质心 n_inliers: int note: str @@ -128,10 +127,10 @@ def workpiece_xyz_in_box( inner: float = 0.40, band_m: float = 0.06, ) -> Optional[PlaneFeature]: - """Physical workpiece point: median 3D of the near surface in the inner box. + """工件上的实体点：内层检测框里近表面深度的三维中位数。 - Not a monocular ray (holes punch through to the table) and not the - whole-ROI cloud mean (table / one-sided depth pulls it off the part). + 不是单目射线（孔会穿透到桌面），也不是整个 ROI 的点云均值 + （桌面 / 单侧深度会把它从工件上拉走）。 """ meters, _ = _depth_meters(depth_buf, encoding) if meters.ndim != 2: @@ -212,7 +211,7 @@ def geometric_center_on_plane( inlier_m: float = 0.008, band_m: float = 0.06, ) -> Optional[PlaneFeature]: - """Box-center point on the fitted near plane, or None.""" + """拟合出的近平面上、位于检测框中心的点，拿不到则返回 None。""" meters, _ = _depth_meters(depth_buf, encoding) if meters.ndim != 2: return None @@ -224,7 +223,7 @@ def geometric_center_on_plane( if dw != img_w or dh != img_h: sx, sy = dw / max(float(img_w), 1.0), dh / max(float(img_h), 1.0) x1, y1, x2, y2 = x1 * sx, y1 * sy, x2 * sx, y2 * sy - # Keep uv in color pixels: work in depth pixels then scale back. + # uv 保持用彩色像素坐标：先在深度像素下计算，再缩放回来。 scale_u, scale_v = 1.0 / sx, 1.0 / sy else: scale_u = scale_v = 1.0 @@ -246,7 +245,7 @@ def geometric_center_on_plane( yy, xx = np.nonzero(near) zs = roi[yy, xx] - # Color-equivalent pixel for back-projection (aligned depth uses color K). + # 反投影用的彩色等效像素（对齐后的深度使用的是彩色内参 K）。 us = (xx + x0) * scale_u vs = (yy + y0) * scale_v pts = np.stack( diff --git a/src/arm_application/arm_application/realsense_color_pub.py b/src/arm_application/arm_application/realsense_color_pub.py index bf2db7b..f6b5e4a 100644 --- a/src/arm_application/arm_application/realsense_color_pub.py +++ b/src/arm_application/arm_application/realsense_color_pub.py @@ -1,18 +1,18 @@ #!/usr/bin/env python3 -"""Publish RealSense color stream to ROS topics (no realsense2_camera apt pkg). +"""把 RealSense 彩色流发布到 ROS 话题（不依赖 realsense2_camera 的 apt 包）。 -Use when ``ros-humble-realsense2-camera`` is not installed but ``pyrealsense2`` works. +当你没装 ``ros-humble-realsense2-camera``、但 ``pyrealsense2`` 能正常工作时用它。 -Default topics (camera_name=d405): +默认话题（camera_name=d405）： /d405/color/image_raw /d405/color/camera_info - /d405/aligned_depth_to_color/image_raw (16UC1 mm, if enable_depth) + /d405/aligned_depth_to_color/image_raw (16UC1 mm，需开启 enable_depth) -Performance notes ------------------ -* Publishes from the capture thread (no timer lag). -* Uses SensorDataQoS (BEST_EFFORT, depth=1) so slow subscribers drop old frames. -* Optional SDK preview: ``-p show_preview:=true`` (Intel sample path: pipeline → numpy → window). +性能说明 +-------- +* 在采集线程里直接发布（没有定时器带来的延迟）。 +* 使用 SensorDataQoS（BEST_EFFORT，depth=1），这样慢的订阅者会主动丢旧帧。 +* 可选的 SDK 预览：``-p show_preview:=true``（沿用 Intel 示例的链路：pipeline → numpy → 窗口）。 """ from __future__ import annotations @@ -34,7 +34,7 @@ from std_msgs.msg import Float32MultiArray def _param_as_str(node: Node, name: str, default: str = '') -> str: - """String param that also accepts numeric CLI overrides (e.g. serial numbers).""" + """字符串参数，同时兼容命令行传数字的情况（例如序列号）。""" node.declare_parameter( name, default, ParameterDescriptor(dynamic_typing=True) ) @@ -82,7 +82,7 @@ def _site_yaml_path() -> Path: class RealsenseColorPub(Node): def __init__(self) -> None: super().__init__('realsense_color_pub') - self.declare_parameter('camera_name', 'd405') # topic namespace + self.declare_parameter('camera_name', 'd405') # 话题命名空间 self._name = str(self.get_parameter('camera_name').value).strip().strip('/') self._serial = _param_as_str(self, 'camera_serial', '') self.declare_parameter('width', 1280) @@ -94,8 +94,8 @@ class RealsenseColorPub(Node): self.declare_parameter('enable_depth', True) self.declare_parameter('enable_infra', True) self.declare_parameter('preview_stereo', True) - # Render the Qt preview only every Nth frame — publishing stays at full - # rate; the CPU colorizer + 4-pane paint must not slow the stream. + # Qt 预览每 N 帧才渲染一次 —— 发布始终保持满帧率； + # CPU 上色 + 四窗格绘制绝不能拖慢数据流。 self.declare_parameter('preview_decimation', 2) self.declare_parameter('auto_exposure', True) self.declare_parameter('ae_limit_us', 3000.0) @@ -122,7 +122,7 @@ class RealsenseColorPub(Node): self._laser_power = float(self.get_parameter('laser_power').value) self._exposure_us = float(self.get_parameter('exposure_us').value) self._gain = float(self.get_parameter('gain').value) - # FPS meter: hardware frame numbers vs actually published (loop rate) + # FPS 表：硬件帧号对比实际发布帧数（循环速率） self._fps_t0 = 0.0 self._fps_pub_color = 0 self._fps_pub_depth = 0 @@ -132,7 +132,7 @@ class RealsenseColorPub(Node): self._t_align = 0.0 self._t_pub = 0.0 - qos = qos_profile_sensor_data # BEST_EFFORT, keep last 1 + qos = qos_profile_sensor_data # BEST_EFFORT，只保留最后 1 帧 base = f'/{self._name}/color' self._img_pub = self.create_publisher( Image, f'{base}/image_raw', qos @@ -143,8 +143,8 @@ class RealsenseColorPub(Node): self._depth_pub = self.create_publisher( Image, f'/{self._name}/aligned_depth_to_color/image_raw', qos ) - # IR streams are published too so the preview can live in a separate - # viewer process (Qt must not share the GIL with the capture loop). + # IR 流也会一起发布，这样预览可以放在独立的查看器进程里 + # （Qt 不能和采集循环共享 GIL）。 self._ir1_pub = None self._ir2_pub = None if self._enable_infra_param and self._preview_stereo: @@ -163,8 +163,8 @@ class RealsenseColorPub(Node): self._infra_mode = 'none' self._colorizer = None self._logged_depth = False - # Device depth scale → multiply raw uint16 by this to get true mm. - # D405 = 0.1 mm/unit, D435 = 1.0 mm/unit. Read from the sensor at startup. + # 设备深度尺度 → 用原始 uint16 乘上它才是真实的 mm。 + # D405 = 0.1 mm/单位，D435 = 1.0 mm/单位。启动时从传感器读取。 self._depth_to_mm = 1.0 self._running = True self._thread: Optional[threading.Thread] = None @@ -205,13 +205,13 @@ class RealsenseColorPub(Node): det_qos, ) self.get_logger().info( - 'SDK preview ON — COLOR|DEPTH|IR-L|IR-R in this process ' - f'(hold {self._overlay_hold_sec:.1f}s). Close window ≠ stop camera.' + 'SDK 预览已开启 —— 本进程内显示 COLOR|DEPTH|IR-L|IR-R ' + f'（保留 {self._overlay_hold_sec:.1f}s）。关窗口 ≠ 停相机。' ) self._start_pipeline() def _pick_device(self, devices, rs): - """Prefer configured serial, then D405 by name (this PC also has a D435).""" + """优先用配置的序列号，其次按名字选 D405（这台机器上还接了 D435）。""" hint = (self._name or 'd405').lower() if self._serial: for d in devices: @@ -222,7 +222,7 @@ class RealsenseColorPub(Node): for d in devices ) raise RuntimeError( - f'Serial {self._serial!r} not found. Connected: {avail}' + f'找不到序列号 {self._serial!r}。当前已连接：{avail}' ) for d in devices: name = str(d.get_info(rs.camera_info.name) or '').lower() @@ -231,7 +231,7 @@ class RealsenseColorPub(Node): return devices[0] def _reload_site_preview(self, stream_w: int = 0, stream_h: int = 0) -> None: - """Pick up overlay_hold_sec (and warn if width/height changed) without restart.""" + """不重启就能读到新的 overlay_hold_sec（宽高变了则给出警告）。""" path = self._site_yaml if not path.is_file(): return @@ -248,7 +248,7 @@ class RealsenseColorPub(Node): data = yaml.safe_load(path.read_text(encoding="utf-8")) or {} cam = data.get("camera") or {} except Exception as exc: # noqa: BLE001 - self.get_logger().warn(f"site.yaml reload failed: {exc}") + self.get_logger().warn(f"site.yaml 重新加载失败：{exc}") return if "overlay_hold_sec" in cam: hold = max(0.0, float(cam.get("overlay_hold_sec") or 0.0)) @@ -264,14 +264,14 @@ class RealsenseColorPub(Node): if (want_w, want_h) != (live_w, live_h) and not self._site_wh_warned: self._site_wh_warned = True self.get_logger().warn( - f"preview stream is {live_w}x{live_h}, site.yaml camera is " - f"{want_w}x{want_h} — restart: bash ~/ros2/scripts/run_camera_d405.sh --preview" + f"预览流是 {live_w}x{live_h}，而 site.yaml 里的 camera 是 " + f"{want_w}x{want_h} —— 请重启：bash ~/ros2/scripts/run_camera_d405.sh --preview" ) elif (want_w, want_h) == (live_w, live_h): self._site_wh_warned = False def _on_det(self, msg: Float32MultiArray) -> None: - # Empty payload = perception miss → clear green box immediately + # 空负载 = 感知漏检 → 立刻清掉绿框 if not msg.data or len(msg.data) < 8: self._overlay = None self._overlay_stamp = 0.0 @@ -295,8 +295,8 @@ class RealsenseColorPub(Node): img.encoding = 'bgr8' img.is_bigendian = 0 img.step = w * 3 - # array.array('B', ...) hits the setter's fast path — plain bytes gets - # an element-wise debug check that costs ~40 ms per frame here. + # array.array('B', ...) 会走 setter 的快路径 —— 直接传 bytes 会触发 + # 逐元素的 debug 检查，在这里每帧要多花约 40 ms。 img.data = array.array('B', np.ascontiguousarray(bgr).tobytes()) self._img_pub.publish(img) @@ -324,7 +324,7 @@ class RealsenseColorPub(Node): if not self._logged_depth: self._logged_depth = True self.get_logger().info( - f'first aligned depth {dw}x{dh} → ' + f'首个对齐深度 {dw}x{dh} → ' f'/{self._name}/aligned_depth_to_color/image_raw' ) @@ -333,10 +333,10 @@ class RealsenseColorPub(Node): import pyrealsense2 as rs except Exception as exc: # noqa: BLE001 raise RuntimeError( - 'pyrealsense2 not available. conda activate ros_flexiv' + 'pyrealsense2 不可用。请执行 conda activate ros_flexiv' ) from exc - # After killing a previous publisher, USB may take 1–3s to re-enumerate. + # 杀掉上一个发布进程后，USB 需要 1–3s 才能重新枚举。 ctx = rs.context() devices = [] for attempt in range(8): @@ -347,9 +347,9 @@ class RealsenseColorPub(Node): ctx = rs.context() if not devices: raise RuntimeError( - 'No RealSense connected (USB empty after retries). ' - 'Plug D405 into THIS machine on a USB3 (blue) port, unplug/replug, ' - 'and close realsense-viewer / the 192.168.0.108 capture site if it holds the camera.' + '没有连接 RealSense（多次重试后 USB 仍为空）。' + '请把 D405 插到**本机**的 USB3（蓝色）口，拔插一次，' + '并关闭 realsense-viewer / 192.168.0.108 上占用该相机的采集端。' ) found = self._pick_device(devices, rs) @@ -375,8 +375,8 @@ class RealsenseColorPub(Node): ) if usb2: self.get_logger().warn( - f'{cam_name} is on USB {usb_desc} — keeping depth/IR at ' - '640x480@15. Plug D405 into a USB3 (blue) port for 848@30.' + f'{cam_name} 接在 USB {usb_desc} 上 —— 深度/IR 将保持 ' + '640x480@15。把 D405 插到 USB3（蓝色）口可获得 848@30。' ) self._width, self._height, self._fps = 640, 480, 15 @@ -399,7 +399,7 @@ class RealsenseColorPub(Node): ] if usb2: candidates = [(640, 480, 15), (640, 480, 5), (424, 240, 15)] - # Drop firmware junk (848@90 etc.) that ioctl-fails and wedges USB. + # 丢弃固件里的垃圾模式（848@90 之类），它们会 ioctl 失败并卡死 USB。 seen = set() trimmed = [] for item in candidates: @@ -425,8 +425,8 @@ class RealsenseColorPub(Node): have_depth = False have_infra = False - # USB3: try real IR-L+IR-R first (same stereo AE). Color+IR-L often - # fails on D405 — then IR-R + color-as-left (different look, not AE). + # USB3：先尝试真正的 IR-L+IR-R（共享立体自动曝光）。彩色+IR-L 在 D405 上 + # 经常失败 —— 那就退回 IR-R + 把彩色当左目（观感不同，且不共享自动曝光）。 def _modes(): if want_depth and want_infra and not usb2: yield True, 'both' @@ -461,25 +461,25 @@ class RealsenseColorPub(Node): self._infra_mode = infra_mode if (w, h, fps) != want: self.get_logger().warn( - f'Requested {want[0]}x{want[1]}@{want[2]}Hz ' - f'→ using {w}x{h}@{fps}Hz' + f'请求 {want[0]}x{want[1]}@{want[2]}Hz ' + f'→ 改用 {w}x{h}@{fps}Hz' ) if want_depth and not with_depth: self.get_logger().warn( - f'{w}x{h}@{fps}Hz color+depth failed — color only' + f'{w}x{h}@{fps}Hz 彩色+深度失败 —— 只用彩色' ) elif want_infra and infra_mode == 'none': self.get_logger().warn( - f'{w}x{h}@{fps}Hz stereo IR failed — color+depth preview' + f'{w}x{h}@{fps}Hz 立体 IR 失败 —— 改用彩色+深度预览' ) elif infra_mode == 'both': self.get_logger().info( - f'{w}x{h}@{fps}Hz stereo IR-L+IR-R (shared AE) + depth' + f'{w}x{h}@{fps}Hz 立体 IR-L+IR-R（共享自动曝光）+ 深度' ) elif infra_mode == 'right': self.get_logger().info( - f'{w}x{h}@{fps}Hz IR-L=color gray, IR-R=raw IR ' - '(looks brighter/darker — not two AE loops)' + f'{w}x{h}@{fps}Hz IR-L=彩色灰度，IR-R=原始 IR ' + '（明暗不同 —— 这不是两路自动曝光）' ) self._width, self._height, self._fps = w, h, fps self._last_ok = (w, h, fps) @@ -487,7 +487,7 @@ class RealsenseColorPub(Node): except Exception as exc: # noqa: BLE001 last_err = exc kind = f'color+depth+IR-{infra_mode}' if with_depth else 'color' - self.get_logger().warn(f'{w}x{h}@{fps}Hz {kind} failed: {exc}') + self.get_logger().warn(f'{w}x{h}@{fps}Hz {kind} 失败：{exc}') profile = None have_depth = False have_infra = False @@ -501,7 +501,7 @@ class RealsenseColorPub(Node): break if profile is None: raise RuntimeError( - f'No supported color profile for D405. Last error: {last_err}' + f'D405 没有可用的彩色配置。最后的错误：{last_err}' ) self._pipe = pipe @@ -509,18 +509,18 @@ class RealsenseColorPub(Node): self._have_depth = have_depth self._have_infra = have_infra self._logged_depth = False - # Publish depth in TRUE millimetres. D405 raw units are 0.1 mm (D435 1 mm); - # downstream (perception/servo) assumes mm, so normalise by the device scale. + # 深度按**真实毫米**发布。D405 的原始单位是 0.1 mm（D435 是 1 mm）； + # 下游（感知/伺服）都按 mm 处理，所以要按设备尺度归一化。 self._depth_to_mm = 1.0 if have_depth: try: ds = float(profile.get_device().first_depth_sensor().get_depth_scale()) - self._depth_to_mm = ds * 1000.0 # m/unit → mm/unit + self._depth_to_mm = ds * 1000.0 # m/单位 → mm/单位 self.get_logger().info( - f'depth scale {ds * 1000.0:.4f} mm/unit → publishing true mm' + f'深度尺度 {ds * 1000.0:.4f} mm/单位 → 按真实 mm 发布' ) except Exception as exc: # noqa: BLE001 - self.get_logger().warn(f'depth scale read failed, assume mm: {exc}') + self.get_logger().warn(f'读取深度尺度失败，按 mm 处理：{exc}') self._depth_to_mm = 1.0 self._align = rs.align(rs.stream.color) if have_depth else None self._colorizer = None @@ -546,11 +546,11 @@ class RealsenseColorPub(Node): log=self.get_logger().info, ) apply_realsense_exposure(profile.get_device(), **kwargs) - # Firmware often resets AE on the first frames; stamp it again. + # 固件经常会在最初几帧重置自动曝光；所以再设置一次。 _time.sleep(0.25) apply_realsense_exposure(profile.get_device(), **kwargs) except Exception as exc: # noqa: BLE001 - self.get_logger().warn(f'exposure apply: {exc}') + self.get_logger().warn(f'应用曝光参数失败：{exc}') if self._preview is not None: if self._infra_mode == 'both': self._preview.set_pane_titles( @@ -575,8 +575,8 @@ class RealsenseColorPub(Node): dist = list(intr.coeffs[:5]) if intr.coeffs else [0.0] * 5 except Exception as exc: # noqa: BLE001 self.get_logger().warn( - f'get_intrinsics failed ({exc}); using D405 defaults for ' - f'{self._width}x{self._height}' + f'get_intrinsics 失败（{exc}）；改用 D405 的默认内参，' + f'分辨率 {self._width}x{self._height}' ) w, h = float(self._width), float(self._height) if w >= 1200: @@ -595,10 +595,10 @@ class RealsenseColorPub(Node): depth_txt = ( f' + aligned depth /{self._name}/aligned_depth_to_color/image_raw' if have_depth - else ' (color only)' + else ' （仅彩色）' ) if have_infra: - depth_txt += ' + IR-L/IR-R preview' + depth_txt += ' + IR-L/IR-R 预览' self.get_logger().info( f'{model} serial={sn} USB={usb_desc} → /{self._name}/color/image_raw ' f'{self._width}x{self._height}@{self._fps}Hz ' @@ -611,30 +611,30 @@ class RealsenseColorPub(Node): self._thread.start() def _enable_color_ae_fast(self, profile, rs) -> None: - """Color auto-exposure that prefers short exposure (less motion blur).""" + """彩色自动曝光，优先用短曝光（减少运动模糊）。""" try: sensor = profile.get_device().first_color_sensor() except Exception as exc: # noqa: BLE001 - self.get_logger().warn(f'color AE: no color sensor ({exc})') + self.get_logger().warn(f'彩色 AE：没有彩色传感器（{exc}）') return try: if sensor.supports(rs.option.enable_auto_exposure): sensor.set_option(rs.option.enable_auto_exposure, 1.0) if sensor.supports(rs.option.auto_exposure_priority): - # 0 = keep FPS, shorten exposure when the scene is bright enough + # 0 = 保住帧率，场景够亮时缩短曝光时间 sensor.set_option(rs.option.auto_exposure_priority, 0.0) limit = getattr(rs.option, "auto_exposure_limit", None) if limit is not None and sensor.supports(limit): try: - sensor.set_option(limit, 5000.0) # µs; less motion blur at 30 Hz + sensor.set_option(limit, 5000.0) # µs；30 Hz 下减少运动模糊 except Exception: pass - self.get_logger().info('color auto-exposure ON (prefer FPS / less blur)') + self.get_logger().info('彩色自动曝光已开启（优先帧率 / 减少模糊）') except Exception as exc: # noqa: BLE001 - self.get_logger().warn(f'color AE: {exc}') + self.get_logger().warn(f'彩色 AE：{exc}') def _enable_stereo_ae(self, profile, rs) -> None: - """One auto-exposure for both IR imagers (stereo module).""" + """两路 IR 成像器共用一路自动曝光（立体模块）。""" try: sensor = profile.get_device().first_depth_sensor() if sensor.supports(rs.option.enable_auto_exposure): @@ -642,15 +642,15 @@ class RealsenseColorPub(Node): limit = getattr(rs.option, "auto_exposure_limit", None) if limit is not None and sensor.supports(limit): try: - # Cap IR exposure well under the 33 ms frame period so a - # dim scene cannot silently drop the stream below 30 Hz. + # 把 IR 曝光上限压到远低于 33 ms 的帧周期，这样光线暗的场景 + # 也不会悄悄把数据流掉到 30 Hz 以下。 sensor.set_option(limit, 20000.0) - self.get_logger().info('stereo IR exposure limit 20ms') + self.get_logger().info('立体 IR 曝光上限 20ms') except Exception: pass - self.get_logger().info('stereo IR auto-exposure ON (shared L/R)') + self.get_logger().info('立体 IR 自动曝光已开启（左右共享）') except Exception as exc: # noqa: BLE001 - self.get_logger().warn(f'stereo AE: {exc}') + self.get_logger().warn(f'立体 AE：{exc}') def _colorize_depth(self, depth_frame, depth_u16: np.ndarray) -> np.ndarray: if self._colorizer is not None: @@ -695,7 +695,7 @@ class RealsenseColorPub(Node): return self._pipe is not None except Exception as exc: # noqa: BLE001 self.get_logger().error( - f'reconnect failed: {exc}. Unplug/replug USB3, slack the cable.', + f'重连失败：{exc}。请拔插 USB3，并给线缆留点余量。', throttle_duration_sec=5.0, ) return False @@ -738,15 +738,15 @@ class RealsenseColorPub(Node): if dropped or misses >= 4: usb = getattr(self, '_usb_desc', '?') self.get_logger().error( - f'Camera stream lost (USB={usb}): {text[:120]}. ' - 'Reconnecting — slack the USB cable so the arm does not yank it.' + f'相机数据流中断（USB={usb}）：{text[:120]}。' + '正在重连 —— 请给 USB 线缆留点余量，别让机械臂把它拽掉。' ) got = False misses = 0 self._reconnect() continue if misses == 1: - self.get_logger().warn(f'wait_for_frames: {exc}') + self.get_logger().warn(f'wait_for_frames：{exc}') continue if not color: misses += 1 @@ -756,14 +756,14 @@ class RealsenseColorPub(Node): depth = frames.get_depth_frame() depth_u16 = np.asanyarray(depth.get_data()) if depth else None if depth_u16 is not None and self._depth_to_mm != 1.0: - # raw device units → true mm (D405 raw is 0.1 mm/unit) + # 原始设备单位 → 真实 mm（D405 原始为 0.1 mm/单位） depth_u16 = np.clip( depth_u16.astype(np.float32) * self._depth_to_mm, 0, 65535 ).astype(np.uint16) self._frame_i += 1 self._fps_meter(color, depth, depth_u16) - # Preview work (CPU colorizer, IR copies, 4-pane paint) only every - # Nth frame — it must not slow down publishing. + # 预览相关的工作（CPU 上色、IR 拷贝、四窗格绘制）每 N 帧才做一次 + # —— 它绝不能拖慢发布。 do_push = ( self._preview is not None and (self._frame_i % self._preview_every) == 0 @@ -776,13 +776,13 @@ class RealsenseColorPub(Node): depth_bgr = self._colorize_depth(depth, depth_u16) ir_l_img = np.asanyarray(ir_l.get_data()).copy() if ir_l else None ir_r_img = np.asanyarray(ir_r.get_data()).copy() if ir_r else None - # Only substitute color-gray when the raw left IR stream is unavailable. + # 只有在拿不到原始左目 IR 流时，才用彩色灰度来顶替。 if ir_l_img is None and self._infra_mode != 'both': ir_l_img = np.mean(bgr, axis=2).astype(np.uint8) if not got: got = True sw, sh = int(bgr.shape[1]), int(bgr.shape[0]) - self.get_logger().info(f'first frame {sw}x{sh}') + self.get_logger().info(f'首帧 {sw}x{sh}') if self._preview is not None: self._preview.set_pane_titles( f"COLOR {sw}x{sh}", @@ -809,7 +809,7 @@ class RealsenseColorPub(Node): self._overlay is None or age > hold ): self._preview.set_status( - 'NO box — check perception_node / conf' + '没有框 —— 检查 perception_node / conf' ) else: self._preview.set_status('') @@ -817,15 +817,15 @@ class RealsenseColorPub(Node): bgr.copy(), depth_bgr, ir_l_img, ir_r_img ) except Exception as exc: # noqa: BLE001 - # Context may be shutting down + # 上下文可能正在关闭 if not self._running: break self.get_logger().warn( - f'publish failed: {exc}', throttle_duration_sec=2.0 + f'发布失败：{exc}', throttle_duration_sec=2.0 ) def _publish_ir(self, frame, pub) -> None: - """Raw IR imager as mono8 for the external viewer process.""" + """把原始 IR 成像器以 mono8 发布，供外部查看器进程使用。""" arr = np.asanyarray(frame.get_data()) h, w = int(arr.shape[0]), int(arr.shape[1]) msg = Image() @@ -840,8 +840,8 @@ class RealsenseColorPub(Node): pub.publish(msg) def _fps_meter(self, color, depth, depth_u16) -> None: - """Log published vs hardware frame rate — separates loop slowness - (preview/CPU) from SDK-side drops (AE long exposure / USB).""" + """记录发布帧率与硬件帧率 —— 用来区分循环太慢 + （预览/CPU）和 SDK 侧掉帧（自动曝光长曝光 / USB）。""" now = time.monotonic() try: fn_c = int(color.get_frame_number()) @@ -886,13 +886,13 @@ def main(args=None) -> None: preview = getattr(node, '_preview', None) if preview is not None: app = node._qt_app - # Closing the window must NOT stop the camera — track needs the stream. + # 关闭窗口绝不能停掉相机 —— 跟踪需要这条数据流。 while rclpy.ok() and node._running: rclpy.spin_once(node, timeout_sec=0.01) app.processEvents() if node._preview is not None and not node._preview.isVisible(): node.get_logger().warn( - 'Preview closed — camera keeps publishing' + '预览已关闭 —— 相机继续发布' ) node._preview = None else: diff --git a/src/arm_application/arm_application/realsense_sdk_view.py b/src/arm_application/arm_application/realsense_sdk_view.py index b2e6e79..1bb3b14 100644 --- a/src/arm_application/arm_application/realsense_sdk_view.py +++ b/src/arm_application/arm_application/realsense_sdk_view.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 -"""RealSense SDK preview: fixed 2x2 Qt panes (color / depth / IR-L / IR-R). +"""RealSense SDK 预览：固定的 2x2 Qt 窗格（彩色 / 深度 / IR-L / IR-R）。 -Stitching four streams into one pixmap looked like a single camera view when -depth/IR were missing. Each stream now has its own labeled pane. +把四路画面拼成一张 pixmap 时，一旦缺了深度 / IR 看起来就像单相机视图。 +现在每路画面都有自己带标题的独立窗格。 """ from __future__ import annotations @@ -221,15 +221,15 @@ class SdkPreviewWindow(QMainWindow): return color = self._draw_overlay(color, ov, status) labs = self._img_labels - self._set_pane(labs[0], color, "NO COLOR") - self._set_pane(labs[1], depth, "NO DEPTH") - self._set_pane(labs[2], ir_l, "NO IR-L") - self._set_pane(labs[3], ir_r, "NO IR-R") + self._set_pane(labs[0], color, "无彩色") + self._set_pane(labs[1], depth, "无深度") + self._set_pane(labs[2], ir_l, "无 IR-L") + self._set_pane(labs[3], ir_r, "无 IR-R") def ensure_app() -> "QApplication": if QApplication is None: - raise RuntimeError("PySide6 not available for RealSense preview window") + raise RuntimeError("RealSense 预览窗口缺少 PySide6，无法创建") app = QApplication.instance() if app is None: app = QApplication([]) diff --git a/src/arm_application/arm_application/realsense_view.py b/src/arm_application/arm_application/realsense_view.py index 954d621..3c1d866 100644 --- a/src/arm_application/arm_application/realsense_view.py +++ b/src/arm_application/arm_application/realsense_view.py @@ -1,11 +1,10 @@ #!/usr/bin/env python3 -"""realsense_view — D405 4-pane preview (COLOR|DEPTH|IR-L|IR-R), own process. +"""realsense_view —— D405 四窗格预览（COLOR|DEPTH|IR-L|IR-R），独立进程运行。 -The camera node (``realsense_color_pub``) only publishes topics. All Qt work -runs here so painting can never starve the camera capture loop (Python GIL -made ``publish`` wall-time jump from 1 ms to ~70 ms when they shared a -process). Closing the window only hides the preview; Ctrl+C stops the viewer -(the camera keeps publishing). +相机节点（``realsense_color_pub``）只负责发布话题。所有 Qt 工作都放在这里， +这样绘制永远不会拖垮相机采集循环（两者同进程时，Python 的 GIL 会让 ``publish`` +的墙钟耗时从 1 ms 跳到约 70 ms）。关闭窗口只是隐藏预览；Ctrl+C 才停止查看器 +（相机仍在持续发布）。 """ from __future__ import annotations @@ -31,7 +30,7 @@ from arm_application.realsense_color_pub import _site_yaml_path def _colorize_depth_u16(d: np.ndarray) -> np.ndarray: - """Fast numpy JET colorize for the preview pane (no SDK colorizer).""" + """为预览窗格做快速 numpy JET 上色（不使用 SDK 的 colorizer）。""" vis = np.zeros((d.shape[0], d.shape[1], 3), dtype=np.uint8) valid = d > 0 if not valid.any(): @@ -79,7 +78,7 @@ class RealsenseView(Node): self._stage2_stamp = 0.0 self._site_yaml = _site_yaml_path() self._site_mtime = 0.0 - # RX rate meter — visible stutter diagnosis (transport vs paint) + # RX 速率表 —— 用来直观诊断卡顿（是传输问题还是绘制问题） self._rx_color = 0 self._rx_depth = 0 self._rx_ir = 0 @@ -115,7 +114,7 @@ class RealsenseView(Node): self._qt_app = ensure_app() hud = self._hud or self._name self._preview = SdkPreviewWindow( - title=f'{hud} /{self._name} COLOR|DEPTH|IR (viewer)' + title=f'{hud} /{self._name} COLOR|DEPTH|IR （查看器）' ) self._preview.set_pane_titles( 'COLOR', 'DEPTH', 'IR-L (左红外)', 'IR-R (右红外)' @@ -124,11 +123,11 @@ class RealsenseView(Node): hz = max(5.0, min(30.0, float(self.get_parameter('view_hz').value))) self.create_timer(1.0 / hz, self._push) self.get_logger().info( - f'viewer ON — subscribing /{self._name}/color|depth|infra1|infra2 ' - f'(hold {self._hold:.2f}s). Close window = hide only.' + f'查看器已启动 —— 订阅 /{self._name}/color|depth|infra1|infra2 ' + f'（保留 {self._hold:.2f}s）。关闭窗口 = 仅隐藏。' ) - # Callbacks only store the newest message; conversion happens in _push. + # 回调只存最新消息；转换在 _push 里做。 def _on_color(self, msg: Image) -> None: self._color = msg self._rx_color += 1 @@ -184,7 +183,7 @@ class RealsenseView(Node): data = yaml.safe_load(path.read_text(encoding="utf-8")) or {} cam = data.get("camera") or {} except Exception as exc: # noqa: BLE001 - self.get_logger().warn(f"site.yaml reload failed: {exc}") + self.get_logger().warn(f"site.yaml 重新加载失败：{exc}") return if "overlay_hold_sec" in cam: hold = max(0.0, float(cam.get("overlay_hold_sec") or 0.0)) @@ -216,7 +215,7 @@ class RealsenseView(Node): elif now - self._rx_t0 >= 5.0: dt = now - self._rx_t0 self.get_logger().info( - f'viewer rx color={self._rx_color / dt:.1f}/s ' + f'查看器接收 color={self._rx_color / dt:.1f}/s ' f'depth={self._rx_depth / dt:.1f}/s ir={self._rx_ir / dt:.1f}/s' ) self._rx_t0 = now @@ -228,7 +227,7 @@ class RealsenseView(Node): self._overlay = None self._preview.set_overlay(None) if self._overlay is None or age > self._hold: - miss = 'NO box — check perception_node / conf' + miss = '没有框 —— 检查 perception_node / conf' self._preview.set_status(f'{self._hud} {miss}' if self._hud else miss) else: self._preview.set_status(self._hud) @@ -244,7 +243,7 @@ class RealsenseView(Node): int(self._depth.height), int(self._depth.width) ) depth_bgr = _colorize_depth_u16(d) - # Mono→BGR here (executor thread) so the Qt paint thread stays cheap. + # 在这里（执行器线程）做 Mono→BGR，让 Qt 绘制线程保持轻量。 ir1 = None if self._ir1 is None else _mono_to_bgr(self._mono(self._ir1)) ir2 = None if self._ir2 is None else _mono_to_bgr(self._mono(self._ir2)) self._preview.push_views(color, depth_bgr, ir1, ir2) @@ -254,8 +253,8 @@ def main(args=None) -> None: rclpy.init(args=args) node = RealsenseView() app = node._qt_app - # ROS callbacks (image deserialization, depth colorize, push) run on - # executor threads; the main thread only paints Qt — no shared GIL bursts. + # ROS 回调（图像反序列化、深度上色、推送）跑在执行器线程上； + # 主线程只负责绘制 Qt —— 不会有共享的 GIL 突发。 executor = MultiThreadedExecutor(num_threads=3) executor.add_node(node) spin_t = threading.Thread(target=executor.spin, daemon=True) @@ -265,7 +264,7 @@ def main(args=None) -> None: app.processEvents() time.sleep(0.005) if node._preview is not None and not node._preview.isVisible(): - node.get_logger().warn('Preview closed — viewer idles (camera keeps publishing)') + node.get_logger().warn('预览已关闭 —— 查看器挂起（相机仍在发布）') node._preview = None except KeyboardInterrupt: pass diff --git a/src/arm_application/arm_application/run_joint_trajectory.py b/src/arm_application/arm_application/run_joint_trajectory.py index 195fc7c..f0acd93 100644 --- a/src/arm_application/arm_application/run_joint_trajectory.py +++ b/src/arm_application/arm_application/run_joint_trajectory.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Send a multi-point joint trajectory via /follow_joint_trajectory (real + mock).""" +"""通过 ``/follow_joint_trajectory`` 发送多点关节轨迹（真机 + mock 都适用）。""" from __future__ import annotations @@ -16,7 +16,7 @@ DEFAULT_JOINTS = [ 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7', ] NUM_JOINTS = 7 -# flat positions (3 pts x 7 joints) + times — ROS params do not support dict lists +# 扁平化存放的位置（3 个点 x 7 个关节）+ 时间 —— ROS 参数不支持列表套字典 DEFAULT_TIMES = [3.0, 6.0, 9.0] DEFAULT_POSITIONS = [ 0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0, @@ -34,7 +34,7 @@ class RunJointTrajectory(Node): self.declare_parameter('action_name', 'follow_joint_trajectory') self.declare_parameter('waypoint_times', DEFAULT_TIMES) self.declare_parameter('waypoint_positions', DEFAULT_POSITIONS) - # 1 = run once (default); 0 = loop until Ctrl+C; N = repeat N times + # 1 = 只跑一次（默认）；0 = 循环直到 Ctrl+C；N = 重复 N 次 self.declare_parameter('loop_count', 1) self.declare_parameter('pause_between_loops', 0.5) @@ -52,8 +52,8 @@ class RunJointTrajectory(Node): def run(self) -> int: if not self._client.wait_for_server(timeout_sec=15.0): self.get_logger().error( - f'Action [{self._action_name}] not available. ' - 'Start real stack first: bash scripts/run_real.sh' + f'Action [{self._action_name}] 不可用。' + '请先启动真机栈：bash scripts/run_real.sh' ) return 1 @@ -62,12 +62,12 @@ class RunJointTrajectory(Node): n_j = len(self.joint_names) n_pts = len(times) if n_pts < 1: - self.get_logger().error('waypoint_times is empty') + self.get_logger().error('waypoint_times 为空') return 1 if len(positions) != n_pts * n_j: self.get_logger().error( - f'waypoint_positions length {len(positions)} != ' - f'{n_pts} points x {n_j} joints = {n_pts * n_j}' + f'waypoint_positions 长度 {len(positions)} != ' + f'{n_pts} 个点 x {n_j} 个关节 = {n_pts * n_j}' ) return 1 @@ -83,35 +83,35 @@ class RunJointTrajectory(Node): duration = goal.time_from_start[-1] if loop_count == 0: self.get_logger().info( - f'Loop mode: {goal.num_points} points, ~{duration:.1f}s/cycle ' - f'(Ctrl+C to stop)' + f'循环模式：{goal.num_points} 个点，约 {duration:.1f}s/圈 ' + f'（Ctrl+C 停止）' ) elif loop_count > 1: self.get_logger().info( - f'Loop mode: {loop_count} cycles, {goal.num_points} points, ' - f'~{duration:.1f}s/cycle' + f'循环模式：{loop_count} 圈，{goal.num_points} 个点，' + f'约 {duration:.1f}s/圈' ) else: self.get_logger().info( - f'Sending {goal.num_points} points, v_scale={goal.velocity_scale}, ' - f'duration={duration:.1f}s' + f'发送 {goal.num_points} 个点，v_scale={goal.velocity_scale}，' + f'时长={duration:.1f}s' ) iteration = 0 while rclpy.ok(): iteration += 1 if loop_count > 1: - self.get_logger().info(f'Cycle {iteration}/{loop_count}') + self.get_logger().info(f'第 {iteration}/{loop_count} 圈') code = self._send_once(goal, duration) if code != 0: return code if loop_count == 1: - self.get_logger().info('Done: OK') + self.get_logger().info('完成：OK') return 0 if loop_count > 1 and iteration >= loop_count: - self.get_logger().info(f'Done: completed {loop_count} cycles') + self.get_logger().info(f'完成：已跑完 {loop_count} 圈') return 0 if pause > 0: time.sleep(pause) @@ -122,11 +122,11 @@ class RunJointTrajectory(Node): while rclpy.ok() and not send.done(): rclpy.spin_once(self, timeout_sec=0.05) if time.monotonic() > deadline: - self.get_logger().error('Timed out sending goal') + self.get_logger().error('发送目标超时') return 2 handle = send.result() if handle is None or not handle.accepted: - self.get_logger().error('Goal rejected') + self.get_logger().error('目标被拒绝') return 3 res_fut = handle.get_result_async() @@ -138,18 +138,18 @@ class RunJointTrajectory(Node): if now >= next_log: left = max(0.0, deadline - now) self.get_logger().info( - f'Executing trajectory (~{duration:.0f}s planned), ' - f'waiting for robot... ({left:.0f}s timeout left)' + f'正在执行轨迹（计划约 {duration:.0f}s），' + f'等待机器人……（剩余超时 {left:.0f}s）' ) next_log = now + 10.0 if now > deadline: - self.get_logger().error('Timed out waiting for result') + self.get_logger().error('等待结果超时') return 4 result = res_fut.result().result if not result.success: - self.get_logger().error(f'Failed: {result.message}') + self.get_logger().error(f'失败：{result.message}') return 5 - self.get_logger().info(f'Cycle finished: {result.message or "OK"}') + self.get_logger().info(f'本圈结束：{result.message or "OK"}') return 0 diff --git a/src/arm_application/arm_application/servo_node.py b/src/arm_application/arm_application/servo_node.py index fcc21ba..3e8dafe 100644 --- a/src/arm_application/arm_application/servo_node.py +++ b/src/arm_application/arm_application/servo_node.py @@ -1,13 +1,13 @@ #!/usr/bin/env python3 -"""servo_node — 100 Hz Twist PBVS (own process, no YOLO). +"""servo_node —— 100 Hz 的 Twist 视觉伺服（PBVS，独立进程，不跑 YOLO）。 -Hard rules: -- MultiThreadedExecutor + Reentrant ControlCallbackGroup for 100 Hz timer -- Subscribe PointStamped only (no Service) -- cmd_vel: SystemDefaults + keep_last(1) so driver always sees latest -- Watchdog: lost target → freeze extrapolate, zero vel, clear I, zero traj -- Latency compensation only while target is fresh -- Never sendTransform in the 100 Hz loop +硬性规则： +- 用 ``MultiThreadedExecutor`` + ``Reentrant`` 的 ``ControlCallbackGroup`` 驱动 100 Hz 定时器 +- 只订阅 ``PointStamped``（不用 Service） +- ``cmd_vel`` 用 ``SystemDefaults`` + ``keep_last(1)``，保证驱动器总能看到最新指令 +- 看门狗：目标丢失 → 冻结外推、速度置零、清 I、轨迹置零 +- 只有在目标数据新鲜时才做延迟补偿 +- 绝不在 100 Hz 循环里调用 ``sendTransform`` """ from __future__ import annotations @@ -59,7 +59,7 @@ class ServoNode(Node): super().__init__("servo_node") tid = threading.get_ident() self.get_logger().warn( - f"servo_node started pid={os.getpid()} thread_id={tid}" + f"servo_node 已启动 pid={os.getpid()} thread_id={tid}" ) self.declare_parameter("joint_prefix", "Rizon4-063989_") @@ -87,9 +87,9 @@ class ServoNode(Node): self.declare_parameter("predict_v_max", 0.35) self.declare_parameter("predict_jump_m", 0.08) - # --- Watchdog (lost target hard stop) --- + # --- 看门狗（丢失目标即硬停） --- self.declare_parameter("watchdog_timeout_sec", 1.2) - self.declare_parameter("stale_hold_sec", 0.28) # freeze XY before watchdog trip + self.declare_parameter("stale_hold_sec", 0.28) # 触发看门狗前先冻结 XY # --- PID --- self.declare_parameter("kp_xy", 2.00) @@ -100,7 +100,7 @@ class ServoNode(Node): self.declare_parameter("kd_z", 0.45) self.declare_parameter("i_limit_xy", 0.03) self.declare_parameter("i_limit_z", 0.025) - # Integral separation thresholds + # 积分分离阈值 self.declare_parameter("i_sep_pos_m", 0.005) self.declare_parameter("i_sep_ang_rad", 0.02) @@ -109,27 +109,27 @@ class ServoNode(Node): self.declare_parameter("v_max_xy", 0.06) self.declare_parameter("v_max_z", 0.04) self.declare_parameter("a_max", 0.45) - self.declare_parameter("lost_timeout_sec", 1.2) # alias / stamp-age backup - self.declare_parameter("e_xy_clip_m", 0.080) # never treat error as >8 cm + self.declare_parameter("lost_timeout_sec", 1.2) # 别名 / 用时间戳年龄兜底 + self.declare_parameter("e_xy_clip_m", 0.080) # 误差绝不按大于 8 cm 处理 - # --- Velocity LPF (smooth 100Hz → traj) --- + # --- 速度低通滤波（把 100Hz 平滑成轨迹） --- self.declare_parameter("vel_lpf_alpha", 0.40) self.declare_parameter("wz_lpf_alpha", 0.40) - # --- JTC dq sliding window --- + # --- JTC 的 dq 滑动窗口 --- self.declare_parameter("dq_window_size", 5) self.declare_parameter("min_ee_z_m", 0.20) self.declare_parameter("workspace_xyz_min", [0.15, -0.50, 0.18]) self.declare_parameter("workspace_xyz_max", [0.80, 0.50, 0.60]) - # Base-frame EE hold (flange). Z/XY in metres, editable via site.yaml. + # base 系下的末端保持（法兰）。Z/XY 单位为米，可通过 site.yaml 修改。 self.declare_parameter("ee_x_m", 0.25) self.declare_parameter("ee_y_m", 0.00) self.declare_parameter("ee_z_m", 0.25) self.declare_parameter("ee_hold_xy", False) self.declare_parameter("ee_vertical_down", True) - # hover = flange/gripper-center XY on the 3D feature (any lock height). - # lookat = image center. hover_offset_ee_xy_m is flange-XY to jaw center. + # hover = 让法兰/夹爪中心 XY 停在三维特征点上（任意锁定高度都可）。 + # lookat = 对准图像中心。hover_offset_ee_xy_m 是法兰 XY 到爪中心的偏移。 self.declare_parameter("xy_track", "hover") self.declare_parameter("hover_offset_ee_xy_m", [0.0, 0.0]) self.declare_parameter("arm_label", "") @@ -144,14 +144,14 @@ class ServoNode(Node): self.declare_parameter("camera_fy", 390.0) self.declare_parameter("camera_cx", 320.0) self.declare_parameter("camera_cy", 240.0) - # Seek: at lock height with no valid target, scan the work area so the - # eye-in-hand camera can find the workpiece on its own (自适应找工件). + # Seek：到达锁定高度却没有有效目标时，扫描工作区域，让 eye-in-hand + # 相机自己去找到工件（自适应找工件）。 self.declare_parameter("seek_enabled", True) self.declare_parameter("seek_radius_max_m", 0.16) self.declare_parameter("seek_speed_mps", 0.05) self.declare_parameter("seek_timeout_sec", 45.0) - # Hover integral: breaks the ~4mm joint-command deadband that otherwise - # stalls the last few mm of XY convergence onto the workpiece. + # 悬停积分：用来突破约 4mm 的关节指令死区，否则 XY 收敛到最后几毫米 + # 时就会停滞、贴不到工件上。 self.declare_parameter("ki_hover", 0.8) # 串联用：XY 入死区 + Z 到锁高持续这么久 → /visual_servo/locked=True self.declare_parameter("lock_hold_sec", 2.0) @@ -253,7 +253,7 @@ class ServoNode(Node): f"HANDEYE yaml={he_path} " f"cam_in_flange=({t_he[0]*1000:.1f},{t_he[1]*1000:.1f},{t_he[2]*1000:.1f})mm " f"optical_vs_flangeZ={self._cam_tilt_deg:.1f}deg " - f"— hover uses this T, not TF lookup" + f"— 悬停使用这个 T，而不是用 TF 查询" ) self.pid_x = PidController( @@ -287,12 +287,12 @@ class ServoNode(Node): self._lock = threading.Lock() self._p_meas = np.zeros(3, dtype=float) self._stamp_sec = -1.0 - self._last_seen_time = -1.0 # monotonic, updated on every PointStamped + self._last_seen_time = -1.0 # 单调时钟，每次收到 PointStamped 都会更新 self._have_target = False self._target_lost_flag = True self._v_raw = np.zeros(3, dtype=float) - self._v_filt = np.zeros(3, dtype=float) # LPF linear (cam) - self._wz_filt = 0.0 # reserved angular z LPF + self._v_filt = np.zeros(3, dtype=float) # 线速度低通（相机系） + self._wz_filt = 0.0 # 预留的角速度 z 低通 self._wz_raw = 0.0 self._zeroed = True self._last_js: Optional[JointState] = None @@ -305,14 +305,14 @@ class ServoNode(Node): self._p_feat_base: Optional[np.ndarray] = None self._he_cmp_done = False self._feat_kf = KalmanFilter3D(q_pos=2.0e-3, q_vel=1.5e-1, r_pos=4.0e-4) - # XY hover only after flange Z is on the lock plane (fresh depth). + # 只有当法兰 Z 已经落到锁定平面上（深度新鲜）才开启 XY 悬停。 self._xy_armed = bool(self.ee_z_m <= 0.0) - # Seek (search) state: scan for the workpiece when at lock_z with no target. + # Seek（搜索）状态：在锁定高度且没有目标时扫描工件。 self._seek_t0 = 0.0 self._seek_theta = 0.0 self._seek_origin = (0.0, 0.0) self._seek_warned = False - self._hover_ixy = np.zeros(2, dtype=float) # hover integral (anti-deadband) + self._hover_ixy = np.zeros(2, dtype=float) # 悬停积分（抗死区） point_topic = str(self.get_parameter("point_topic").value) point_qos = QoSProfile( @@ -363,10 +363,10 @@ class ServoNode(Node): callback_group=self._control_cbg, ) - mode = "CONTROL ON" if self.enable_control else "cmd_vel only" + mode = "控制已开启" if self.enable_control else "仅 cmd_vel" arm = self.arm_label or self.ee_frame self.get_logger().info( - f"servo ready [{arm}] [{mode}] {control_hz:.0f}Hz watchdog={self.watchdog_timeout:.2f}s " + f"伺服就绪 [{arm}] [{mode}] {control_hz:.0f}Hz watchdog={self.watchdog_timeout:.2f}s " f"vel_lpf_α={self.vel_lpf_alpha:.2f} dq_win={self.dq_window_size} " f"lock_z={self.ee_z_m:.3f}m (site servo.ee_z_m) " f"xy_track={self.xy_track} cam_tilt={self._cam_tilt_deg:.1f}° " @@ -381,7 +381,7 @@ class ServoNode(Node): self._last_js = msg def _on_point(self, msg: PointStamped) -> None: - """Fresh vision measurement — arm the watchdog and clear lost flag.""" + """新的视觉测量 —— 给看门狗“上弦”并清除丢失标志。""" p_cam = np.array( [float(msg.point.x), float(msg.point.y), float(msg.point.z)], dtype=float ) @@ -417,7 +417,7 @@ class ServoNode(Node): self._last_seen_time = now_m self._have_target = True self._target_lost_flag = False - self._seek_t0 = 0.0 # target acquired — reset seek for next time + self._seek_t0 = 0.0 # 已捕获目标 —— 重置 seek，供下次使用 self._seek_warned = False if msg.header.frame_id: self.camera_frame = msg.header.frame_id @@ -432,12 +432,12 @@ class ServoNode(Node): if was_lost: self._reacq_mono = time.monotonic() self.get_logger().info( - "target reacquired — resume servo", + "已重新捕获目标 —— 继续伺服", throttle_duration_sec=0.5, ) def _reset_motion_state(self) -> None: - """Hard stop internals: I=0, filt vel=0, dq window clear.""" + """硬停内部状态：I=0、滤波速度=0、dq 窗口清空。""" self.pid_x.reset() self.pid_y.reset() self.pid_z.reset() @@ -455,9 +455,9 @@ class ServoNode(Node): self._twist_pub.publish(tw) def _watchdog_trip(self, reason: str) -> None: - """Priority-1 hard stop: no extrapolate, zero cmd, zero traj delta.""" + """优先级 1 的硬停：不外推、指令置零、轨迹增量为零。""" if not self._target_lost_flag: - self.get_logger().warn(f"WATCHDOG STOP: {reason}") + self.get_logger().warn(f"看门狗急停：{reason}") self._target_lost_flag = True self._reset_motion_state() self._zeroed = True @@ -475,7 +475,7 @@ class ServoNode(Node): if self._ctrl_count >= int(round(1.0 / self._ctrl_dt)): elapsed = (now_msg - self._ctrl_t0).nanoseconds * 1e-9 hz = self._ctrl_count / max(elapsed, 1e-6) - self.get_logger().info(f"cmd_vel rate ~{hz:.1f} Hz", throttle_duration_sec=1.0) + self.get_logger().info(f"cmd_vel 速率约 {hz:.1f} Hz", throttle_duration_sec=1.0) self._ctrl_count = 0 self._ctrl_t0 = now_msg @@ -486,9 +486,9 @@ class ServoNode(Node): p_meas = self._p_meas.copy() lost = self._target_lost_flag - # ----- Startup: descend to lock height FIRST, even before any detection. - # have=False only until the first PointStamped ever arrives, so this is - # a pure startup phase: Z-only descent, XY pinned, vertical-down. + # ----- 启动阶段：**先**下降到锁定高度，甚至在任何检测到来之前。 + # 在第一个 PointStamped 到达之前 have 一直是 False，所以这是一个纯启动 + # 阶段：只动 Z 下降，XY 锁死，姿态朝下。 if ( self.enable_control and self.ee_z_m > 0.0 @@ -497,22 +497,21 @@ class ServoNode(Node): ): T_ee0 = self._lookup_T(self.base_frame, self.ee_frame) if T_ee0 is None: - return # TF not ready yet — wait, do not hold-trip + return # TF 还没就绪 —— 等待，不要触发保持急停 self._update_xy_arm(float(T_ee0[2, 3])) if not self._xy_armed: self.get_logger().info( - f"startup descend to lock_z {self.ee_z_m:.3f}m " - f"now={T_ee0[2, 3]:.3f}m (no target yet, XY pinned)", + f"启动下降至 lock_z {self.ee_z_m:.3f}m " + f"当前={T_ee0[2, 3]:.3f}m（还没有目标，XY 锁死）", throttle_duration_sec=1.0, ) - self._target_lost_flag = False # allow execute during blind descent + self._target_lost_flag = False # 盲降期间允许执行 self._execute_twist(np.zeros(3, dtype=float), p_cam=None, hold_xy=True) self._lock_feedback(False) return - # ----- Seek: at lock height but no valid target yet. The eye-in-hand - # camera only sees a small patch, so scan the work area (drift to the - # nominal work XY, then spiral out) until the workpiece enters view. + # ----- Seek：已在锁定高度但还没有有效目标。eye-in-hand 相机只能看到 + # 一小块区域，所以扫描工作区（先漂到标称工作 XY，再向外螺旋）直到工件进入视野。 if ( self.enable_control and self.seek_enabled @@ -522,53 +521,53 @@ class ServoNode(Node): ): T_ee0 = self._lookup_T(self.base_frame, self.ee_frame) if T_ee0 is None: - return # TF not ready — wait, do not hold-trip + return # TF 还没就绪 —— 等待，不要触发保持急停 v_seek = self._seek_velocity(T_ee0[:3, 3]) if v_seek is None: - # timed out — give up searching, hold in place + # 超时了 —— 放弃搜索，原地保持 if not self._seek_warned: self._seek_warned = True self.get_logger().warn( - f"seek timeout {self.seek_timeout:.0f}s — workpiece not found. " - "Hold position. Put the workpiece on the table in view, or widen " - "seek_radius_max_m / check camera mount & handeye." + f"seek 超时 {self.seek_timeout:.0f}s —— 没找到工件。" + "保持当前位置。请把工件放到桌面视野内，或调大 " + "seek_radius_max_m / 检查相机安装与手眼标定。" ) self._lock_feedback(False) self._pub_zero_twist() self._send_hold() return self.get_logger().info( - f"seek workpiece… ee_xy=({T_ee0[0,3]:.3f},{T_ee0[1,3]:.3f}) " + f"寻找工件…… ee_xy=({T_ee0[0,3]:.3f},{T_ee0[1,3]:.3f}) " f"v=({v_seek[0]:+.3f},{v_seek[1]:+.3f})", throttle_duration_sec=1.0, ) - self._target_lost_flag = False # allow execute during seek + self._target_lost_flag = False # seek 期间允许执行 self._execute_twist( np.zeros(3, dtype=float), p_cam=None, v_base_override=v_seek ) self._lock_feedback(False) return - # ----- Item 1: Watchdog (receive-time) — freeze ALL motion if stale ----- + # ----- 第 1 项：看门狗（按接收时间）—— 数据过期就冻结所有运动 ----- if (not have) or last_seen < 0.0: self._lock_feedback(False) - self._watchdog_trip("no PointStamped yet") + self._watchdog_trip("还没有收到 PointStamped") return seen_age = now_mono - last_seen stamp_age = now_sec - stamp if stamp >= 0.0 else 1e9 if seen_age > self.watchdog_timeout or stamp_age > self.lost_timeout: self._watchdog_trip( - f"target lost seen_age={seen_age:.3f}s stamp_age={stamp_age:.3f}s" + f"目标丢失 seen_age={seen_age:.3f}s stamp_age={stamp_age:.3f}s" ) self._lock_feedback(False) return - # Frozen box / USB stall: pin joints, do not keep driving last error. + # 检测框冻结 / USB 卡住：锁住关节，不要继续按上一个误差驱动。 stale = max(0.15, float(self.stale_hold_sec)) if seen_age > stale or stamp_age > stale: self.get_logger().info( - f"stale hold seen_age={seen_age:.2f}s — not tracking a frozen box", + f"过期保持 seen_age={seen_age:.2f}s —— 不跟踪冻结的检测框", throttle_duration_sec=1.0, ) self._v_raw[:] = 0.0 @@ -580,13 +579,13 @@ class ServoNode(Node): return if lost: - # Should have been cleared on _on_point; belt-and-suspenders + # 本应在 _on_point 里清掉；这里再加一道保险 self._lock_feedback(False) self._pub_zero_twist() return - # Hover uses the vision 3D point as-is. Do not add cmd_vel (wrong frame) - # onto p_cam — that walks the target off the workpiece. + # Hover 直接使用视觉三维点。不要把 cmd_vel（坐标系不对） + # 叠加到 p_cam 上 —— 那样会把目标从工件上带跑。 p = p_meas.copy() if self.xy_track == "lookat": dt_lat = float(np.clip(stamp_age, 0.0, self.max_extrap)) @@ -618,7 +617,7 @@ class ServoNode(Node): ex *= s ey *= s r_xy = clip_xy - # Camera-depth Z is unused when holding flange height in base frame + # 在 base 系里保持法兰高度时，相机深度 Z 就不再使用 z_dead = True if self.ee_z_m > 0.0 else (abs(ez) <= self.deadzone_z_m) if xy_dead: ex = ey = 0.0 @@ -628,7 +627,7 @@ class ServoNode(Node): ez = 0.0 self.pid_z.reset() - # ----- Item 2: PID with integral separation (inside PidController) ----- + # ----- 第 2 项：带积分分离的 PID（分离逻辑在 PidController 内部） ----- v_des = np.array( [ 0.0 if self.ee_hold_xy else self.pid_x.update(ex, float(p[0]), dt), @@ -644,7 +643,7 @@ class ServoNode(Node): v_des[:2] = _saturate_vec(v_des[:2].copy(), self.v_max_xy) v_des[2] = float(np.clip(v_des[2], -self.v_max_z, self.v_max_z)) - # Base-plane mode: never command camera +Z (that dives into the part) + # base 平面模式：绝不指令相机 +Z（那会直接扎进工件里） if self.ee_z_m > 0.0: v_des[2] = 0.0 else: @@ -661,7 +660,7 @@ class ServoNode(Node): if z_meas < (self.z_min_m - 0.02): v_des[2] = min(v_des[2], -0.03) - # Accel limit on raw desire, then LPF + # 先对原始期望速度做加速度限制，再做低通滤波 dv = v_des - self._v_raw n_dv = float(np.linalg.norm(dv)) dv_max = self.a_max * dt @@ -669,10 +668,10 @@ class ServoNode(Node): dv *= dv_max / n_dv self._v_raw = self._v_raw + dv - # ----- Item 3: first-order LPF on linear (+ reserved wz) ----- + # ----- 第 3 项：对线速度做一阶低通（+ 预留的 wz） ----- a = self.vel_lpf_alpha self._v_filt = a * self._v_raw + (1.0 - a) * self._v_filt - # Angular reserved (PBVS currently linear-only); still filter for API completeness + # 角速度目前预留（PBVS 当前只做线速度）；仍做滤波以保持接口完整 self._wz_raw = 0.0 aw = self.wz_lpf_alpha self._wz_filt = aw * self._wz_raw + (1.0 - aw) * self._wz_filt @@ -706,11 +705,11 @@ class ServoNode(Node): if xy_dead and z_dead: self._lock_feedback(True) # 串联信号：活目标 + 死区驻留 = 收敛 self.get_logger().info( - f"deadzone hold |e_xy|={r_xy_raw*1000:.1f}mm ez={ez*1000:.1f}mm", + f"死区保持 |e_xy|={r_xy_raw*1000:.1f}mm ez={ez*1000:.1f}mm", throttle_duration_sec=1.0, ) if self.enable_control: - # Still pin height / vertical-down if slightly off, else freeze + # 若高度 / 垂直朝下稍有偏差仍要修正，否则彻底冻结 if need_base: self._execute_twist(np.zeros(3, dtype=float), p_cam=p, hold_xy=True) elif not self._zeroed: @@ -728,7 +727,7 @@ class ServoNode(Node): f" feat=({look[1][0]:.3f},{look[1][1]:.3f})" ) self.get_logger().info( - f"twist cam v=({v_cam[0]:+.3f},{v_cam[1]:+.3f},{v_cam[2]:+.3f}) " + f"twist 相机系 v=({v_cam[0]:+.3f},{v_cam[1]:+.3f},{v_cam[2]:+.3f}) " f"|e_xy|={r_xy_raw*1000:.0f}mm ez={ez*1000:.0f}mm age={stamp_age*1000:.0f}ms" f"{extra}", throttle_duration_sec=0.5, @@ -739,14 +738,14 @@ class ServoNode(Node): self._execute_twist(v_cam, p_cam=p) def _e_stop(self, reason: str) -> None: - self.get_logger().warn(f"E-STOP: {reason}") + self.get_logger().warn(f"急停：{reason}") self._watchdog_trip(reason) def _R_cam_to_base(self, T_ee: np.ndarray) -> np.ndarray: return (T_ee @ self._T_ee_cam())[:3, :3] def _T_ee_cam(self) -> np.ndarray: - # Always the calibrated yaml. TF is only for RViz / a sanity print. + # 始终使用标定出的 yaml。TF 只用于 RViz / 打印做核对。 return np.array(self.T_cam_to_gripper, dtype=float) def _log_handeye_tf_vs_yaml(self) -> None: @@ -754,7 +753,7 @@ class ServoNode(Node): T_tf = self._lookup_T(self.ee_frame, self.handeye_child) if T_tf is None: self.get_logger().warn( - "HANDEYE TF lookup failed (ok) — servo uses yaml " + "HANDEYE TF 查询失败（无妨）—— 伺服使用 yaml " f"t=({t_yaml[0]*1000:.1f},{t_yaml[1]*1000:.1f},{t_yaml[2]*1000:.1f})mm" ) return @@ -767,8 +766,8 @@ class ServoNode(Node): ) if d > 0.015: self.get_logger().error( - "HANDEYE TF ≠ yaml by >15mm — duplicate parent on camera frame? " - "Servo still uses yaml." + "HANDEYE TF 与 yaml 的偏差 >15mm —— 相机坐标系上挂了重复的父节点？ " + "伺服仍然使用 yaml。" ) def _intersect_z( @@ -789,11 +788,11 @@ class ServoNode(Node): def _lookat_vs_feature( self, T_ee: np.ndarray, p_cam: np.ndarray ) -> Optional[tuple]: - """Base XY error that centers the feature in the image. + """让特征点居中到图像里所需的 base 系 XY 误差。 - 30° eye-in-hand: image center is the optical-axis hit on the work - plane, not the flange XY. Translating the flange in XY at fixed - height/attitude moves that look-at 1:1. + 30° 的 eye-in-hand 安装：图像中心是光轴打到工作平面上的点， + 而不是法兰的 XY。在固定高度 / 姿态下平移法兰 XY，会把该 look-at 点 + 按 1:1 一起带走。 """ T_bc = T_ee @ self._T_ee_cam() R = T_bc[:3, :3] @@ -812,7 +811,7 @@ class ServoNode(Node): return err, p_feat, p_look def _feature_in_base(self, T_ee: np.ndarray, p_cam: np.ndarray) -> np.ndarray: - """Workpiece 3D point (camera frame) → base XYZ. Keep XYZ consistent.""" + """工件三维点（相机系）→ base 系 XYZ。保持 XYZ 一致。""" T_bc = T_ee @ self._T_ee_cam() ph = np.array( [float(p_cam[0]), float(p_cam[1]), float(p_cam[2]), 1.0], dtype=float @@ -822,11 +821,11 @@ class ServoNode(Node): def _hover_vs_feature( self, T_ee: np.ndarray, p_cam: np.ndarray ) -> Optional[tuple]: - """Park flange (and coaxial gripper center) XY on the workpiece XY in base. + """把法兰（以及同轴的夹爪中心）的 XY 停在 base 系里的工件 XY 上。 - hover_offset_ee_xy_m shifts the tracked point in the flange frame - (jaw center if it is not on the flange origin). RGI is coaxial: leave [0,0]. - Must NOT re-multiply a stale p_cam by the live T_ee. + hover_offset_ee_xy_m 会在法兰坐标系里平移被跟踪点 + （当爪中心不在法兰原点时用它）。RGI 是同轴的：保持 [0,0] 即可。 + 绝不能拿实时的 T_ee 去重复乘一个过期的 p_cam。 """ p_ee = np.asarray(T_ee[:3, 3], dtype=float) off = self.hover_offset_ee_xy @@ -887,7 +886,7 @@ class ServoNode(Node): return None def _xy_deadzone(self, r_xy: float, *, update: bool = True) -> bool: - """Hysteresis: enter at deadzone_m, leave at 1.6× so noise does not chatter.""" + """滞回：在 deadzone_m 处进入，在 1.6 倍处退出，避免噪声来回抖。""" enter = max(0.002, float(self.deadzone_m)) leave = enter * 1.6 if self._xy_in_dead: @@ -925,7 +924,7 @@ class ServoNode(Node): self._lock_since = None def _w_align_flange_down(self, R_ee: np.ndarray) -> np.ndarray: - """Angular vel in base to put flange +Z along world −Z (tool down).""" + """base 系下的角速度，用来把法兰 +Z 转到世界 −Z 方向（工具朝下）。""" z_now = np.asarray(R_ee, dtype=float).reshape(3, 3)[:, 2] z_des = np.array([0.0, 0.0, -1.0], dtype=float) c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0)) @@ -942,7 +941,7 @@ class ServoNode(Node): return axis * mag def _shape_xy_vel(self, v_des_xy: np.ndarray, dt: float) -> np.ndarray: - """Slew-rate + LPF on the hover command that actually moves the arm.""" + """真正驱动机械臂的那条悬停指令，外加变化率限制 + 低通滤波。""" v_des = np.asarray(v_des_xy, dtype=float).reshape(2) prev = self._v_xy_cmd dv = v_des - prev @@ -961,7 +960,7 @@ class ServoNode(Node): return out def _update_xy_arm(self, ee_z: float) -> bool: - """XY hover only after flange is on lock_z (real depth, adaptive hover).""" + """只有当法兰到达 lock_z（真实深度、自适应悬停）后才做 XY 悬停。""" if self.ee_z_m <= 0.0: self._xy_armed = True return True @@ -977,8 +976,8 @@ class ServoNode(Node): self.pid_x.reset() self.pid_y.reset() self.get_logger().warn( - f"lock-z ready ee_z={z:.3f}m (want {self.ee_z_m:.3f}, no climb) " - "— start XY hover on live handeye" + f"lock-z 就绪 ee_z={z:.3f}m（期望 {self.ee_z_m:.3f}，不再回抬） " + "—— 在实时 handeye 上开始 XY 悬停" ) return True err = abs(z - float(self.ee_z_m)) @@ -991,8 +990,8 @@ class ServoNode(Node): self.pid_x.reset() self.pid_y.reset() self.get_logger().warn( - f"lock-z reached ee_z={z:.3f}m (want {self.ee_z_m:.3f}) " - "— start XY hover on live handeye" + f"已到达 lock-z ee_z={z:.3f}m（期望 {self.ee_z_m:.3f}）" + " —— 在实时 handeye 上开始 XY 悬停" ) elif err > gate + 0.05: self._xy_armed = False @@ -1000,20 +999,19 @@ class ServoNode(Node): self._feat_kf.initialized = False self._v_xy_cmd[:] = 0.0 self.get_logger().info( - f"left lock-z band ee_z={z:.3f}m — XY hold, re-descend", + f"已离开 lock-z 区间 ee_z={z:.3f}m —— XY 保持，重新下降", throttle_duration_sec=1.0, ) return self._xy_armed def _seek_velocity(self, p_ee: np.ndarray) -> Optional[np.ndarray]: - """Base-frame XY velocity to scan for the workpiece: spiral outward around - the point where the arm reached lock height, so the eye-in-hand camera - sweeps the table. Returns None when the seek has timed out (caller holds).""" + """base 系下的 XY 速度，用来扫描寻找工件：以「机械臂到达锁定高度时的位置」 + 为中心向外螺旋，让 eye-in-hand 相机扫过桌面。超时后返回 None（由调用方保持）。""" now = time.monotonic() if self._seek_t0 <= 0.0: self._seek_t0 = now self._seek_theta = 0.0 - self._seek_origin = (float(p_ee[0]), float(p_ee[1])) # search near descent XY + self._seek_origin = (float(p_ee[0]), float(p_ee[1])) # 在下降点附近搜索 XY if (now - self._seek_t0) > self.seek_timeout: return None ox, oy = self._seek_origin @@ -1037,7 +1035,7 @@ class ServoNode(Node): def _clip_workspace_vel( self, p_ee: np.ndarray, v_base: np.ndarray, T: float ) -> np.ndarray: - """Keep commands inside the box; if already outside, allow recovery in.""" + """把指令限制在盒子里；如果本来就在盒子外，允许它往回恢复。""" v = np.asarray(v_base, dtype=float).copy() dt = max(float(T), 1e-3) for i in range(3): @@ -1063,7 +1061,7 @@ class ServoNode(Node): hold_xy: bool = False, v_base_override: Optional[np.ndarray] = None, ) -> None: - # Hard freeze if watchdog already tripped mid-cycle + # 如果看门狗在本周期中途已经触发，就硬冻结 if self._target_lost_flag: self._dq_hist.clear() return @@ -1078,7 +1076,7 @@ class ServoNode(Node): T_ee = self._lookup_T(self.base_frame, self.ee_frame) if q is None or T_ee is None: self.get_logger().warn( - "execute skip: missing joint_states or TF base→ee", + "execute 跳过：缺少 joint_states 或 base→ee 的 TF", throttle_duration_sec=1.0, ) return @@ -1097,8 +1095,8 @@ class ServoNode(Node): if self._reacq_mono > 0.0 and (now_m - self._reacq_mono) < 0.12: v_xy *= 0.85 - # hover: park flange XY on the 3D feature at any lock height. - # lookat: drive optical-axis hit (image center) onto the feature. + # hover：在任意锁定高度上把法兰 XY 停在三维特征点上。 + # lookat：把光轴命中点（图像中心）驱到特征点上。 xy_ok = self._update_xy_arm(float(p_ee[2])) if ( xy_ok @@ -1120,11 +1118,11 @@ class ServoNode(Node): v_base[1] = 0.0 self._hover_ixy[:] = 0.0 else: - # Taper near the deadzone so P-control does not bang-bang. + # 在死区附近做渐变，避免 P 控制来回 bang-bang。 enter = max(0.002, float(self.deadzone_m)) taper = float(np.clip((nerr - enter) / max(enter, 1e-3), 0.0, 1.0)) - # Integral (anti-windup ±4cm) breaks the joint-command deadband - # that otherwise stalls the last few mm of XY convergence. + # 积分项（抗积分饱和 ±4cm）用来突破关节指令死区， + # 否则 XY 收敛的最后几毫米就会停滞。 self._hover_ixy[0] = float(np.clip( self._hover_ixy[0] + float(err[0]) * dt_use, -0.04, 0.04)) self._hover_ixy[1] = float(np.clip( @@ -1149,8 +1147,8 @@ class ServoNode(Node): v_base[0] *= v_xy / nxy v_base[1] *= v_xy / nxy - # Hold flange XYZ in base (config, default ~250 mm). Visual XY is - # overwritten when ee_hold_xy is true. + # 在 base 系里保持法兰 XYZ（可配置，默认约 250 mm）。当 ee_hold_xy + # 为真时，视觉算出的 XY 会被覆盖。 if self.ee_hold_xy: v_base[0] = float(np.clip(kp * (self.ee_x_m - p_ee[0]), -self.v_max_xy, self.v_max_xy)) v_base[1] = float(np.clip(kp * (self.ee_y_m - p_ee[1]), -self.v_max_xy, self.v_max_xy)) @@ -1164,7 +1162,7 @@ class ServoNode(Node): self._v_xy_cmd[:] = 0.0 if self.ee_z_m > 0.0: self.get_logger().info( - f"descend to lock_z {self.ee_z_m:.3f}m now={p_ee[2]:.3f}m XY hold", + f"下降至 lock_z {self.ee_z_m:.3f}m 当前={p_ee[2]:.3f}m XY 保持", throttle_duration_sec=1.0, ) shaped = self._shape_xy_vel(v_base[:2], dt_use) @@ -1189,7 +1187,7 @@ class ServoNode(Node): p_next = p_ee + v_base * T if self.ee_z_m > 0.0: - # Never command below the configured plane + # 绝不指令到配置平面以下 floor = max(self.min_ee_z, self.ee_z_m - 0.02) if p_next[2] < floor and v_base[2] < 0.0: v_base[2] = 0.0 @@ -1205,7 +1203,7 @@ class ServoNode(Node): Tl = self._lookup_T(self.base_frame, fr) if Tl is None: self.get_logger().warn( - f"execute skip: TF miss {fr}", + f"execute 跳过：TF 缺 {fr}", throttle_duration_sec=1.0, ) return @@ -1217,10 +1215,10 @@ class ServoNode(Node): ) dq = qdot * T - # ----- Item 5: sliding-window average on joint displacement ----- + # ----- 第 5 项：对关节位移做滑动窗口平均 ----- self._dq_hist.append(dq.copy()) dq_avg = np.mean(np.stack(list(self._dq_hist), axis=0), axis=0) - # Keep end-velocity consistent with averaged step + # 让末端速度与平均后的步长保持一致 qdot_avg = dq_avg / T mag = float(np.max(np.abs(dq_avg))) @@ -1281,7 +1279,7 @@ def main(args=None) -> None: except KeyboardInterrupt: pass finally: - node._e_stop("node shutdown") + node._e_stop("节点关闭") executor.shutdown() node.destroy_node() if rclpy.ok(): diff --git a/src/arm_application/arm_application/site_io.py b/src/arm_application/arm_application/site_io.py index 6bb88d1..10a8a73 100644 --- a/src/arm_application/arm_application/site_io.py +++ b/src/arm_application/arm_application/site_io.py @@ -1,4 +1,4 @@ -"""Load ~/ros2/config/site.yaml from application nodes.""" +"""供应用节点加载 ``~/ros2/config/site.yaml``。""" from __future__ import annotations diff --git a/src/arm_application/arm_application/stage2_locate_node.py b/src/arm_application/arm_application/stage2_locate_node.py index a94e1f8..e06108d 100644 --- a/src/arm_application/arm_application/stage2_locate_node.py +++ b/src/arm_application/arm_application/stage2_locate_node.py @@ -149,8 +149,8 @@ class Stage2Locate(Node): self.declare_parameter("robot_sn", os.environ.get("SITE_ROBOT_SN") or "") self.declare_parameter("hover_only", False) - self.declare_parameter("target_hole", "") # "" → site stage2.target_hole - self.declare_parameter("hover_z_m", -1.0) # >0 → 覆盖 site + self.declare_parameter("target_hole", "") # 空字符串 → 用 site 的 stage2.target_hole + self.declare_parameter("hover_z_m", -1.0) # 大于 0 → 覆盖 site 里的值 self.declare_parameter("preview", True) self.declare_parameter("action_name", "rizon_arm_controller/follow_joint_trajectory") diff --git a/src/arm_application/arm_application/tactile_cli.py b/src/arm_application/arm_application/tactile_cli.py index d39f027..d8cced6 100644 --- a/src/arm_application/arm_application/tactile_cli.py +++ b/src/arm_application/arm_application/tactile_cli.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Orisys visuo-tactile CLI. Ports / config from site.yaml tactile:.""" +"""Orisys 视触觉命令行工具。端口 / 配置来自 ``site.yaml`` 的 ``tactile`` 段。""" from __future__ import annotations @@ -12,7 +12,7 @@ from arm_application.site_io import load_site_dict, site_path def main() -> int: - parser = argparse.ArgumentParser(description="Orisys tactile CLI (site.yaml)") + parser = argparse.ArgumentParser(description="Orisys 触觉命令行工具（site.yaml）") parser.add_argument("--duration", type=float, default=10.0) parser.add_argument("--site", default="") args = parser.parse_args() @@ -29,7 +29,7 @@ def main() -> int: try: tactile.connect() except Exception as exc: - print(f"ERROR: {exc}", file=sys.stderr) + print(f"错误：{exc}", file=sys.stderr) return 1 t0 = time.time() n = 0 @@ -49,10 +49,10 @@ def main() -> int: ) time.sleep(0.01) except KeyboardInterrupt: - print("interrupt") + print("已中断") finally: tactile.disconnect() - print(f"done frames={n}") + print(f"完成 frames={n}") return 0 diff --git a/src/arm_application/arm_application/tactile_interface.py b/src/arm_application/arm_application/tactile_interface.py index f538d80..6f369ea 100644 --- a/src/arm_application/arm_application/tactile_interface.py +++ b/src/arm_application/arm_application/tactile_interface.py @@ -1,4 +1,4 @@ -"""Tactile sample / command types. Backend (Orisys / mock) is swappable.""" +"""触觉采样 / 指令类型。后端（Orisys / mock）可替换。""" from __future__ import annotations diff --git a/src/arm_application/arm_application/yolo_detector.py b/src/arm_application/arm_application/yolo_detector.py index 40ab1da..38edb80 100644 --- a/src/arm_application/arm_application/yolo_detector.py +++ b/src/arm_application/arm_application/yolo_detector.py @@ -1,9 +1,9 @@ #!/usr/bin/env python3 -"""Ultralytics YOLO / TensorRT wrapper. +"""Ultralytics YOLO / TensorRT 封装。 -Public: ``resolve_weights``, ``YoloDetector``, ``Detection``, ``pick_best``, -``MpYoloDetector``. TensorRT loads the sibling ``.engine`` next to the -``.onnx`` / ``.pt`` path when ``backend='tensorrt'``. +对外接口：``resolve_weights``、``YoloDetector``、``Detection``、``pick_best``、 +``MpYoloDetector``。当 ``backend='tensorrt'`` 时，TensorRT 会加载与 +``.onnx`` / ``.pt`` 路径同级的 ``.engine`` 文件。 类别名（标签）由 ``model_labels`` 统一提供：优先读权重旁边的 ``<模型名>.labels.yaml``（用户可随时手改），其次读模型内嵌 metadata。 @@ -33,13 +33,13 @@ from .model_labels import ( logger = logging.getLogger(__name__) -# Sentinel: worker still on the previous frame — caller must NOT count a miss. +# 哨兵值：worker 还停在上一次的那帧 —— 调用方不能把它算作漏检。 INFER_BUSY = object() @dataclass class Detection: - xyxy: np.ndarray # (4,) float: x1,y1,x2,y2 + xyxy: np.ndarray # (4,) float：x1,y1,x2,y2 conf: float cls_id: int cls_name: str @@ -69,7 +69,7 @@ def _resolve_device(device: str) -> str: def resolve_weights(weights: Union[str, Path], backend: str = "auto") -> Path: - """Prefer TensorRT .engine next to the .pt when backend=auto/tensorrt.""" + """当 backend=auto/tensorrt 时，优先使用与 .pt 同级的 TensorRT .engine。""" p = Path(weights).expanduser() b = (backend or "auto").strip().lower() engine = p.with_suffix(".engine") @@ -78,13 +78,13 @@ def resolve_weights(weights: Union[str, Path], backend: str = "auto") -> Path: if engine.is_file(): return engine raise FileNotFoundError( - f"TensorRT engine missing: {engine}. " - f"Export: bash ~/ros2/scripts/weights.sh convert {p} --name {p.stem}" + f"缺少 TensorRT engine：{engine}。" + f"导出命令：bash ~/ros2/scripts/weights.sh convert {p} --name {p.stem}" ) if b == "onnx": if onnx.is_file(): return onnx - raise FileNotFoundError(f"ONNX missing: {onnx}") + raise FileNotFoundError(f"缺少 ONNX 文件：{onnx}") if b in ("", "auto"): if engine.is_file(): return engine @@ -155,19 +155,19 @@ class YoloDetector: from ultralytics import YOLO except Exception as exc: # noqa: BLE001 raise RuntimeError( - "ultralytics not installed. In ros_flexiv: pip install ultralytics" + "未安装 ultralytics。在 ros_flexiv 环境里执行：pip install ultralytics" ) from exc path = Path(weights).expanduser() if not path.is_file(): raise FileNotFoundError( - f"YOLO weights not found: {path}. " - f"Place your .pt at ~/ros2/models/yolo/workpiece.pt" + f"找不到 YOLO 权重：{path}。" + f"请把你的 .pt 放到 ~/ros2/models/yolo/workpiece.pt" ) try: import torch - # Do not let PyTorch occupy all CPU cores (camera + ROS need them). + # 别让 PyTorch 占满所有 CPU 核心（相机 + ROS 都要用）。 torch.set_num_threads(1) torch.set_num_interop_threads(1) if torch.cuda.is_available(): @@ -243,7 +243,7 @@ class YoloDetector: imgsz: int | None = None, **_kwargs, ) -> List[Detection]: - """Alias used by hole detectors (xyxy + conf).""" + """给孔检测器用的别名接口（xyxy + conf）。""" if conf is not None: self.conf = float(conf) if iou is not None: @@ -302,7 +302,7 @@ class YoloDetector: def _yolo_worker_main(q_in, q_out, cfg: dict) -> None: - """Spawned process: GPU YOLO only. Never import rclpy here.""" + """spawn 出来的子进程：只跑 GPU 上的 YOLO。这里绝不能 import rclpy。""" try: path = resolve_weights(cfg["weights"], cfg.get("backend") or "auto") det = YoloDetector( @@ -321,7 +321,7 @@ def _yolo_worker_main(q_in, q_out, cfg: dict) -> None: item = q_in.get() if item is None: return - # Drain to the newest queued frame — never spend GPU on stale images. + # 一直排空到队列里最新的帧 —— 绝不把 GPU 浪费在过期图像上。 try: while True: newer = q_in.get_nowait() @@ -345,7 +345,7 @@ def _yolo_worker_main(q_in, q_out, cfg: dict) -> None: def _put_drop_old(q, item) -> None: - """Never block the GPU process on a full result queue.""" + """绝不让 GPU 进程因为结果队列满了而阻塞。""" try: q.put_nowait(item) return @@ -362,7 +362,7 @@ def _put_drop_old(q, item) -> None: class YoloInferProcess: - """YOLO in a spawn() child so ROS callbacks never share the PyTorch GIL.""" + """把 YOLO 放进 spawn() 子进程，让 ROS 回调永远不用和 PyTorch 抢 GIL。""" def __init__(self, cfg: dict) -> None: self._cfg = dict(cfg) @@ -371,9 +371,9 @@ class YoloInferProcess: def _start(self) -> None: ctx = mp.get_context("spawn") - # maxsize=2 + worker drains to newest: parent never has to drop frames - # (Queue put_nowait/get_nowait races were the source of multi-second - # "silent" freezes with the worker alive). + # maxsize=2 + worker 排空到最新：父进程永远不需要主动丢帧 + # （之前 Queue put_nowait/get_nowait 的竞争，正是 worker 明明活着 + # 却出现数秒“静默”冻结的根源）。 self._q_in = ctx.Queue(maxsize=2) self._q_out = ctx.Queue(maxsize=4) self._p = ctx.Process( @@ -386,12 +386,12 @@ class YoloInferProcess: msg = self._q_out.get(timeout=90.0) if not msg or msg[0] != "ready": err = msg[1] if msg and len(msg) > 1 else "worker failed" - raise RuntimeError(f"YOLO worker: {err}") + raise RuntimeError(f"YOLO 工作进程：{err}") self.weights_path = str(msg[1]) self.imgsz = int(self._cfg.get("imgsz") or 416) def restart(self) -> None: - """Kill a hung/dead worker and spawn a fresh one (engine reload ~2-5 s).""" + """杀掉卡死 / 挂掉的 worker，再起一个新的（重新加载 engine 约 2-5 s）。""" self.close() self._start() @@ -400,19 +400,19 @@ class YoloInferProcess: return bool(self._p is not None and self._p.is_alive()) def submit(self, image_bgr: np.ndarray, seq: int = 0) -> None: - """Queue the newest frame. If the worker is busy, skip — it drains to - the newest queued frame after every infer, so nothing stale runs.""" + """把最新一帧排进队列。如果 worker 正忙就跳过 —— 它在每次推理后都会 + 排空到队列里最新的帧，所以不会跑任何过期数据。""" if not self.alive: - raise RuntimeError("YOLO worker died") + raise RuntimeError("YOLO 工作进程已死亡") try: self._q_in.put((int(seq), image_bgr), timeout=0.010) except Exception: pass def wait_result(self, timeout: float = 0.045): - """Return ``(seq, list[Detection], gpu_ms)`` or ``None``.""" + """返回 ``(seq, list[Detection], gpu_ms)`` 或 ``None``。""" if not self.alive: - raise RuntimeError("YOLO worker died") + raise RuntimeError("YOLO 工作进程已死亡") deadline = time.monotonic() + max(0.004, float(timeout)) latest = None while time.monotonic() < deadline: @@ -456,7 +456,7 @@ class YoloInferProcess: return seq, dets, gpu_ms def infer(self, image_bgr: np.ndarray, *, half: bool = False, seq: int = 0): - """Compat: submit + wait. Prefer submit/wait_result for pipelining.""" + """兼容接口：提交 + 等待。要做流水线的话优先用 submit/wait_result。""" del half self.submit(image_bgr, seq) got = self.wait_result(timeout=0.08) @@ -476,7 +476,7 @@ class YoloInferProcess: self._p.terminate() self._p.join(timeout=1.0) if self._p.is_alive(): - self._p.kill() # stuck in a CUDA call — SIGKILL + self._p.kill() # 卡在 CUDA 调用里出不来 —— 直接 SIGKILL self._p.join(timeout=1.0) self._p = None @@ -507,16 +507,16 @@ def pick_best( min_conf: float = 0.12, min_area: float = 4000.0, ) -> Optional[Detection]: - """Keep the current workpiece under fast motion. + """让当前工件在快速运动下也能被持续跟踪。 - Match against a *predicted* previous box with a wide gate. If IoU fails - but YOLO still fired above min_conf, keep that detection — do not return - None (that was the main source of dropouts while moving). - Tiny boxes (logo / 通止规尖) are rejected so the arm does not lock on the mat. + 用一个较宽的匹配门限去和**预测出的**上一帧检测框做匹配。如果 IoU 不达标、 + 但 YOLO 仍以高于 min_conf 的置信度报出了目标，就保留该检测 —— 不要返回 + None（那正是运动过程中目标丢失的主要来源）。 + 过小的框（logo / 通止规尖）会被拒掉，避免机械臂锁到垫子上。 """ if not dets: return None - # Locked track may dip below min_conf while moving (blur). New locks cannot. + # 已锁定的跟踪在运动（模糊）时可能掉到 min_conf 以下；但新锁定不允许这样。 floor = max(0.06, 0.50 * float(min_conf)) usable = [d for d in dets if float(d.conf) >= floor and float(d.area) >= float(min_area)] if not usable: @@ -553,7 +553,7 @@ def pick_best( return s best = max(usable, key=score_new) - # Fast move: keep a live YOLO even if it jumped far from the last box. + # 快速移动：即使 YOLO 报出的框离上一帧很远，也保留这个实时检测。 need = max(float(min_conf), float(reacquire_conf)) if float(best.conf) >= need: return best diff --git a/src/arm_application/config/dual_grippers.yaml b/src/arm_application/config/dual_grippers.yaml index 502c0d0..0132c6f 100644 --- a/src/arm_application/config/dual_grippers.yaml +++ b/src/arm_application/config/dual_grippers.yaml @@ -1,5 +1,5 @@ -# Optional fallback for DualArmGripperManager(load_dual_arm_gripper_config(path)). -# Daily use reads ~/ros2/config/site.yaml via load_from_site(). +# DualArmGripperManager(load_dual_arm_gripper_config(path)) 的可选兜底配置。 +# 日常使用通过 load_from_site() 读取 ~/ros2/config/site.yaml。 left_arm: robot_sn: "Rizon4-063989" gripper_port: "/dev/ttyUSB0" diff --git a/src/arm_application/config/perception.yaml b/src/arm_application/config/perception.yaml index 7b9ab6d..6d79c9e 100644 --- a/src/arm_application/config/perception.yaml +++ b/src/arm_application/config/perception.yaml @@ -21,26 +21,26 @@ perception_node: min_conf: 0.12 vision_hz: 20.0 z_ref_m: 0.30 - # Large gate + reacquire: fast motion must NOT freeze track + # 大跳变门限 + 重新捕获：快速运动绝不能把跟踪卡死 jump_px: 280.0 jump_z_m: 0.20 min_depth_m: 0.05 max_depth_m: 2.50 object_frame: "tracked_object" camera_frame: "d405_color_optical_frame" - # Snappy KF + blend toward raw measurement + # 灵敏的 KF + 向原始观测融合 kf_q_pos: 0.002 kf_q_vel: 0.05 kf_r_pos: 0.0008 meas_blend: 0.65 - # Faster infer + tight box follow + # 更快的推理 + 更紧的框跟随 infer_max_width: 640 box_ema_alpha: 0.85 miss_clear_frames: 2 - # Item 4 — adaptive Z depth LPF + # 第 4 项 — 自适应 Z 深度低通滤波 z_alpha_min: 0.10 z_alpha_max: 0.60 z_alpha_gain: 2.0 - feature_mode: "plane" # stereo 3D centroid; box = monocular box-center ray + feature_mode: "plane" # stereo 为 3D 质心；box = 单目框中心的射线 plane_inlier_m: 0.008 plane_band_m: 0.06 diff --git a/src/arm_application/config/servo.yaml b/src/arm_application/config/servo.yaml index 42a5930..62dd669 100644 --- a/src/arm_application/config/servo.yaml +++ b/src/arm_application/config/servo.yaml @@ -1,5 +1,5 @@ -# servo_node defaults. Launch overlays ~/ros2/config/site.yaml → servo: -# Do not edit this file for day-to-day tuning. +# servo_node 的默认值。launch 会用 ~/ros2/config/site.yaml 的 servo: 覆盖。 +# 日常调参不要改这个文件。 servo_node: ros__parameters: @@ -19,7 +19,7 @@ servo_node: enable_control: false max_extrapolate_sec: 0.08 - # Item 1 — lost-target hard stop + # 第 1 项 — 丢失目标硬停 watchdog_timeout_sec: 0.3 lost_timeout_sec: 0.3 @@ -31,7 +31,7 @@ servo_node: kd_z: 0.45 i_limit_xy: 0.03 i_limit_z: 0.025 - # Item 2 — integral separation + # 第 2 项 — 积分分离 i_sep_pos_m: 0.005 i_sep_ang_rad: 0.02 @@ -41,11 +41,11 @@ servo_node: v_max_z: 0.07 a_max: 0.70 - # Item 3 — velocity LPF + # 第 3 项 — 速度低通滤波 vel_lpf_alpha: 0.40 wz_lpf_alpha: 0.40 - # Item 5 — dq sliding window + # 第 5 项 — dq 滑动窗口 dq_window_size: 5 min_ee_z_m: 0.20 diff --git a/src/arm_application/launch/dh_pgea.launch.py b/src/arm_application/launch/dh_pgea.launch.py index e58b2ef..5f9203f 100644 --- a/src/arm_application/launch/dh_pgea.launch.py +++ b/src/arm_application/launch/dh_pgea.launch.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Launch DH PGEA Modbus node. Config from ~/ros2/config/site.yaml gripper:.""" +"""启动 DH PGEA Modbus 节点。配置来自 ``~/ros2/config/site.yaml`` 的 ``gripper`` 段。""" from __future__ import annotations @@ -44,8 +44,8 @@ def _setup(context, *args, **kwargs): ), LogInfo( msg=( - "[dh_pgea] Flexiv load_gripper stays false — this is DH Modbus, " - "not a Flexiv Studio gripper." + "[dh_pgea] Flexiv 的 load_gripper 保持 false —— 这里是 DH Modbus，" + "不是 Flexiv Studio 夹爪。" ) ), Node( diff --git a/src/arm_application/launch/dual_gripper.launch.py b/src/arm_application/launch/dual_gripper.launch.py index 4d8ecdf..59571f4 100644 --- a/src/arm_application/launch/dual_gripper.launch.py +++ b/src/arm_application/launch/dual_gripper.launch.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Launch dual DH grippers. Ports come from site.yaml robots[].gripper.""" +"""启动双路 DH 夹爪。端口来自 ``site.yaml`` 的 ``robots[].gripper``。""" from __future__ import annotations @@ -20,7 +20,7 @@ def generate_launch_description() -> LaunchDescription: [ DeclareLaunchArgument("sides", default_value="left,right"), DeclareLaunchArgument("allow_offline", default_value="true"), - LogInfo(msg=f"[dual_gripper] site={site} (arm_msgs, IPC Modbus, not Flexiv gripper)"), + LogInfo(msg=f"[dual_gripper] site={site}（arm_msgs，IPC Modbus，不是 Flexiv 夹爪）"), Node( package="arm_application", executable="dual_gripper", diff --git a/src/arm_application/launch/hole_detect.launch.py b/src/arm_application/launch/hole_detect.launch.py index 9922f2a..8a72a78 100644 --- a/src/arm_application/launch/hole_detect.launch.py +++ b/src/arm_application/launch/hole_detect.launch.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Launch hole_detect on the current-arm color topic.""" +"""在当前机械臂的彩色话题上启动 hole_detect。""" from __future__ import annotations diff --git a/src/arm_application/launch/visual_servo.launch.py b/src/arm_application/launch/visual_servo.launch.py index 0d4fb91..4380a3e 100644 --- a/src/arm_application/launch/visual_servo.launch.py +++ b/src/arm_application/launch/visual_servo.launch.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 -"""Launch: perception_node + servo_node + static hand-eye TF. +"""启动：perception_node + servo_node + 静态手眼 TF。 -All tunables come from ~/ros2/config/site.yaml (override via launch args if needed). +所有可调参数都来自 ``~/ros2/config/site.yaml``（必要时可用 launch 参数覆盖）。 """ from __future__ import annotations @@ -20,12 +20,12 @@ from launch_ros.actions import Node def _ws_root() -> Path: - # Prefer env from env.sh / load_site_config + # 优先使用 env.sh / load_site_config 提供的环境变量 for key in ("ROS2_WS", "SITE_ROS2_ROOT"): v = os.environ.get(key, "").strip() if v: return Path(v) - # launch file lives in install/.../share/arm_application/launch or src/... + # launch 文件可能位于 install/.../share/arm_application/launch 或 src/... here = Path(__file__).resolve() for p in [here.parents[i] for i in range(2, 8)]: if (p / "config" / "site.yaml").is_file(): @@ -46,7 +46,7 @@ def _load_site(): cfg = os.environ.get("SITE_CONFIG", "").strip() path = Path(cfg).expanduser() if cfg else site_path() - # site_path() uses script location; force workspace root file + # site_path() 依赖脚本所在位置；这里强制使用工作区根目录下的文件 if not path.is_file(): path = root / "config" / "site.yaml" data = load_site(path) @@ -97,7 +97,7 @@ def _launch_setup(context, *args, **kwargs): share = Path(get_package_share_directory("arm_application")) site, site_path, perc_over, servo_over, robot = _load_site() - # Launch args override site when explicitly passed (non-empty / non-default handled below) + # launch 参数显式传入时覆盖 site 配置（空值 / 默认值的处理见下） handeye = LaunchConfiguration("handeye_file").perform(context).strip() site_he = str(servo_over.get("handeye_file") or (site.get("paths") or {}).get("handeye_file") or "") if not handeye or handeye in ( @@ -107,8 +107,8 @@ def _launch_setup(context, *args, **kwargs): handeye = site_he or handeye if not Path(handeye).is_file(): raise FileNotFoundError( - f"hand-eye result missing: {handeye}. " - "Re-run: bash ~/ros2/scripts/run_handeye.sh calibrate" + f"手眼标定结果缺失：{handeye}。" + "请重新运行：bash ~/ros2/scripts/run_handeye.sh calibrate" ) enable = LaunchConfiguration("enable_control").perform(context).lower() in ( @@ -197,8 +197,8 @@ def _launch_setup(context, *args, **kwargs): ), LogInfo(msg=f"[site] YOLO weights: {weights}"), LogInfo( - msg="nodes: perception_node (YOLO) | servo_node (100Hz) — " - "separate processes, topic /visual_servo/filtered_point" + msg="节点：perception_node（YOLO）| servo_node（100Hz）—— " + "各自独立进程，话题 /visual_servo/filtered_point" ), ] pub_tf = LaunchConfiguration("publish_handeye_tf").perform(context).lower() in ( @@ -206,7 +206,7 @@ def _launch_setup(context, *args, **kwargs): ) if pub_tf: nodes.append( - LogInfo(msg=f"static hand-eye {ee_frame} → {cam_frame} t=[{tx:.3f},{ty:.3f},{tz:.3f}]") + LogInfo(msg=f"静态手眼 {ee_frame} → {cam_frame} t=[{tx:.3f},{ty:.3f},{tz:.3f}]") ) nodes.append(static_tf) nodes.extend([perception, servo]) diff --git a/src/arm_application/setup.py b/src/arm_application/setup.py index 8b2dfc6..e15c5ef 100644 --- a/src/arm_application/setup.py +++ b/src/arm_application/setup.py @@ -55,28 +55,28 @@ setup( 'move_to_pose_client = arm_application.move_to_pose_client:main', 'run_joint_trajectory = arm_application.run_joint_trajectory:main', 'goto_ee_plane = arm_application.goto_ee_plane:main', - # Camera + # 相机 'realsense_color_pub = arm_application.realsense_color_pub:main', 'realsense_view = arm_application.realsense_view:main', - # Hand-eye + # 手眼标定 'handeye_teach = arm_application.handeye_teach:main', 'handeye_waypoints = arm_application.handeye_waypoints:main', 'handeye_calibrate = arm_application.handeye_calibrate:main', - # Visual servo (current stack) + # 视觉伺服（当前主链路） 'perception_node = arm_application.perception_node:main', 'servo_node = arm_application.servo_node:main', - # Gripper + # 夹爪 'dh_pgea = arm_application.dh_pgea_node:main', 'dh_pgea_cli = arm_application.dh_pgea:main', 'dual_gripper = arm_application.dual_gripper_node:main', 'dual_gripper_cli = arm_application.dual_gripper_cli:main', - # Hole detect (no insert motion) + # 入孔检测（只检测，不做插入动作） 'hole_detect = arm_application.hole_detect_node:main', - # Stage-2 one-shot hole localization (k1/k2) + gauge alignment + # 第二阶段：一次性孔定位（k1/k2）+ 对规 'stage2_locate = arm_application.stage2_locate_node:main', - # Stage-3 compliant hole search (Orisys tactile spiral + insert) + # 第三阶段：柔顺找孔（Orisys 触觉螺旋 + 插入） 'stage3_search = arm_application.stage3_search_node:main', - # Three-stage assembly pipeline (track → locate → search, one process) + # 三段式装配流水线（跟踪 → 定位 → 找孔，单进程） 'assembly_pipeline = arm_application.pipeline_node:main', 'tactile = arm_application.tactile_node:main', 'tactile_cli = arm_application.tactile_cli:main', diff --git a/src/arm_bringup/config/bringup.yaml b/src/arm_bringup/config/bringup.yaml index 25356ed..3bbe47a 100644 --- a/src/arm_bringup/config/bringup.yaml +++ b/src/arm_bringup/config/bringup.yaml @@ -1,12 +1,12 @@ -# Site defaults for Flexiv Rizon 4 (SN: Rizon4-063989) +# Flexiv Rizon 4 的站点默认值（SN: Rizon4-063989） robot_sn: Rizon4-063989 rizon_type: Rizon4 -# Flexiv Studio gripper (GN01) — keep false. DH PGEA uses run_gripper.sh. +# Flexiv Studio 夹爪（GN01）—— 保持 false。DH PGEA 使用 run_gripper.sh。 load_gripper: false use_fake_hardware: false start_rviz: true -# Official Flexiv joint naming: Rizon4-063989_joint1 ... joint7 +# Flexiv 官方关节命名：Rizon4-063989_joint1 ... joint7 joint_prefix: Rizon4-063989_ base_frame: base_link diff --git a/src/arm_bringup/launch/real.launch.py b/src/arm_bringup/launch/real.launch.py index c0bb9ad..ba82e06 100644 --- a/src/arm_bringup/launch/real.launch.py +++ b/src/arm_bringup/launch/real.launch.py @@ -7,8 +7,8 @@ from ament_index_python.packages import get_package_share_directory, PackageNotF import os -# Site robot — DH PGEA is a separate Modbus node (run_gripper.sh). -# Keep Flexiv load_gripper false (that flag is Flexiv-GN01 in Studio). +# 现场机械臂 —— DH PGEA 是独立的 Modbus 节点（run_gripper.sh）。 +# 保持 Flexiv 的 load_gripper 为 false（该开关在 Studio 里对应 Flexiv-GN01）。 SITE_ROBOT_SN = os.environ.get('SITE_ROBOT_SN', 'Rizon4-063989') @@ -21,7 +21,7 @@ def _launch_setup(context, *args, **kwargs): if not robot_sn: return [ - LogInfo(msg=f'[arm_bringup] ERROR: robot_sn is required, e.g. robot_sn:={SITE_ROBOT_SN}') + LogInfo(msg=f'[arm_bringup] 错误：必须提供 robot_sn，例如 robot_sn:={SITE_ROBOT_SN}') ] try: @@ -30,8 +30,8 @@ def _launch_setup(context, *args, **kwargs): return [ LogInfo( msg=( - '[arm_bringup] flexiv_bringup not found. ' - 'Run scripts/fetch_flexiv.sh and build, or use sim.launch.py for mock.' + '[arm_bringup] 找不到 flexiv_bringup。' + '请运行 scripts/fetch_flexiv.sh 并编译，或改用 sim.launch.py 跑 mock。' ) ) ] @@ -39,7 +39,7 @@ def _launch_setup(context, *args, **kwargs): return [ LogInfo( msg=( - f'[arm_bringup] Starting Flexiv {rizon_type} ({robot_sn}), ' + f'[arm_bringup] 启动 Flexiv {rizon_type}（{robot_sn}），' f'load_gripper={load_gripper}' ) ), @@ -61,16 +61,16 @@ def _launch_setup(context, *args, **kwargs): def generate_launch_description(): - """Real Rizon4 bringup via official flexiv_bringup (RDK v1 / humble-v1).""" + """通过官方 flexiv_bringup 启动真机 Rizon4（RDK v1 / humble-v1）。""" return LaunchDescription([ DeclareLaunchArgument( 'robot_sn', default_value=SITE_ROBOT_SN, - description='Site robot serial number (no spaces)', + description='现场机械臂序列号（不能有空格）', ), DeclareLaunchArgument('rizon_type', default_value='Rizon4'), DeclareLaunchArgument('use_fake_hardware', default_value='false'), - # Flexiv-GN01 only. DH PGEA: bash ~/ros2/scripts/run_gripper.sh + # 仅适用于 Flexiv-GN01。DH PGEA 用: bash ~/ros2/scripts/run_gripper.sh DeclareLaunchArgument('load_gripper', default_value='false'), DeclareLaunchArgument('start_rviz', default_value='true'), OpaqueFunction(function=_launch_setup), diff --git a/src/arm_bringup/launch/sim.launch.py b/src/arm_bringup/launch/sim.launch.py index decb36d..ab82d1a 100644 --- a/src/arm_bringup/launch/sim.launch.py +++ b/src/arm_bringup/launch/sim.launch.py @@ -7,7 +7,7 @@ from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - """Offline mock bringup for Flexiv Rizon 4 (7-DOF), no real robot required.""" + """Flexiv Rizon 4（7 自由度）的离线 mock 启动，无需真机。""" use_rviz = LaunchConfiguration('use_rviz') use_moveit = LaunchConfiguration('use_moveit') robot_sn = LaunchConfiguration('robot_sn') @@ -39,7 +39,7 @@ def generate_launch_description(): DeclareLaunchArgument( 'robot_sn', default_value='Rizon4-063989', - description='Site SN; joints become Rizon4-063989_joint1..joint7', + description='现场 SN；关节名会变成 Rizon4-063989_joint1..joint7', ), mock, moveit, diff --git a/src/arm_bringup/launch/stack.launch.py b/src/arm_bringup/launch/stack.launch.py index 55669e0..278df13 100644 --- a/src/arm_bringup/launch/stack.launch.py +++ b/src/arm_bringup/launch/stack.launch.py @@ -1,6 +1,6 @@ -"""Unified stack launch: backend:=mock|moveit|real|isaac +"""统一的功能栈启动：backend:=mock|moveit|real|isaac -Application interfaces stay the same across backends: +各后端下应用接口保持不变： /set_named_pose, /get_arm_state, /arm_status, /follow_joint_trajectory """ @@ -82,9 +82,9 @@ def _setup(context, *args, **kwargs): actions.append( LogInfo( msg=( - '[arm_bringup] MoveIt on top of mock. ' - 'Named poses still via /set_named_pose; ' - 'MoveIt plans execute through trajectory_bridge.' + '[arm_bringup] 在 mock 之上叠加 MoveIt。' + '命名位姿仍走 /set_named_pose；' + 'MoveIt 的规划通过 trajectory_bridge 执行。' ) ) ) @@ -96,9 +96,9 @@ def _setup(context, *args, **kwargs): actions.append( LogInfo( msg=( - '[arm_bringup] flexiv_bringup missing. ' - 'Run: ./scripts/fetch_flexiv.sh && rebuild. ' - 'Falling back tip: use backend:=mock for now.' + '[arm_bringup] 缺少 flexiv_bringup。' + '请运行: ./scripts/fetch_flexiv.sh && 重新编译。' + '临时退回建议：先用 backend:=mock。' ) ) ) @@ -119,7 +119,7 @@ def _setup(context, *args, **kwargs): }.items(), ) ) - # Keep application services on top of real HW when possible + # 尽量把应用服务叠在真机之上 actions.append( Node( package='arm_control', @@ -167,9 +167,9 @@ def _setup(context, *args, **kwargs): actions.append( LogInfo( msg=( - '[arm_bringup] REAL mode: flexiv_bringup + named_pose_server ' - '+ arm_status_node. ' - 'Ensure RDK is enabled and network reaches the robot.' + '[arm_bringup] REAL 模式：flexiv_bringup + named_pose_server ' + '+ arm_status_node。' + '请确保 RDK 已使能，且网络能连到机械臂。' ) ) ) @@ -178,17 +178,17 @@ def _setup(context, *args, **kwargs): actions.append( LogInfo( msg=( - '[arm_bringup] ISAAC mode: mock ROS stack is up for bridge testing. ' - 'Start Isaac Sim + flexiv_sim_plugin separately ' - '(see docs/INTERFACES.md). ' - 'Sync joints via /joint_states or RDK bridge topics.' + '[arm_bringup] ISAAC 模式：已起 mock ROS 栈用于桥接测试。' + '请另行启动 Isaac Sim + flexiv_sim_plugin ' + '（见 docs/INTERFACES.md）。' + '通过 /joint_states 或 RDK 桥接话题同步关节。' ) ) ) if backend not in ('mock', 'moveit', 'real', 'isaac'): actions.append( - LogInfo(msg=f'[arm_bringup] Unknown backend "{backend}". Use mock|moveit|real|isaac') + LogInfo(msg=f'[arm_bringup] 未知 backend "{backend}"。可用 mock|moveit|real|isaac') ) return actions diff --git a/src/arm_control/arm_control/__init__.py b/src/arm_control/arm_control/__init__.py index cd2be83..e7f3b24 100644 --- a/src/arm_control/arm_control/__init__.py +++ b/src/arm_control/arm_control/__init__.py @@ -1 +1 @@ -"""Package init for arm_control.""" +"""arm_control 的包初始化。""" diff --git a/src/arm_control/arm_control/arm_status_node.py b/src/arm_control/arm_control/arm_status_node.py index 0610d09..3d5a3a3 100644 --- a/src/arm_control/arm_control/arm_status_node.py +++ b/src/arm_control/arm_control/arm_status_node.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Publish ArmStatus + GetArmState from joint_states, TF, and Flexiv tcp_pose.""" +"""根据 joint_states、TF 与 Flexiv 的 tcp_pose 发布 ArmStatus，并提供 GetArmState 服务。""" from __future__ import annotations @@ -43,7 +43,7 @@ class ArmStatusNode(Node): self.create_timer(1.0 / rate, self._tick) self.get_logger().info( f'ArmStatus: base={self.base_frame} ee={self.ee_frame} ' - f'tcp_topic={self.tcp_pose_topic or "(none)"}' + f'tcp_topic={self.tcp_pose_topic or "（无）"}' ) def _on_js(self, msg: JointState) -> None: @@ -55,7 +55,7 @@ class ArmStatusNode(Node): self._last_tcp = msg def _ee(self) -> Optional[PoseStamped]: - # Prefer Flexiv-published TCP pose (reliable on real stack) + # 优先用 Flexiv 发布的 TCP 位姿（真机栈上更可靠） if self._last_tcp is not None: return self._last_tcp try: @@ -101,7 +101,7 @@ class ArmStatusNode(Node): and abs(s.ee_pose.pose.position.y) < 1e-9 and abs(s.ee_pose.pose.position.z) < 1e-9 ): - response.message = 'ok (ee pose unavailable — check TF/tcp_pose)' + response.message = 'ok（末端位姿不可用 —— 请检查 TF/tcp_pose）' else: response.message = 'ok' return response diff --git a/src/arm_control/arm_control/named_pose_server.py b/src/arm_control/arm_control/named_pose_server.py index 14be1cc..91afd5b 100644 --- a/src/arm_control/arm_control/named_pose_server.py +++ b/src/arm_control/arm_control/named_pose_server.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Service server: SetNamedPose -> FollowJointTrajectory (backend-agnostic).""" +"""服务端：SetNamedPose -> FollowJointTrajectory（与底层后端无关）。""" from __future__ import annotations @@ -65,18 +65,18 @@ class NamedPoseServer(Node): SetNamedPose, 'set_named_pose', self._on_set, callback_group=self._cb ) self.get_logger().info( - f'Named poses {list(self.poses)} joints={self.joint_names}' + f'命名位姿 {list(self.poses)} joints={self.joint_names}' ) def _on_set(self, request: SetNamedPose.Request, response: SetNamedPose.Response): name = request.pose_name if name not in self.poses: response.success = False - response.message = f'Unknown pose "{name}". Available: {list(self.poses)}' + response.message = f'未知位姿"{name}"。可用: {list(self.poses)}' return response if not self._client.wait_for_server(timeout_sec=3.0): response.success = False - response.message = 'follow_joint_trajectory action not available' + response.message = 'follow_joint_trajectory action 不可用' return response goal = FollowJointTrajectory.Goal() @@ -87,30 +87,30 @@ class NamedPoseServer(Node): goal.time_from_start = [duration] goal.velocity_scale = float(request.velocity_scale) - self.get_logger().info(f'-> pose [{name}] T={duration:.2f}s') + self.get_logger().info(f'-> 位姿 [{name}] T={duration:.2f}s') send_fut = self._client.send_goal_async(goal) if not _wait_future(send_fut, 5.0): response.success = False - response.message = 'Timed out waiting for goal accept' + response.message = '等待目标被接受超时' return response handle = send_fut.result() if handle is None or not handle.accepted: response.success = False - response.message = 'Goal rejected' + response.message = '目标被拒绝' return response res_fut = handle.get_result_async() if not _wait_future(res_fut, 60.0): response.success = False - response.message = 'Timed out waiting for trajectory result' + response.message = '等待轨迹结果超时' return response wrapped = res_fut.result() if wrapped is None: response.success = False - response.message = 'Empty result' + response.message = '结果为空' return response result = wrapped.result response.success = bool(result.success) - response.message = result.message or f'Moved to {name}' + response.message = result.message or f'已移动到位姿 {name}' return response diff --git a/src/arm_control/arm_control/real_trajectory_adapter.py b/src/arm_control/arm_control/real_trajectory_adapter.py index 7fa8a62..ebed816 100644 --- a/src/arm_control/arm_control/real_trajectory_adapter.py +++ b/src/arm_control/arm_control/real_trajectory_adapter.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Expose arm_msgs/FollowJointTrajectory for apps; drive flexiv JTC (control_msgs).""" +"""对应用侧暴露 arm_msgs/FollowJointTrajectory；实际驱动 Flexiv JTC（control_msgs）。""" from __future__ import annotations @@ -45,19 +45,19 @@ class RealTrajectoryAdapter(Node): cancel_callback=lambda _g: CancelResponse.ACCEPT, callback_group=self._cb, ) - self.get_logger().info(f'Real adapter {arm} -> {ctrl}') + self.get_logger().info(f'真机适配器 {arm} -> {ctrl}') def _execute(self, goal_handle): result = ArmFJT.Result() req = goal_handle.request if not req.joint_names or req.num_points < 1: result.success = False - result.message = 'Invalid goal' + result.message = '目标非法' goal_handle.abort() return result if not self._ctrl.wait_for_server(timeout_sec=5.0): result.success = False - result.message = f'Controller action not available' + result.message = f'控制器 action 不可用' goal_handle.abort() return result @@ -72,8 +72,8 @@ class RealTrajectoryAdapter(Node): pt = JointTrajectoryPoint() pt.positions = pos[idx : idx + n_j] idx += n_j - # Point-to-point named poses must end at rest — nonzero end vel - # + open_loop JTC makes the arm coast / shake after arrival. + # 点到点的命名位姿必须在静止状态下结束 —— 末速度非零 + # 再叠加开环 JTC，会让机械臂到位后滑行/抖动。 pt.velocities = [0.0] * n_j pt.accelerations = [0.0] * n_j sec = int(t) @@ -86,13 +86,13 @@ class RealTrajectoryAdapter(Node): send = self._ctrl.send_goal_async(ctrl_goal) if not _wait(send, 5.0): result.success = False - result.message = 'Timed out sending to controller' + result.message = '发送给控制器超时' goal_handle.abort() return result handle = send.result() if handle is None or not handle.accepted: result.success = False - result.message = 'Controller rejected goal' + result.message = '控制器拒绝了目标' goal_handle.abort() return result traj_duration = 0.0 @@ -102,13 +102,13 @@ class RealTrajectoryAdapter(Node): res_fut = handle.get_result_async() if not _wait(res_fut, max(120.0, traj_duration * 3.0)): result.success = False - result.message = 'Timed out waiting for controller' + result.message = '等待控制器超时' goal_handle.abort() return result ctrl_res = res_fut.result().result ok = ctrl_res.error_code == CtrlFJT.Result.SUCCESSFUL result.success = ok - result.message = 'OK' if ok else f'controller error_code={ctrl_res.error_code}' + result.message = 'OK' if ok else f'控制器 error_code={ctrl_res.error_code}' if ok: goal_handle.succeed() else: diff --git a/src/arm_control/arm_control/trajectory_bridge.py b/src/arm_control/arm_control/trajectory_bridge.py index 7fd4f15..bce90a5 100644 --- a/src/arm_control/arm_control/trajectory_bridge.py +++ b/src/arm_control/arm_control/trajectory_bridge.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Bridge control_msgs/FollowJointTrajectory -> arm_msgs/FollowJointTrajectory.""" +"""把 control_msgs/FollowJointTrajectory 桥接到 arm_msgs/FollowJointTrajectory。""" from __future__ import annotations @@ -44,7 +44,7 @@ class TrajectoryBridge(Node): cancel_callback=lambda _g: CancelResponse.ACCEPT, callback_group=self._cb, ) - self.get_logger().info(f'Bridge {ctrl} -> {arm}') + self.get_logger().info(f'桥接 {ctrl} -> {arm}') def _execute(self, goal_handle): result = CtrlFJT.Result() diff --git a/src/arm_control/config/controllers.yaml b/src/arm_control/config/controllers.yaml index 78f4086..f42793b 100644 --- a/src/arm_control/config/controllers.yaml +++ b/src/arm_control/config/controllers.yaml @@ -5,13 +5,13 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - # Name aligned with Flexiv official bringup + # 名字与 Flexiv 官方 bringup 对齐 rizon_arm_controller: type: joint_trajectory_controller/JointTrajectoryController rizon_arm_controller: ros__parameters: - # Site robot Rizon4-063989 (official flexiv_description keeps hyphen) + # 站点机器人 Rizon4-063989（官方 flexiv_description 保留连字符） joints: - Rizon4-063989_joint1 - Rizon4-063989_joint2 @@ -26,8 +26,8 @@ rizon_arm_controller: - position - velocity allow_partial_joints_goal: false - # Must be false: true interpolates from the last *command* (already at target - # after a "success" with no hardware motion) so the arm never moves. + # 必须为 false：若为 true，会从最后一次 *命令* 开始插值（在硬件没动 + # 却报"成功"之后，命令已经落在目标点上），于是机械臂永远不动。 open_loop_control: false constraints: stopped_velocity_tolerance: 0.05 diff --git a/src/arm_control/config/named_poses.yaml b/src/arm_control/config/named_poses.yaml index 51761d0..9c3ce1d 100644 --- a/src/arm_control/config/named_poses.yaml +++ b/src/arm_control/config/named_poses.yaml @@ -1,5 +1,5 @@ -# Named poses — site robot Rizon4-063989 (7-DOF), radians -# Joint base names match flexiv_description: joint1..joint7 +# 命名位姿 — 站点机器人 Rizon4-063989（7 自由度），弧度 +# 关节基础名与 flexiv_description 一致：joint1..joint7 named_pose_server: ros__parameters: joint_prefix: "Rizon4-063989_" diff --git a/src/arm_control/config/named_poses_91.yaml b/src/arm_control/config/named_poses_91.yaml index ccf5881..6a21a4d 100644 --- a/src/arm_control/config/named_poses_91.yaml +++ b/src/arm_control/config/named_poses_91.yaml @@ -1,5 +1,5 @@ -# Right arm 91 (Rizon4-063991) named poses (radians). -# Starting values match the generic home; teach cell-specific poses on 91. +# 右臂 91（Rizon4-063991）的命名位姿（弧度）。 +# 起始值与通用 home 一致；在 91 上示教该工位专用的位姿。 named_pose_server: ros__parameters: joint_prefix: "Rizon4-063991_" diff --git a/src/arm_control/launch/mock_control.launch.py b/src/arm_control/launch/mock_control.launch.py index 3d97f71..20a1ec6 100644 --- a/src/arm_control/launch/mock_control.launch.py +++ b/src/arm_control/launch/mock_control.launch.py @@ -8,7 +8,7 @@ from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue from launch_ros.substitutions import FindPackageShare -# Site robot. Official flexiv_description keeps the hyphen in the SN prefix. +# 现场机械臂。官方 flexiv_description 要求 SN 前缀里**保留连字符**。 SITE_ROBOT_SN = os.environ.get('SITE_ROBOT_SN', 'Rizon4-063989') SITE_JOINT_PREFIX = os.environ.get('SITE_JOINT_PREFIX', f'{SITE_ROBOT_SN}_') SITE_NAMED_POSES = os.environ.get('SITE_NAMED_POSES_FILE', '').strip() diff --git a/src/arm_description/urdf/arm.macro.xacro b/src/arm_description/urdf/arm.macro.xacro index b777c63..30ab029 100644 --- a/src/arm_description/urdf/arm.macro.xacro +++ b/src/arm_description/urdf/arm.macro.xacro @@ -2,11 +2,11 @@ @@ -41,7 +41,7 @@ - + @@ -65,7 +65,7 @@ - + @@ -155,7 +155,7 @@ - + @@ -177,7 +177,7 @@ - + @@ -192,7 +192,7 @@ - + diff --git a/src/arm_description/urdf/arm.ros2_control.xacro b/src/arm_description/urdf/arm.ros2_control.xacro index 4b29c2b..05431c6 100644 --- a/src/arm_description/urdf/arm.ros2_control.xacro +++ b/src/arm_description/urdf/arm.ros2_control.xacro @@ -8,7 +8,7 @@ mock_components/GenericSystem - + flexiv_hardware/FlexivHardwareInterface diff --git a/src/arm_interfaces/arm_interfaces/__init__.py b/src/arm_interfaces/arm_interfaces/__init__.py index 669d039..e5acf03 100644 --- a/src/arm_interfaces/arm_interfaces/__init__.py +++ b/src/arm_interfaces/arm_interfaces/__init__.py @@ -1,14 +1,14 @@ -"""Device contract layer for the robotic arm stack. +"""机械臂栈的设备契约层。 -Public entry points:: +公开入口:: from arm_interfaces import load_device_stack, check_requirement - stack = load_device_stack() # raises ContractError with a fix hint - check_requirement(stack, "handeye") # raises CapabilityError if unmet + stack = load_device_stack() # 失败时抛出带修复提示的 ContractError + check_requirement(stack, "handeye") # 能力不满足时抛出 CapabilityError -Everything device-selection related funnels through here so that the rest of the -framework never reads ``devices/`` or guesses at ``site.yaml`` structure itself. +所有与设备选型相关的事都汇集到这里，框架其余部分因此永远不必自己读 +``devices/``，也不必猜测 ``site.yaml`` 的结构。 """ from .capabilities import ( @@ -42,7 +42,7 @@ from .registry import Registry, list_available_brands, load_registry __version__ = "0.1.0" __all__ = [ - # loading + # 加载 "load_device_stack", "load_registry", "load_manifest", @@ -54,18 +54,18 @@ __all__ = [ "effective_capabilities", "list_available_brands", "manifest_path", - # types + # 类型 "DeviceStack", "Manifest", "Registry", - # capabilities + # 能力 "ALL_CAPABILITIES", "CAPABILITIES_BY_KIND", "REQUIREMENTS", "ROS_INTERFACE", "capabilities_for_kind", "describe", - # errors + # 错误 "ContractError", "RegistryError", "ManifestError", diff --git a/src/arm_interfaces/arm_interfaces/capabilities.py b/src/arm_interfaces/arm_interfaces/capabilities.py index 60201d9..7754290 100644 --- a/src/arm_interfaces/arm_interfaces/capabilities.py +++ b/src/arm_interfaces/arm_interfaces/capabilities.py @@ -1,23 +1,19 @@ -"""Capability vocabulary and the requirement sets upper layers depend on. +"""能力词表，以及上层依赖的能力需求集。 -Two distinct ideas live here, and keeping them separate matters: +这里有彼此不同的两个概念，把它们分开很重要： ``Capability`` - What a *driver* can do. Declared by each brand's ``manifest.yaml``. Adding a - new brand never changes this file — only edit it when there is genuinely a - new kind of motion or sensing to express. + *驱动*能做什么。由每个品牌的 ``manifest.yaml`` 声明。新增品牌从不改动 + 本文件——只有当确实要表达一种新的运动或感知方式时才编辑它。 ``Requirement`` - What an *upper layer* needs. A flow (tracking, hand-eye calibration, the - through-gauge assembly state machine) declares the set of capabilities it - will call. The contract layer checks the selected drivers satisfy it at - startup, so a missing capability fails immediately with a clear message - instead of failing mid-motion. + *上层*需要什么。一个流程（视觉跟踪、手眼标定、通止规装配状态机）声明 + 它将要调用的能力集合。契约层在启动时检查所选驱动是否满足它，因此缺失 + 能力会立刻报出清晰的消息，而不是在运动中途才失败。 -The ``ROS_INTERFACE`` table maps each capability to the concrete ROS 2 endpoint -that implements it. It exists so error messages can name the interface an -operator should check, and so future drivers know exactly what to publish -or serve. +``ROS_INTERFACE`` 表把每项能力映射到实现它的具体 ROS 2 端点。它的存在， +一是让错误消息能点名操作员该检查哪个接口，二是让未来的驱动清楚自己该 +发布或提供什么。 """ from __future__ import annotations @@ -26,17 +22,17 @@ from typing import Dict, FrozenSet, NamedTuple, Tuple class CapabilityInfo(NamedTuple): - """Documentation for one capability.""" + """对某一项能力的说明。""" owner: str - """Which device kind provides it: robot | gripper | sensor | camera.""" + """由哪类设备提供：robot | gripper | sensor | camera。""" summary: str - """One-line description of what the capability does.""" + """一句话说明该能力做什么。""" # --------------------------------------------------------------------------- -# Robot capabilities +# 机器人能力 # --------------------------------------------------------------------------- ROBOT_CAPABILITIES: Dict[str, CapabilityInfo] = { "get_arm_state": CapabilityInfo( @@ -72,7 +68,7 @@ ROBOT_CAPABILITIES: Dict[str, CapabilityInfo] = { } # --------------------------------------------------------------------------- -# Gripper capabilities +# 夹爪能力 # --------------------------------------------------------------------------- GRIPPER_CAPABILITIES: Dict[str, CapabilityInfo] = { "open": CapabilityInfo("gripper", "张开。"), @@ -85,7 +81,7 @@ GRIPPER_CAPABILITIES: Dict[str, CapabilityInfo] = { } # --------------------------------------------------------------------------- -# Sensor capabilities +# 传感器能力 # --------------------------------------------------------------------------- SENSOR_CAPABILITIES: Dict[str, CapabilityInfo] = { "read_tangential": CapabilityInfo( @@ -100,7 +96,7 @@ SENSOR_CAPABILITIES: Dict[str, CapabilityInfo] = { } # --------------------------------------------------------------------------- -# Camera capabilities +# 相机能力 # --------------------------------------------------------------------------- CAMERA_CAPABILITIES: Dict[str, CapabilityInfo] = { "color": CapabilityInfo("camera", "彩色图。推理与跟踪依赖它。"), @@ -116,7 +112,7 @@ CAMERA_CAPABILITIES: Dict[str, CapabilityInfo] = { "set_exposure": CapabilityInfo("camera", "设置曝光。跟踪需要限制曝光避免运动拖影。"), } -#: Every capability name known to the framework, tagged with its device kind. +#: 框架已知的全部能力名，并标注其设备类别。 ALL_CAPABILITIES: Dict[str, CapabilityInfo] = { **ROBOT_CAPABILITIES, **GRIPPER_CAPABILITIES, @@ -124,7 +120,7 @@ ALL_CAPABILITIES: Dict[str, CapabilityInfo] = { **CAMERA_CAPABILITIES, } -#: Device kind -> the capability names that kind is allowed to declare. +#: 设备类别 -> 该类设备允许声明的能力名。 CAPABILITIES_BY_KIND: Dict[str, FrozenSet[str]] = { "robot": frozenset(ROBOT_CAPABILITIES), "gripper": frozenset(GRIPPER_CAPABILITIES), @@ -134,10 +130,10 @@ CAPABILITIES_BY_KIND: Dict[str, FrozenSet[str]] = { # --------------------------------------------------------------------------- -# ROS 2 interface mapping +# ROS 2 接口映射 # --------------------------------------------------------------------------- class RosInterface(NamedTuple): - """The concrete ROS 2 endpoint implementing a capability.""" + """实现某项能力的具体 ROS 2 端点。""" namespace: str name: str @@ -145,10 +141,10 @@ class RosInterface(NamedTuple): type: str -#: capability -> ROS 2 endpoint. Names are relative to the device namespace and -#: are resolved per-instance (two arms never share a namespace). +#: capability -> ROS 2 端点。名称相对于设备命名空间，且按实例解析 +#:（两条机械臂绝不共用命名空间）。 ROS_INTERFACE: Dict[str, RosInterface] = { - # --- robot --- + # --- 机器人 --- "get_arm_state": RosInterface("", "get_arm_state", "service", "arm_msgs/srv/GetArmState"), "follow_joint_trajectory": RosInterface( "", "follow_joint_trajectory", "action", "arm_msgs/action/FollowJointTrajectory" @@ -164,19 +160,19 @@ ROS_INTERFACE: Dict[str, RosInterface] = { "enable": RosInterface("", "enable", "service", "std_srvs/srv/SetBool"), "clear_fault": RosInterface("", "clear_fault", "service", "std_srvs/srv/Trigger"), "soft_drag": RosInterface("", "soft_drag", "service", "std_srvs/srv/SetBool"), - # --- gripper --- + # --- 夹爪 --- "open": RosInterface("", "open", "service", "arm_msgs/srv/GripperExecute"), "close": RosInterface("", "close", "service", "arm_msgs/srv/GripperExecute"), "move": RosInterface("", "move", "service", "arm_msgs/srv/GripperExecute"), "rotate": RosInterface("", "rotate", "service", "arm_msgs/srv/GripperExecute"), "get_status": RosInterface("", "status", "topic", "arm_msgs/msg/GripperStatus"), - # --- sensor --- + # --- 传感器 --- "read_tangential": RosInterface("", "tangential", "topic", "sensor_msgs/msg/Float32MultiArray"), "read_normal": RosInterface("", "normal", "topic", "sensor_msgs/msg/Float32"), "read_depth": RosInterface("", "depth", "topic", "sensor_msgs/msg/Image"), "read_force_vector": RosInterface("", "force_vector", "topic", "sensor_msgs/msg/Image"), "measure_baseline": RosInterface("", "measure_baseline", "service", "std_srvs/srv/Trigger"), - # --- camera --- + # --- 相机 --- "color": RosInterface("", "color/image_raw", "topic", "sensor_msgs/msg/Image"), "depth": RosInterface("", "depth/image_raw", "topic", "sensor_msgs/msg/Image"), "aligned_depth_to_color": RosInterface( @@ -192,10 +188,10 @@ ROS_INTERFACE: Dict[str, RosInterface] = { # --------------------------------------------------------------------------- -# Requirement sets +# 需求集 # --------------------------------------------------------------------------- class Requirement(NamedTuple): - """A named flow and the capabilities it needs, grouped by device kind.""" + """一个具名流程及其所需能力，按设备类别分组。""" summary: str robot: Tuple[str, ...] @@ -261,12 +257,12 @@ REQUIREMENTS: Dict[str, Requirement] = { def capabilities_for_kind(kind: str) -> FrozenSet[str]: - """Return the set of capability names valid for a device kind.""" + """返回某类设备合法的能力名集合。""" return CAPABILITIES_BY_KIND.get(kind, frozenset()) def describe(capability: str) -> str: - """Human-readable description used in error messages.""" + """用于错误消息的、给人看的说明。""" info = ALL_CAPABILITIES.get(capability) if info is None: return f"{capability}（框架未登记的能力名）" diff --git a/src/arm_interfaces/arm_interfaces/cli.py b/src/arm_interfaces/arm_interfaces/cli.py index 08ae929..6746f04 100644 --- a/src/arm_interfaces/arm_interfaces/cli.py +++ b/src/arm_interfaces/arm_interfaces/cli.py @@ -1,19 +1,19 @@ -"""``device_check`` — validate the device stack from the command line. +"""``device_check`` —— 从命令行校验设备栈。 -Usage:: +用法:: - ros2 run arm_interfaces device_check # load everything, print summary + ros2 run arm_interfaces device_check # 全量加载并打印摘要 ros2 run arm_interfaces device_check --requirement handeye ros2 run arm_interfaces device_check --json ros2 run arm_interfaces device_check --list-brands - ros2 run arm_interfaces device_check --no-assets # skip vendor asset checks + ros2 run arm_interfaces device_check --no-assets # 跳过厂商物料检查 -Exit codes are meaningful so scripts can branch on them: +退出码有明确含义，脚本可据此分支： === ========================================================== -0 contract satisfied -2 contract violation (missing brand, config, asset, capability) -3 contract layer itself broke (unexpected exception) +0 契约满足 +2 契约违规（缺少品牌、配置、物料或能力） +3 契约层自身出错（意料之外的异常） === ========================================================== """ @@ -119,7 +119,7 @@ def main(argv: Optional[List[str]] = None) -> int: print(format_error(err), file=sys.stderr) return EXIT_CONTRACT - except Exception as exc: # noqa: BLE001 - the CLI is the last line of defence + except Exception as exc: # noqa: BLE001 - CLI 是最后一道防线 print( f"契约层内部错误（这本身是框架 bug，请上报）: {type(exc).__name__}: {exc}", file=sys.stderr, diff --git a/src/arm_interfaces/arm_interfaces/config_guard.py b/src/arm_interfaces/arm_interfaces/config_guard.py index a79b252..70bbc30 100644 --- a/src/arm_interfaces/arm_interfaces/config_guard.py +++ b/src/arm_interfaces/arm_interfaces/config_guard.py @@ -1,20 +1,19 @@ -"""Validate ``config/site.yaml`` against a manifest's declared ``config_schema``. +"""按清单声明的 ``config_schema`` 校验 ``config/site.yaml``。 -Two classes of bug motivated this module: +有两类 bug 促成了本模块： -1. **Silent no-ops.** ``run_handeye.sh`` passed five parameters - (``settle_qd_max``, ``pin_holds``, ...) to a node that never declared them. - YAML accepted the values, nothing read them, tuning had no effect. -2. **Unit confusion.** A value typed in millimetres but interpreted as metres - produces a plausible-looking but wrong motion. +1. **静默失效。** ``run_handeye.sh`` 把五个参数 + （``settle_qd_max``、``pin_holds``……）传给了一个从未声明它们的节点。 + YAML 接受了这些值，却没人读它们，调参毫无效果。 +2. **单位混淆。** 按毫米输入却被当成米解释的值，会产生看似合理但错误的 + 运动。 -The guard addresses both: every declared field is checked for presence and type, -and ``unit`` is echoed in error messages so the operator sees the expected unit -next to the value they wrote. +配置守卫同时解决这两点：每个已声明的字段都会检查是否存在、类型是否正确， +并且 ``unit`` 会回显在错误消息里，让操作员在写下的值旁边就能看到期望的单位。 -``site_path`` supports a list-expansion token so per-instance config can be -declared once: ``robot.robots[].ip`` resolves to ``robot.robots[0].ip`` and -``robot.robots[1].ip`` and requires *every* instance to supply the value. +``site_path`` 支持列表展开记号，使按实例的配置只需声明一次： +``robot.robots[].ip`` 会解析为 ``robot.robots[0].ip`` 和 +``robot.robots[1].ip``，并要求*每一个*实例都提供该值。 """ from __future__ import annotations @@ -27,9 +26,8 @@ import yaml from .errors import BadConfigValueError, MissingConfigError from .manifest import ConfigField, Manifest -#: Python types accepted for each declared type name. ``bool`` is deliberately -#: excluded from the numeric types: in YAML ``true`` is a bool, and silently -#: using it as 1.0 has caused real bugs. +#: 每个已声明类型名所接受的 Python 类型。``bool`` 被刻意排除在数值类型之外： +#: 在 YAML 里 ``true`` 是布尔值，把它悄悄当作 1.0 用曾引发过真实 bug。 _TYPE_CHECKS = { "str": (str,), "int": (int,), @@ -41,20 +39,19 @@ _TYPE_CHECKS = { class ResolvedValue(NamedTuple): - """One concrete value found in ``site.yaml`` for a declared field.""" + """在 ``site.yaml`` 中为某个已声明字段找到的一个具体值。""" path: str - """Concrete path, e.g. ``robot.robots[1].ip``.""" + """具体路径，例如 ``robot.robots[1].ip``。""" value: Any def load_site(path: Path | None = None) -> Dict[str, Any]: - """Read ``config/site.yaml``. + """读取 ``config/site.yaml``。 - Returns an empty dict when the file is absent, matching the lenient - behaviour the old ``site_io.load_site_dict`` had — absence is reported by - the guard as missing required config rather than as a parse crash. + 文件不存在时返回空字典，与旧 ``site_io.load_site_dict`` 的宽松行为保持 + 一致——缺失会由配置守卫报告为“缺少必需配置”，而不是解析崩溃。 """ from . import paths @@ -66,30 +63,27 @@ def load_site(path: Path | None = None) -> Dict[str, Any]: def resolve_path(site: Dict[str, Any], site_path: str) -> List[ResolvedValue]: - """Resolve a (possibly list-expanding) dotted path against parsed YAML. + """在解析后的 YAML 上解析一个（可能带列表展开的）点分路径。 - The ``[]`` token expands a collection into one entry per member, so - per-instance config can be declared once and still require *every* instance - to supply it: + ``[]`` 记号会把一个集合展开成每个成员一条记录，因此按实例的配置只需 + 声明一次，却仍要求*每一个*实例都提供它： - * sequence — ``robot.robots[].sn`` → ``robot.robots[0].sn``, ``...[1].sn`` - * mapping — ``cameras[].serial`` → ``cameras.d405_left.serial``, ... + * 序列 — ``robot.robots[].sn`` → ``robot.robots[0].sn``、``...[1].sn`` + * 映射 — ``cameras[].serial`` → ``cameras.d405_left.serial``…… - Mappings are supported because ``site.yaml`` registers cameras (and other - named devices) as mappings keyed by device name rather than as sequences. + 之所以支持映射，是因为 ``site.yaml`` 把相机（以及其他具名设备）登记为 + 以设备名为键的映射，而不是序列。 - Missing branches produce no results; the caller decides whether that is an - error. A *scalar* where a collection is expected (``cameras: 5``) is - reported as a single value resolved at the collection level, so the - type check can flag it instead of the operator being told, misleadingly, - that the value is simply absent. + 缺失的分支不产生结果；是否算错误由调用方决定。在本该是集合的位置出现 + *标量*（``cameras: 5``）时，会作为在集合层级解析出的单个值上报，让类型 + 检查能指出问题，而不是误导性地告诉操作员“该值不存在”。 """ tokens = [t.strip() for t in site_path.split(".") if t.strip()] return _resolve(site, tokens, "") def _resolve(node: Any, tokens: List[str], prefix: str) -> List[ResolvedValue]: - """Recursive worker for :func:`resolve_path`.""" + """:func:`resolve_path` 的递归工作函数。""" if not tokens: return [ResolvedValue(path=prefix, value=node)] @@ -122,8 +116,8 @@ def _resolve(node: Any, tokens: List[str], prefix: str) -> List[ResolvedValue]: if child is None: return [] - # Scalar where a collection was declared. Stop here and let the type check - # produce a precise complaint rather than a misleading "missing". + # 声明为集合的位置却是标量。在这里停下，让类型检查给出精确的报错， + # 而不是误导性的“缺失”。 return [ResolvedValue(path=child_path, value=child)] @@ -132,17 +126,17 @@ def check_config( site: Dict[str, Any], site_file: Optional[Path] = None, ) -> Dict[str, List[ResolvedValue]]: - """Validate all declared fields of a manifest against parsed site config. + """按解析后的现场配置校验清单中所有已声明的字段。 - Returns a mapping of ``field.key`` to the resolved values, so callers that - need the values anyway do not have to resolve them a second time. + 返回 ``field.key`` 到已解析值的映射，这样本来就需要这些值的调用方不必 + 再解析第二遍。 Raises ------ MissingConfigError - A required field has no value in ``site.yaml``. + 某个必需字段在 ``site.yaml`` 中没有值。 BadConfigValueError - A field is present but has the wrong YAML type. + 某个字段存在，但字段值的 YAML 类型不对。 """ resolved: Dict[str, List[ResolvedValue]] = {} problems: List[str] = [] @@ -156,9 +150,8 @@ def check_config( problems.append(_missing_message(field)) continue - # A required field with per-instance expansion needs *every* instance - # populated. Reporting only when all were blank would let one - # unconfigured arm slip through. + # 带按实例展开的必需字段需要*每一个*实例都填好。若只在全部为空时 + # 才报告，就会放过某一台未配置的机械臂。 if field.required: blank_paths = [v.path for v in values if _is_blank(v.value)] if blank_paths: @@ -204,7 +197,7 @@ def _default_site_file() -> Path: def _is_blank(value: Any) -> bool: - """Whether a value should be treated as 'not supplied'.""" + """判断某个值是否应被视为“未提供”。""" if value is None: return True if isinstance(value, str) and not value.strip(): @@ -217,7 +210,7 @@ def _is_blank(value: Any) -> bool: def _matches_type(value: Any, declared: str) -> bool: expected = _TYPE_CHECKS.get(declared) if expected is None: - # Unknown declared type is rejected at manifest load time. + # 未知的已声明类型会在清单加载阶段被拒。 return True if declared in ("int", "float") and isinstance(value, bool): return False @@ -225,11 +218,10 @@ def _matches_type(value: Any, declared: str) -> bool: def _missing_message(field: ConfigField, blank_paths: Optional[List[str]] = None) -> str: - """Render one missing-config problem. + """渲染一条缺失配置问题。 - ``blank_paths`` lists concrete per-instance paths that were present but - empty (e.g. ``robot.robots[1].sn``), which is far more actionable than - repeating the abstract declaration path. + ``blank_paths`` 列出存在但为空的具体按实例路径（例如 + ``robot.robots[1].sn``），这远比重复抽象的声明路径更具可操作性。 """ lines = [f" 缺: {field.key}"] if blank_paths: diff --git a/src/arm_interfaces/arm_interfaces/errors.py b/src/arm_interfaces/arm_interfaces/errors.py index cbc0ae4..5ed4261 100644 --- a/src/arm_interfaces/arm_interfaces/errors.py +++ b/src/arm_interfaces/arm_interfaces/errors.py @@ -1,13 +1,12 @@ -"""Structured contract errors. +"""结构化的契约错误。 -Every error carries a ``fix_hint`` that names the exact file and key a human -should edit. This is deliberate: the whole point of the contract layer is to -replace "something silently did nothing" (the old behaviour, where -``run_handeye.sh`` passed five parameters the node never declared) with an -actionable message. +每个错误都带有 ``fix_hint``，明确指出人应该去改哪个文件、哪个键。这是刻意 +设计的：契约层的全部意义，就是把“某个东西静默地什么都没做”（旧行为，例如 +``run_handeye.sh`` 传给节点的五个参数节点根本不曾声明）替换成一条可执行的 +提示。 -Callers should format errors with :func:`format_error`, which renders the -hint on its own indented line so it is impossible to miss in a terminal. +调用方应当用 :func:`format_error` 格式化错误，它会把修复提示单独渲染成 +一行缩进文本，在终端里绝不会被漏看。 """ from __future__ import annotations @@ -16,18 +15,17 @@ from typing import Optional class ContractError(Exception): - """Base class for every device-contract failure. + """所有设备契约失败的基类。 Attributes ---------- what: - One-line description of what is wrong. + 一句话说明哪里不对。 fix_hint: - Concrete instruction naming the file and key to edit. ``None`` means - the caller could not determine a remedy, which callers should treat as - a bug in the contract layer rather than user error. + 指明要改哪个文件、哪个键的具体指示。``None`` 表示调用方无法确定 + 补救办法，此时调用方应把它当作契约层的 bug，而不是用户错误。 detail: - Optional extra context (paths tried, values found, ...). + 可选的补充上下文（尝试过的路径、读到的值……）。 """ def __init__( @@ -41,36 +39,36 @@ class ContractError(Exception): self.fix_hint = fix_hint self.detail = detail - def __str__(self) -> str: # pragma: no cover - exercised via format_error + def __str__(self) -> str: # pragma: no cover - 通过 format_error 覆盖 return format_error(self) class RegistryError(ContractError): - """``devices/registry.yaml`` is missing, unreadable, or incomplete.""" + """``devices/registry.yaml`` 缺失、不可读或不完整。""" class ManifestError(ContractError): - """A per-brand ``manifest.yaml`` is missing, unreadable, or malformed.""" + """某个品牌的 ``manifest.yaml`` 缺失、不可读或格式错误。""" class MissingAssetError(ContractError): - """A vendor asset declared as required in a manifest is not on disk.""" + """清单中声明为必需的厂商物料不在磁盘上。""" class MissingConfigError(ContractError): - """``config/site.yaml`` does not supply a required declared config key.""" + """``config/site.yaml`` 未提供某个必需的已声明配置键。""" class CapabilityError(ContractError): - """An upper layer asked for a capability the selected driver does not declare.""" + """上层请求了所选驱动未声明的能力。""" class BadConfigValueError(ContractError): - """A config key exists but its value has the wrong type or shape.""" + """某个配置键存在，但值的类型或结构不对。""" def format_error(err: ContractError) -> str: - """Render a contract error as a multi-line, human-actionable message.""" + """把契约错误渲染成多行、可执行的中文提示。""" lines = [f"设备契约校验失败: {err.what}"] if err.detail: for line in str(err.detail).splitlines(): diff --git a/src/arm_interfaces/arm_interfaces/loader.py b/src/arm_interfaces/arm_interfaces/loader.py index bf998e4..decaaf8 100644 --- a/src/arm_interfaces/arm_interfaces/loader.py +++ b/src/arm_interfaces/arm_interfaces/loader.py @@ -1,18 +1,16 @@ -"""Load the whole device stack and check flows against it. +"""加载整个设备栈，并据此校验各流程。 -This is the single entry point the rest of the framework uses. It chains: +这是框架其余部分使用的唯一入口。它依次串联： -1. :mod:`arm_interfaces.registry` — which brand implements each kind -2. :mod:`arm_interfaces.manifest` — that brand's capabilities, assets, config -3. :mod:`arm_interfaces.config_guard` — does ``site.yaml`` supply that config +1. :mod:`arm_interfaces.registry` —— 每一类设备由哪个品牌实现 +2. :mod:`arm_interfaces.manifest` —— 该品牌的能力、物料、配置 +3. :mod:`arm_interfaces.config_guard` —— ``site.yaml`` 是否提供了那份配置 -then exposes :func:`check_requirement`, which verifies that a named flow -(tracking, hand-eye, gauge assembly, ...) only asks for capabilities the -selected drivers actually declare. +然后暴露 :func:`check_requirement`，用来验证某个具名流程 +（跟踪、手眼、通止规装配……）只索要所选驱动确实声明了的能力。 -The payoff is at startup: instead of discovering on the first motion that the -selected gripper cannot rotate, the run refuses to start and names the missing -capability, the interface it implies, and which manifest to change. +收益体现在启动时：不会等到第一次运动才发现所选夹爪不能旋转，而是直接 +拒绝启动，并点出缺失的能力、它对应的接口，以及该改哪个清单。 """ from __future__ import annotations @@ -29,31 +27,31 @@ from .registry import REQUIRED_KINDS, Registry, load_registry class DeviceStack(NamedTuple): - """Everything about the currently selected hardware.""" + """当前所选硬件的全部信息。""" registry: Registry manifests: Dict[str, Manifest] - """Registry kind -> loaded manifest, e.g. ``{"robots": }``.""" + """选型类别 -> 已加载的清单，例如 ``{"robots": }``。""" config: Dict[str, List[ResolvedValue]] - """``f"{kind}.{field.key}"`` -> resolved site.yaml values.""" + """``f"{kind}.{field.key}"`` -> 解析出的 site.yaml 值。""" site_path: Path - """The ``config/site.yaml`` that was validated.""" + """被校验过的 ``config/site.yaml``。""" def manifest(self, kind: str) -> Manifest: - """Return the manifest for a registry kind.""" + """返回某个选型类别的清单。""" return self.manifests[kind] def capabilities(self, kind: str) -> Set[str]: - """Effective capabilities of the selected driver for a kind.""" + """某类设备所选驱动的有效能力。""" manifest = self.manifests.get(kind) if manifest is None: return set() return set(effective_capabilities(manifest, self._selected_models(kind))) def _selected_models(self, kind: str) -> List[str]: - """Model ids selected in site.yaml for a kind (may be empty).""" + """某类设备在 site.yaml 中选中的型号 id（可能为空）。""" manifest = self.manifests.get(kind) if manifest is None: return [] @@ -70,12 +68,12 @@ class DeviceStack(NamedTuple): def effective_capabilities( manifest: Manifest, selected_models: Sequence[str] = () ) -> Set[str]: - """Capabilities a driver provides, given which of its models are selected. + """在给定选中哪些型号的前提下，驱动提供的能力。 - Top-level ``capabilities`` are what *every* model of the brand supports. - A model may add more (RGI adds ``rotate`` on top of the shared open/close). - Declared capabilities are never removed, because narrowing silently would - turn a clear startup error into a confusing runtime failure. + 顶层 ``capabilities`` 是该品牌*每一个*型号都支持的能力。某个型号可以 + 追加更多（RGI 在共有的 open/close 之上增加了 ``rotate``）。已声明的 + 能力绝不会被移除，因为悄悄收窄会把清晰的启动报错变成令人困惑的运行时 + 失败。 """ caps: Set[str] = set(manifest.capabilities) for model_id in selected_models: @@ -91,19 +89,16 @@ def load_device_stack( *, verify_assets: bool = True, ) -> DeviceStack: - """Load and validate the entire device stack. + """加载并校验整个设备栈。 - Validation failures across *all* device kinds are collected and reported - together. Reporting only the first one would force the operator to re-run - the check once per missing key, which is exactly the kind of round-trip this - layer exists to eliminate. + *所有*设备类别的校验失败都会被收集起来一并报告。只报告第一个，会迫使 + 操作员每修一个缺失的键就要重跑一次检查，而这种来回折腾正是本层要消除的。 Raises ------ - ContractError subclass - On contract violations, with actionable fix hints. When more than one - kind fails, a single aggregated :class:`ContractError` is raised whose - detail lists every problem. + ContractError 子类 + 出现契约违规时抛出，并附带可执行的修复提示。当不止一类设备失败时， + 会抛出单个聚合的 :class:`ContractError`，其 detail 列出全部问题。 """ registry = load_registry(registry_path) site_file = site_file or paths.site_path() @@ -114,7 +109,7 @@ def load_device_stack( problems: List[ContractError] = [] for kind in (*REQUIRED_KINDS, "sim"): - # A kind that was not selected at all is optional unless required. + # 完全没有选型的类别，除非是必需类别，否则可选。 if kind not in registry.selections: continue @@ -156,11 +151,10 @@ def load_device_stack( def _aggregate(problems: List[ContractError]) -> ContractError: - """Combine several contract errors into one reportable error. + """把多个契约错误合并成一个可上报的错误。 - A single problem is returned unchanged so the common case keeps its focused - message. Multiple problems are merged, preserving each one's own detail and - fix hint. + 只有单个问题时原样返回，让常见情形保持聚焦的消息。多个问题则合并， + 并保留每个问题各自的 detail 与修复提示。 """ if len(problems) == 1: return problems[0] @@ -188,16 +182,15 @@ def _aggregate(problems: List[ContractError]) -> ContractError: def check_requirement( stack: DeviceStack, requirement_name: str ) -> Dict[str, Set[str]]: - """Verify a flow's capability needs are met by the selected drivers. + """验证某个流程的能力需求是否被所选驱动满足。 - Returns the capabilities that were satisfied, grouped by kind, so callers - can log what the flow is running against. + 返回已满足的能力，按类别分组，便于调用方记录该流程运行在什么之上。 Raises ------ CapabilityError - When a needed capability is missing. The message names the capability, - the interface it maps to, the selected brand, and the manifest to edit. + 缺少某项所需能力时抛出。消息会点出该能力、它映射到的接口、所选的 + 品牌，以及该编辑的清单。 """ requirement = REQUIREMENTS.get(requirement_name) if requirement is None: diff --git a/src/arm_interfaces/arm_interfaces/manifest.py b/src/arm_interfaces/arm_interfaces/manifest.py index 43cf648..3b2b97a 100644 --- a/src/arm_interfaces/arm_interfaces/manifest.py +++ b/src/arm_interfaces/arm_interfaces/manifest.py @@ -1,18 +1,17 @@ -"""Load and validate a brand's ``manifest.yaml``. +"""加载并校验某个品牌的 ``manifest.yaml``。 -A manifest is a brand's self-description. It answers four questions: +清单是品牌的自我介绍，回答四个问题： -1. **Who am I?** — ``id``, ``label``, ``vendor``, ``version`` -2. **What vendor material must be on disk?** — ``assets`` (checked, with the - required format and naming convention recorded so operators can comply) -3. **What can I do?** — ``capabilities`` (subset of the vocabulary in - :mod:`arm_interfaces.capabilities`) -4. **What config do I need?** — ``config_schema``, whose ``site_path`` entries - point into ``config/site.yaml`` +1. **我是谁？** —— ``id``、``label``、``vendor``、``version`` +2. **磁盘上必须有哪些厂商物料？** —— ``assets``（会被检查，并记录必需的 + 格式与命名约定，方便操作员照做） +3. **我能做什么？** —— ``capabilities``（:mod:`arm_interfaces.capabilities` + 词表的子集） +4. **我需要什么配置？** —— ``config_schema``，其中每项 ``site_path`` 都指向 + ``config/site.yaml`` 里的位置 -Validation here is intentionally strict: a manifest that declares an unknown -capability or a malformed config entry is a bug that should surface at startup, -not a runtime surprise mid-motion. +这里的校验刻意从严：清单声明了未知能力或格式错误的配置项，属于应当在启动 +时暴露的 bug，而不是运动中途才冒出来的运行时意外。 """ from __future__ import annotations @@ -27,7 +26,7 @@ from .capabilities import ALL_CAPABILITIES, capabilities_for_kind from .errors import ManifestError, MissingAssetError from .registry import KIND_FOLDERS -#: registry key (plural) -> manifest ``kind`` value (singular). +#: 选型中的键（复数）-> 清单里 ``kind`` 的取值（单数）。 KIND_SINGULAR: Dict[str, str] = { "robots": "robot", "grippers": "gripper", @@ -36,15 +35,15 @@ KIND_SINGULAR: Dict[str, str] = { "sim": "sim", } -#: Config value types the contract layer knows how to check. +#: 契约层知道如何检查的配置值类型。 KNOWN_TYPES = {"str", "int", "float", "bool", "list", "dict"} -#: Manifest top-level keys that must be present. +#: 清单中必须存在的顶层键。 REQUIRED_KEYS = ("id", "label", "kind", "driver") class Asset(NamedTuple): - """One piece of vendor material a manifest expects on disk.""" + """清单期望磁盘上存在的一项厂商物料。""" path: str required: bool @@ -56,7 +55,7 @@ class Asset(NamedTuple): class DriverSpec(NamedTuple): - """How to start this brand's adapter.""" + """如何启动该品牌的适配器。""" package: str executable: str @@ -65,7 +64,7 @@ class DriverSpec(NamedTuple): class ConfigField(NamedTuple): - """One declared config key, pointing into ``config/site.yaml``.""" + """一个已声明的配置键，指向 ``config/site.yaml`` 中的位置。""" key: str site_path: str @@ -77,7 +76,7 @@ class ConfigField(NamedTuple): class Manifest(NamedTuple): - """Parsed and validated ``manifest.yaml``.""" + """已解析并校验的 ``manifest.yaml``。""" id: str label: str @@ -94,14 +93,14 @@ class Manifest(NamedTuple): raw: Dict[str, Any] def supports(self, capability: str) -> bool: - """Whether this driver declares the capability.""" + """该驱动是否声明了这项能力。""" return capability in self.capabilities def model_capabilities(self, model_id: str) -> Optional[Tuple[str, ...]]: - """Capabilities for a specific model id, if the manifest lists models. + """某个具体型号 id 的能力（前提是清单列出了型号）。 - Returns ``None`` when the manifest does not enumerate models, which - means the top-level ``capabilities`` list applies to all of them. + 清单未逐一列举型号时返回 ``None``，表示顶层 ``capabilities`` 列表 + 对全部型号都适用。 """ for model in self.models: if model.get("id") == model_id: @@ -113,19 +112,19 @@ class Manifest(NamedTuple): def manifest_path(kind: str, brand: str) -> Path: - """Return the expected manifest path for a kind/brand pair.""" + """返回某个“类别/品牌”组合预期的清单路径。""" return paths.brand_dir(KIND_FOLDERS.get(kind, kind), brand) / "manifest.yaml" def load_manifest(kind: str, brand: str, path: Path | None = None) -> Manifest: - """Read, validate and return the manifest for a kind/brand pair. + """读取、校验并返回某个“类别/品牌”组合的清单。 Parameters ---------- kind: - Registry key — ``robots``, ``grippers``, ``sensors``, ``cameras``, ``sim``. + 选型中的键 —— ``robots``、``grippers``、``sensors``、``cameras``、``sim``。 brand: - Brand folder name under ``devices//``. + ``devices//`` 下的品牌目录名。 """ target = path or manifest_path(kind, brand) @@ -187,11 +186,10 @@ def load_manifest(kind: str, brand: str, path: Path | None = None) -> Manifest: def check_assets(manifest: Manifest, brand: str, kind: str) -> None: - """Verify every ``required`` asset exists, with format hints in the error. + """校验每个 ``required`` 物料都存在，并在错误里给出格式提示。 - Only presence is checked. Format and naming cannot be verified - automatically, so they are reported back to the operator in the failure - message to make compliance possible. + 这里只检查是否存在。格式与命名无法自动验证，因此在失败消息里回告给 + 操作员，让照做成为可能。 """ base = paths.brand_dir(KIND_FOLDERS.get(kind, kind), brand) missing: List[str] = [] @@ -225,7 +223,7 @@ def check_assets(manifest: Manifest, brand: str, kind: str) -> None: # --------------------------------------------------------------------------- -# internal helpers +# 内部辅助函数 # --------------------------------------------------------------------------- def _available_brands(kind: str) -> List[str]: @@ -236,10 +234,10 @@ def _available_brands(kind: str) -> List[str]: def _asset_present(target: Path) -> bool: - """An asset counts as present if it is a file, a dir, or a symlink to either. + """物料是文件、目录，或指向二者的符号链接，都算作存在。 - A directory is accepted because vendors often ship a tree (``rdk/``) and the - operator should not be forced to add a sentinel file inside it. + 接受目录，是因为厂商常以整棵目录树（``rdk/``）交付，不应该逼着操作员 + 在目录里额外放一个占位文件。 """ if target.is_symlink(): return target.exists() @@ -298,7 +296,7 @@ def _parse_capabilities( detail=f"该清单里合法的能力: {', '.join(known_code) or '（无）'}", ) - # A capability declared twice is almost always a copy-paste slip. + # 同一项能力声明两次，几乎总是复制粘贴出的疏漏。 duplicates = sorted({c for c in raw if raw.count(c) > 1}) if duplicates: raise ManifestError( diff --git a/src/arm_interfaces/arm_interfaces/paths.py b/src/arm_interfaces/arm_interfaces/paths.py index be58563..a63e16a 100644 --- a/src/arm_interfaces/arm_interfaces/paths.py +++ b/src/arm_interfaces/arm_interfaces/paths.py @@ -1,15 +1,15 @@ -"""Single source of truth for workspace paths. +"""工作区路径的唯一权威来源。 -The old codebase resolved the workspace root in three different ways: -``arm_application/site_io.py``, ``scripts/lib/export_site_env.py``, and several -nodes that read ``site.yaml`` directly with their own ``_site_path()`` helpers. -They did not always agree. This module is the one place that resolves paths, and -everything else — including ``site_io.py`` — should delegate here. +旧代码库用三种不同方式解析工作区根目录： +``arm_application/site_io.py``、``scripts/lib/export_site_env.py``，以及若干 +用各自 ``_site_path()`` 辅助函数直接读 ``site.yaml`` 的节点。 +它们并不总是一致。本模块是解析路径的唯一场所，其他所有地方 +——包括 ``site_io.py``——都应委托到这里。 -Resolution order (first non-empty wins): +解析顺序（第一个非空者胜出）： -1. Explicit environment override (``SITE_ROS2_ROOT`` / ``SITE_CONFIG``) -2. ``ROS2_WS`` (exported by ``scripts/env.sh``) +1. 显式环境变量覆盖（``SITE_ROS2_ROOT`` / ``SITE_CONFIG``） +2. ``ROS2_WS``（由 ``scripts/env.sh`` 导出） 3. ``~/ros2`` """ @@ -20,7 +20,7 @@ from pathlib import Path def ros2_root() -> Path: - """Return the workspace root, honouring environment overrides.""" + """返回工作区根目录，并遵循环境变量覆盖。""" for key in ("SITE_ROS2_ROOT", "ROS2_WS"): value = os.environ.get(key, "").strip() if value: @@ -29,7 +29,7 @@ def ros2_root() -> Path: def site_path() -> Path: - """Return the path to the master config ``config/site.yaml``.""" + """返回主配置 ``config/site.yaml`` 的路径。""" override = os.environ.get("SITE_CONFIG", "").strip() if override: return Path(override).expanduser() @@ -37,7 +37,7 @@ def site_path() -> Path: def devices_root() -> Path: - """Return the root of the device folder tree.""" + """返回设备目录树的根目录。""" override = os.environ.get("SITE_DEVICES_ROOT", "").strip() if override: return Path(override).expanduser() @@ -45,19 +45,19 @@ def devices_root() -> Path: def registry_path() -> Path: - """Return the path to the device selection file ``devices/registry.yaml``.""" + """返回设备选型文件 ``devices/registry.yaml`` 的路径。""" return devices_root() / "registry.yaml" def handeye_root() -> Path: - """Return the root of the hand-eye module directory (``handeye/``).""" + """返回手眼模块目录（``handeye/``）的根目录。""" return ros2_root() / "handeye" def brand_dir(kind: str, brand: str) -> Path: - """Return the directory holding one brand under a device kind. + """返回某类设备下某个品牌所在的目录。 - ``kind`` is the plural folder name used in ``devices/`` (``robots``, - ``grippers``, ``sensors``, ``cameras``, ``sim``). + ``kind`` 是 ``devices/`` 中使用的复数目录名（``robots``、 + ``grippers``、``sensors``、``cameras``、``sim``）。 """ return devices_root() / kind / brand diff --git a/src/arm_interfaces/arm_interfaces/registry.py b/src/arm_interfaces/arm_interfaces/registry.py index d4fe3d9..a04ce81 100644 --- a/src/arm_interfaces/arm_interfaces/registry.py +++ b/src/arm_interfaces/arm_interfaces/registry.py @@ -1,9 +1,8 @@ -"""Load and validate ``devices/registry.yaml``. +"""加载并校验 ``devices/registry.yaml``。 -The registry answers exactly one question: *which brand implements each device -kind right now?* It holds no field values by design — every speed, threshold and -serial number lives in ``config/site.yaml``. Keeping selection and values apart -is what stops the "I changed the value but nothing happened" class of bug. +选型文件只回答一个问题：*当前每一类设备由哪个品牌实现？* 按设计它不保存 +任何字段值——所有速度、阈值和序列号都在 ``config/site.yaml`` 里。把“选型” +与“取值”分开，正是为了杜绝“改了值却毫无反应”这一类 bug。 """ from __future__ import annotations @@ -16,8 +15,8 @@ import yaml from . import paths from .errors import RegistryError -#: Device kind -> the folder under ``devices/`` holding its brands. -#: Also the key used in ``registry.yaml``. +#: 设备类别 -> ``devices/`` 下存放其品牌的目录。 +#: 同时也是 ``registry.yaml`` 中使用的键。 KIND_FOLDERS: Dict[str, str] = { "robots": "robots", "grippers": "grippers", @@ -26,25 +25,25 @@ KIND_FOLDERS: Dict[str, str] = { "sim": "sim", } -#: Every kind the registry must select. ``sim`` is optional (headless setups). +#: 选型文件必须选择的每一类设备。``sim`` 是可选的（无头环境）。 REQUIRED_KINDS = ("robots", "grippers", "sensors", "cameras") OPTIONAL_KINDS = ("sim",) class Registry(NamedTuple): - """Parsed ``devices/registry.yaml``.""" + """解析后的 ``devices/registry.yaml``。""" selections: Dict[str, str] - """kind -> brand id, e.g. ``{"robots": "flexiv"}``.""" + """kind -> 品牌 id，例如 ``{"robots": "flexiv"}``。""" path: Path - """Where the registry was loaded from (for error messages).""" + """选型文件的加载来源（用于错误消息）。""" raw: Dict[str, Any] - """Full parsed document, for forward compatibility.""" + """完整解析结果，便于向前兼容。""" def brand(self, kind: str) -> str: - """Return the selected brand for a kind, raising if unset.""" + """返回某类设备选中的品牌，未设置则抛错。""" try: return self.selections[kind] except KeyError: @@ -58,13 +57,12 @@ class Registry(NamedTuple): def load_registry(path: Path | None = None) -> Registry: - """Read and validate the registry file. + """读取并校验选型文件。 Raises ------ RegistryError - If the file is missing, unparseable, is not a mapping, or omits a - required kind. + 文件缺失、无法解析、顶层不是映射，或漏掉必需的设备类别时抛出。 """ target = path or paths.registry_path() @@ -119,7 +117,7 @@ def load_registry(path: Path | None = None) -> Registry: def list_available_brands(kind: str) -> list[str]: - """Return brand folder names present on disk for a device kind.""" + """返回到磁盘上某类设备现存的品牌目录名。""" folder = paths.devices_root() / KIND_FOLDERS.get(kind, kind) if not folder.is_dir(): return [] diff --git a/src/arm_interfaces/setup.cfg b/src/arm_interfaces/setup.cfg index 9c8e3dc..24ca376 100644 --- a/src/arm_interfaces/setup.cfg +++ b/src/arm_interfaces/setup.cfg @@ -4,9 +4,9 @@ script_dir=$base/lib/arm_interfaces install_scripts=$base/lib/arm_interfaces [tool:pytest] -# ROS 2 Humble's two launch-testing pytest plugins use old pytest hook -# signatures that current pytest rejects, which aborts collection before our -# tests even run. Disabling them is safe: these are pure-Python unit tests and -# use none of launch_testing's fixtures. +# ROS 2 Humble 的两个 launch-testing pytest 插件使用的旧版 pytest hook +# 签名会被当前 pytest 拒绝，导致测试还没开始就被中止收集。 +# 禁用它们是安全的：这些是纯 Python 单元测试， +# 完全没有用到 launch_testing 的任何 fixture。 addopts = -p no:launch_testing -p no:launch_ros testpaths = test diff --git a/src/arm_interfaces/setup.py b/src/arm_interfaces/setup.py index 2d2e704..ca2b44a 100644 --- a/src/arm_interfaces/setup.py +++ b/src/arm_interfaces/setup.py @@ -18,7 +18,7 @@ setup( license='Apache-2.0', entry_points={ 'console_scripts': [ - # Validate the whole device stack: registry -> manifests -> site.yaml. + # 校验整条设备链：registry → 各 manifest → site.yaml。 'device_check = arm_interfaces.cli:main', ], }, diff --git a/src/arm_interfaces/test/test_contract.py b/src/arm_interfaces/test/test_contract.py index 249bab9..56b378a 100644 --- a/src/arm_interfaces/test/test_contract.py +++ b/src/arm_interfaces/test/test_contract.py @@ -1,10 +1,9 @@ -"""Unit tests for the device contract layer. +"""设备契约层的单元测试。 -These build a throwaway workspace in ``tmp_path`` so they never touch the real -``devices/`` tree or ``config/site.yaml``. Every failure mode that the layer is -supposed to catch has a test asserting the *shape* of the error — in particular -that a ``fix_hint`` is present, because a contract error without a remediation -instruction is only half the job. +这些测试在 ``tmp_path`` 里搭一个用完即弃的工作区，因此绝不会碰真实的 +``devices/`` 目录树或 ``config/site.yaml``。本层应当捕获的每一种失败模式 +都有一个测试断言错误的*形态*——尤其是必须带 ``fix_hint``，因为一条没有 +修复指示的契约错误只算完成了一半工作。 """ from __future__ import annotations @@ -36,7 +35,7 @@ from arm_interfaces.registry import list_available_brands # --------------------------------------------------------------------------- -# fake workspace construction +# 构造假工作区 # --------------------------------------------------------------------------- MINIMAL_REGISTRY = """\ @@ -143,7 +142,7 @@ cameras: @pytest.fixture def workspace(tmp_path: Path, monkeypatch: pytest.MonkeyPatch) -> Dict[str, Path]: - """Create a minimal, valid device workspace and point the layer at it.""" + """创建一个最小可用的设备工作区，并让契约层指向它。""" devices = tmp_path / "devices" for kind, brand, manifest in ( ("robots", "acme", ACME_MANIFEST), @@ -173,7 +172,7 @@ def write(path: Path, content: str) -> None: # --------------------------------------------------------------------------- -# registry +# 选型 # --------------------------------------------------------------------------- class TestRegistry: @@ -217,7 +216,7 @@ class TestRegistry: # --------------------------------------------------------------------------- -# manifest +# 清单 # --------------------------------------------------------------------------- class TestManifest: @@ -254,7 +253,7 @@ class TestManifest: assert "get_arm_state" in (excinfo.value.fix_hint or "") def test_capability_from_another_kind_is_rejected(self, workspace): - """A robot must not be able to claim a gripper capability.""" + """机器人绝不能声称自己拥有夹爪能力。""" write( workspace["devices"] / "robots" / "acme" / "manifest.yaml", ACME_MANIFEST.replace(" - get_arm_state", " - rotate\n - get_arm_state"), @@ -276,7 +275,7 @@ class TestManifest: assert "重复" in excinfo.value.what def test_required_and_default_together_is_rejected(self, workspace): - """Ambiguous intent: either the user must supply it, or we have a fallback.""" + """意图含糊：要么必须由用户提供，要么我们已有兜底值。""" write( workspace["devices"] / "robots" / "acme" / "manifest.yaml", ACME_MANIFEST.replace( @@ -334,7 +333,7 @@ class TestManifest: # --------------------------------------------------------------------------- -# asset presence +# 物料是否存在 # --------------------------------------------------------------------------- class TestAssets: @@ -354,7 +353,7 @@ class TestAssets: with pytest.raises(MissingAssetError) as excinfo: check_assets(loaded, "acme", "robots") message = str(excinfo.value) - # The operator must be told how to comply, not merely that it is missing. + # 必须告诉操作员该怎么照做，而不只是说“缺了”。 assert "厂商原始格式" in message assert "保持原目录名" in message @@ -368,7 +367,7 @@ class TestAssets: write(workspace["devices"] / "robots" / "acme" / "manifest.yaml", manifest) loaded = load_manifest("robots", "acme") (workspace["devices"] / "robots" / "acme" / "rdk").mkdir() - check_assets(loaded, "acme", "robots") # must not raise + check_assets(loaded, "acme", "robots") # 不应抛错 def test_optional_asset_may_be_absent(self, workspace): manifest = ACME_MANIFEST + textwrap.dedent("""\ @@ -379,11 +378,11 @@ class TestAssets: """) write(workspace["devices"] / "robots" / "acme" / "manifest.yaml", manifest) loaded = load_manifest("robots", "acme") - check_assets(loaded, "acme", "robots") # must not raise + check_assets(loaded, "acme", "robots") # 不应抛错 # --------------------------------------------------------------------------- -# config_guard: path resolution and type checking +# config_guard：路径解析与类型检查 # --------------------------------------------------------------------------- class TestResolvePath: @@ -400,7 +399,7 @@ class TestResolvePath: assert found[1].path == "robot.robots[1].sn" def test_mapping_expansion(self): - """site.yaml registers cameras as a mapping keyed by name, not a sequence.""" + """site.yaml 把相机登记为以名称作键的映射，而不是序列。""" site = {"cameras": {"left": {"serial": "1"}, "right": {"serial": "2"}}} found = resolve_path(site, "cameras[].serial") assert [f.value for f in found] == ["1", "2"] @@ -410,7 +409,7 @@ class TestResolvePath: assert resolve_path({"a": {}}, "a.b.c") == [] def test_scalar_where_collection_expected_is_kept_for_type_check(self): - """Better to report a type error than to silently claim 'missing'.""" + """报告类型错误，总好过静默声称“缺失”。""" found = resolve_path({"cameras": 5}, "cameras[].serial") assert len(found) == 1 assert found[0].value == 5 @@ -418,10 +417,10 @@ class TestResolvePath: class TestCheckConfig: def _manifest_with(self, workspace, extra: str): - """Append extra ``config_schema`` entries to the Acme manifest. + """把额外的 ``config_schema`` 条目追加到 Acme 清单上。 - ``extra`` is written flush-left and indented here, so call sites can - indent a YAML snippet naturally without counting spaces. + ``extra`` 以顶格书写后在这里统一缩进，因此调用处可以自然地为 YAML + 片段缩进，而无需数空格。 """ body = textwrap.indent(textwrap.dedent(extra), " ") write(workspace["devices"] / "robots" / "acme" / "manifest.yaml", ACME_MANIFEST + body) @@ -483,7 +482,7 @@ class TestCheckConfig: assert "fast" in (excinfo.value.detail or "") def test_bool_is_not_accepted_as_float(self, workspace): - """YAML ``true`` must not be silently used as 1.0.""" + """YAML 里的 ``true`` 绝不能被悄悄当作 1.0 使用。""" manifest = self._manifest_with( workspace, """\ @@ -499,7 +498,7 @@ class TestCheckConfig: check_config(manifest, site) def test_every_instance_must_supply_per_instance_value(self, workspace): - """robot.robots[].sn requires all arms to be configured.""" + """robot.robots[].sn 要求所有机械臂都完成配置。""" write( workspace["devices"] / "robots" / "acme" / "manifest.yaml", ACME_MANIFEST.replace("site_path: robot.sn", "site_path: robot.robots[].sn"), @@ -509,12 +508,12 @@ class TestCheckConfig: site["robot"]["robots"] = [{"sn": "A"}, {"sn": ""}] with pytest.raises(MissingConfigError) as excinfo: check_config(manifest, site) - # The offending instance must be identified by index. + # 出问题的那个实例必须按索引定位到。 assert "robots[1].sn" in str(excinfo.value) # --------------------------------------------------------------------------- -# loader: aggregation and capabilities +# loader：聚合与能力 # --------------------------------------------------------------------------- class TestLoader: @@ -525,9 +524,9 @@ class TestLoader: assert stack.capabilities("robots") == {"get_arm_state", "follow_joint_trajectory"} def test_all_problems_reported_at_once(self, workspace): - """One run must surface every broken kind, not just the first.""" + """一次运行必须暴露出每一类坏掉的设备，而不只是第一个。""" site = workspace["site"] - write(site, "cameras:\n cam_a:\n serial: '1'\n") # robot.sn etc. missing + write(site, "cameras:\n cam_a:\n serial: '1'\n") # robot.sn 等缺失 with pytest.raises(Exception) as excinfo: load_device_stack() message = str(excinfo.value) @@ -545,7 +544,7 @@ class TestLoader: write(workspace["devices"] / "robots" / "acme" / "manifest.yaml", manifest) with pytest.raises(MissingAssetError): load_device_stack() - load_device_stack(verify_assets=False) # must not raise + load_device_stack(verify_assets=False) # 不应抛错 class TestEffectiveCapabilities: diff --git a/src/arm_interfaces/test/test_real_devices.py b/src/arm_interfaces/test/test_real_devices.py index c191c3a..0264272 100644 --- a/src/arm_interfaces/test/test_real_devices.py +++ b/src/arm_interfaces/test/test_real_devices.py @@ -1,11 +1,10 @@ -"""Integration tests against the real workspace. +"""针对真实工作区的集成测试。 -These are the tests that keep the shipped configuration honest. They load the -actual ``devices/`` tree and the actual ``config/site.yaml``, so a bad edit to -either fails here rather than on the robot. +这些测试用来让随仓库交付的配置保持诚实。它们加载真实的 ``devices/`` 目录树 +和真实的 ``config/site.yaml``，所以对其中任何一处的糟糕改动都会在这里失败， +而不是在机器人上失败。 -They are skipped when the real workspace is not present (for example when the -package is built in isolation), so they never turn into false failures. +真实工作区不存在时会跳过（例如包被单独构建时），因此它们绝不会变成假失败。 """ from __future__ import annotations @@ -31,26 +30,26 @@ pytestmark = pytest.mark.skipif( class TestRealWorkspace: def test_contract_satisfied(self, monkeypatch): - """The shipped device tree and site.yaml must pass validation.""" - # Guard against a leftover SITE_* override from another test leaking in. + """随仓库交付的设备目录树和 site.yaml 必须通过校验。""" + # 防止另一个测试遗留的 SITE_* 覆盖泄漏进来。 for var in ("SITE_DEVICES_ROOT", "SITE_CONFIG", "SITE_ROS2_ROOT"): monkeypatch.delenv(var, raising=False) stack = load_device_stack() - # Every required kind is present and selected. + # 每个必需的设备类别都已存在并完成选型。 for kind in ("robots", "grippers", "sensors", "cameras"): assert stack.manifest(kind) is not None, f"{kind} 未选型" def test_selected_models_resolve(self, monkeypatch): - """Model ids named in site.yaml must exist in the corresponding manifest.""" + """site.yaml 中写明的型号 id 必须存在于对应的清单里。""" for var in ("SITE_DEVICES_ROOT", "SITE_CONFIG"): monkeypatch.delenv(var, raising=False) stack = load_device_stack() site = load_site() - # Cameras: every entry's model must be a declared model id. + # 相机：每一项的 model 都必须是清单声明的型号 id。 camera_manifest = stack.manifest("cameras") declared = {model["id"] for model in camera_manifest.models} for name, camera in (site.get("cameras") or {}).items(): @@ -59,7 +58,7 @@ class TestRealWorkspace: f"{camera_manifest.path} 声明的型号 {sorted(declared)} 里" ) - # Grippers: same check against the gripper manifest's models. + # 夹爪：对夹爪清单的型号做同样的检查。 gripper_manifest = stack.manifest("grippers") gripper_models = {model["id"] for model in gripper_manifest.models} for index, robot in enumerate((site.get("robot") or {}).get("robots") or []): @@ -72,7 +71,7 @@ class TestRealWorkspace: ) def test_sensor_models_folder_matches_config(self, monkeypatch): - """tactile.model must match a subdirectory under sensors/orisys/models/.""" + """tactile.model 必须与 sensors/orisys/models/ 下的某个子目录对应。""" for var in ("SITE_DEVICES_ROOT", "SITE_CONFIG"): monkeypatch.delenv(var, raising=False) @@ -95,7 +94,7 @@ class TestRealWorkspace: ) def test_assembly_requirement_met_by_real_selection(self, monkeypatch): - """The gauge assembly flow must be satisfiable with shipped hardware.""" + """随仓库交付的硬件必须能满足通止规装配流程。""" for var in ("SITE_DEVICES_ROOT", "SITE_CONFIG"): monkeypatch.delenv(var, raising=False) @@ -115,13 +114,11 @@ class TestRealWorkspace: class TestExampleParity: - """``site.example.yaml`` is generated from ``site.yaml``; it must stay in sync. + """``site.example.yaml`` 由 ``site.yaml`` 生成，必须保持同步。 - The template used to be maintained by hand and had drifted 114 keys behind - the live config, so a fresh deployment following it would silently miss - ``servo.*``, ``hole_detect.stage2.*`` and ``sim.*``. It is now generated by - ``scripts/gen_site_example.py``, and this test fails the moment the two - diverge. + 这份模板以前靠手工维护，曾比现场配置落后 114 个键，导致照着它做的新部署 + 会静默漏掉 ``servo.*``、``hole_detect.stage2.*`` 和 ``sim.*``。现在它由 + ``scripts/gen_site_example.py`` 生成，两者一旦分叉，本测试立即失败。 """ def _example_path(self) -> Path: @@ -144,7 +141,7 @@ class TestExampleParity: ) def test_example_contains_no_live_values(self): - """The template must not leak serial numbers, IPs or user paths.""" + """模板绝不能泄漏序列号、IP 或用户路径。""" example = self._example_path() if not example.is_file(): pytest.skip("config/site.example.yaml 不存在")