From 561d7da21f20d3e63030f81510ae2f29eed01d46 Mon Sep 17 00:00:00 2001 From: arm_dev Date: Tue, 20 Oct 2026 11:40:57 +0800 Subject: [PATCH] =?UTF-8?q?=E9=A6=96=E8=BD=AE=E9=A1=BA=E6=97=B6=E9=92=88?= =?UTF-8?q?=E8=BF=9B=E5=AD=94=E6=94=B9=E5=9B=9E=2010=C2=B0/s=EF=BC=8C?= =?UTF-8?q?=E7=A9=BA=E5=9B=9E=E5=9B=BA=E5=AE=9A=E5=81=9A=204=20=E8=BD=AE?= =?UTF-8?q?=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 首轮咬合的顺时针进孔又走了接口上限，螺纹还没咬住就转完了。这一段改回读 extreme_speed_deg_s（现场 10°/s）。首轮逆时针出孔，以及空回每一轮的正反转， 仍走 0.90 rad/s 上限。 空回原先只做一轮。首轮咬合结束后，张开、微抬、逆时针、按本轮实测上抬降回、 合爪、顺时针这一整段固定重复 UNWIND_ROUNDS=4。抬和降留在轮内，下降量用这一轮 记住的上抬，不用上一轮的数。日志打空回第 i/4 轮；某一轮没走完就停，后面不再做。 10:40 合爪命令已经发到话题，节点被 Serial.flush/tcdrain 堵住 120s 才执行， 流水线 8s 时手指还停在 pos=400，第 2–4 轮没开始。发送改成等发送缓冲排空， 0.25s 排不空就失败返回，不再调用 tcdrain。同一次开合状态没跟上再发一次。 Co-authored-by: Cursor --- config/site.example.yaml | 5 +- config/site.yaml | 5 +- docs/GRIPPER.md | 2 + docs/HANDBOOK.md | 2 +- .../arm_application/dh_pgea.py | 63 +++++- .../arm_application/dh_pgea_node.py | 5 + .../arm_application/pipeline_node.py | 202 ++++++++++++------ .../test/test_dh_pgea_latch.py | 34 +++ .../test/test_pipeline_stage1_diag.py | 41 +++- 9 files changed, 278 insertions(+), 81 deletions(-) diff --git a/config/site.example.yaml b/config/site.example.yaml index 7dd45d0..a376815 100644 --- a/config/site.example.yaml +++ b/config/site.example.yaml @@ -936,8 +936,9 @@ gauge_assembly: # ---- S01 回 home 后按工具系逆时针预绕到极限角 ---- # 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。 extreme_angle_deg: 150.0 # 想转多少度（按工具系逆时针） - extreme_speed_deg_s: 10.0 # 规装配预绕的关节角速度 [deg/s]。 - # 插孔后的四段 A7 不读这个数，走接口上限 0.90 rad/s。 + extreme_speed_deg_s: 10.0 # 关节角速度 [deg/s]。规装配预绕，以及插孔后首轮 + # 顺时针进孔，都用这个数。首轮逆时针出孔、空回各轮的 + # 正反转不读它，走接口上限 0.90 rad/s。 unwind_open_permille: 400 # 第三步松爪只开到这个千分比。0=合，1000=全开。 # 2026-10-19 从 200 改成 400，只张开这个量。 unwind_lift_mm: 3.0 # 第三步张开后只抬的 TCP Z [mm]（0.3cm）。 diff --git a/config/site.yaml b/config/site.yaml index 144de26..37ef9b8 100644 --- a/config/site.yaml +++ b/config/site.yaml @@ -920,8 +920,9 @@ gauge_assembly: # ---- S01 回 home 后按工具系逆时针预绕到极限角 ---- # 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。 extreme_angle_deg: 150.0 # 想转多少度（按工具系逆时针） - extreme_speed_deg_s: 10.0 # 规装配预绕的关节角速度 [deg/s]。 - # 插孔后的四段 A7 不读这个数，走接口上限 0.90 rad/s。 + extreme_speed_deg_s: 10.0 # 关节角速度 [deg/s]。规装配预绕，以及插孔后首轮 + # 顺时针进孔，都用这个数。首轮逆时针出孔、空回各轮的 + # 正反转不读它，走接口上限 0.90 rad/s。 unwind_open_permille: 400 # 第三步松爪只开到这个千分比。0=合，1000=全开。 # 2026-10-19 从 200 改成 400，只张开这个量。 unwind_lift_mm: 3.0 # 第三步张开后只抬的 TCP Z [mm]（0.3cm）。 diff --git a/docs/GRIPPER.md b/docs/GRIPPER.md index 0eae70f..d17cc5b 100644 --- a/docs/GRIPPER.md +++ b/docs/GRIPPER.md @@ -80,4 +80,6 @@ USB 未插齐时节点仍可起来（`allow_offline:=true`），对应侧 `statu 串口 `EIO`（USB 掉线）抛 `DhPgeaBusError`，节点先关串口再重开（1 秒最多一次），不再整进程带 traceback 退出。没有应答（`DhPgeaError`）先继续重试；连续 `BUS_REOPEN_AFTER_FAILURES=30` 拍都读不到也重开一次——2026-10-20 那次就是写有回显、读一直无应答，重启节点立刻恢复，说明坏的是串口会话。 +发送不再调用 `Serial.flush()`（底层是 `tcdrain`）。USB 串口不收数据时这个调用不看写超时。2026-10-20 10:40 合爪命令已经发布，节点被它堵住 120s 才打出 `ctrl pos=0`，流水线 8s 时状态还是 `grip=1 pos=400`，这一轮就停了。现在发送缓冲在 `timeout_s`（0.25s）内排不空就失败返回；流水线对同一次开合再发一次命令。 + 用户需在 `dialout`：`sudo usermod -aG dialout $USER`。 diff --git a/docs/HANDBOOK.md b/docs/HANDBOOK.md index cf17863..f497879 100644 --- a/docs/HANDBOOK.md +++ b/docs/HANDBOOK.md @@ -93,7 +93,7 @@ ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。D435 这 `tool10120938` 的 TCP 长度（0.226587 m，2026-10-16 下午 RDK 重读）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。 贴面之后就停在这个法兰高度，不抬回 `hole_hover_z_m`。没有接触传感器。 `stage2.hole_entry_mm`（现为 1）> 0 时，插孔目标改成规尖在工件上表面以下这么深（孔口凹槽），再转 A7；为 0 时仍停在 `final_gap_mm`。进孔超过 8 mm（板厚 12 mm）直接拒绝。2026-10-19 12:20 写成 4 mm 时，臂在面下 1.6 mm 被挡住。 -然后只转左臂 A7（厂家 `SendJointPosition` 的第 7 轴，模式 `NRT_JOINT_POSITION`）：先逆时针转到软件限位内侧 `extreme_margin_deg`，再顺时针转到另一侧。顺时针到位记为「首轮咬合结束」。接着 `run_gripper.sh --arm <臂号> --cli pos` 加上 `unwind_open_permille`（现为 400，不全开），再用一条厂家 `MoveL` 只抬 TCP 的 Z `unwind_lift_mm`（现为 3，即 0.3 cm；XY 和姿态不动，不拆成关节小跳）。这条和随后的下降都带 `zoneRadius=ZFine`、`targetTolerLevel=1`：厂家默认 `Z50` 和等级 3 会在还差约 0.5 mm 时就报到位，下降和记住的上抬对不上，合爪就不会发。A7 再逆时针回到限位。然后下降刚才实测的那一段（不另写一个下降量），`--cli close`。合爪目标是位置 0；固件在位置先到时写 grip=1（到位），力先到才写 grip=2（夹住）。手指已经在合拢端就继续，还停在张开位则不转。再顺时针转到限位。这四段 A7 都请求接口上限 0.90 rad/s；两端速度为 0 的样条峰速约是平均的 1.5 倍，所以整段按平均约 34 °/s 排程，避免时间到了还差几十度。`extreme_speed_deg_s` 只留给规装配预绕。前六轴不动。顺时针是进孔，逆时针是出孔。夹爪或这一段 Z 对不上，就停在当前关节。 +然后只转左臂 A7（厂家 `SendJointPosition` 的第 7 轴，模式 `NRT_JOINT_POSITION`）：先逆时针转到软件限位内侧 `extreme_margin_deg`（接口最快），再顺时针转到另一侧，这一段用 `extreme_speed_deg_s`（现为 10 °/s）。顺时针到位记为「首轮咬合结束」。接着下面这一整段固定重复 4 轮（常量 `UNWIND_ROUNDS`，不进 `site.yaml`）：`run_gripper.sh --arm <臂号> --cli pos` 加上 `unwind_open_permille`（现为 400，不全开），再用一条厂家 `MoveL` 只抬 TCP 的 Z `unwind_lift_mm`（现为 3，即 0.3 cm；XY 和姿态不动，不拆成关节小跳）。这条和随后的下降都带 `zoneRadius=ZFine`、`targetTolerLevel=1`：厂家默认 `Z50` 和等级 3 会在还差约 0.5 mm 时就报到位，下降和记住的上抬对不上，合爪就不会发。A7 再逆时针回到限位。然后下降刚才这一轮实测的那一段（不另写一个下降量，也不用上一轮的数），`--cli close`。合爪目标是位置 0；固件在位置先到时写 grip=1（到位），力先到才写 grip=2（夹住）。手指已经在合拢端就继续，还停在张开位则不转。再顺时针转到限位。日志打 `空回第i/4轮 开始` 和 `完成`，带本轮上抬、下降的毫米数。某一轮没走完就停在当前关节，后面的轮次不再做。首轮逆时针出孔，以及空回每一轮的逆时针、顺时针，请求接口上限 0.90 rad/s；两端速度为 0 的样条峰速约是平均的 1.5 倍，所以这几段按平均约 34 °/s 排程，避免时间到了还差几十度。首轮顺时针进孔不在这几段里，速度是上面的 10 °/s。前六轴不动。顺时针是进孔，逆时针是出孔。夹爪或这一段 Z 对不上，就停在当前关节。 只测 D435 这一段位移时加 `--d435-only`：不交 D405、不进二阶段，并且要求先停 `run_real.sh`。 ```bash diff --git a/src/arm_application/arm_application/dh_pgea.py b/src/arm_application/arm_application/dh_pgea.py index 6c437ea..1baafd0 100644 --- a/src/arm_application/arm_application/dh_pgea.py +++ b/src/arm_application/arm_application/dh_pgea.py @@ -21,11 +21,15 @@ Flexiv RDK 的 ``Gripper`` 驱动不了这个夹爪：它不是 Studio Device。 from __future__ import annotations import argparse +import fcntl +import select +import struct import sys +import termios import threading import time from dataclasses import dataclass -from typing import Optional +from typing import Callable, Optional try: import serial @@ -104,6 +108,21 @@ INIT_STATE_NAME = { } +def wait_tx_empty(queued: Callable[[], int], deadline: float, pause=time.sleep) -> bool: + """等到发送缓冲里的字节数变成 0。到 ``deadline`` 还没空就返回 False。 + + 不要改成 ``Serial.flush()`` / ``tcdrain``。USB 串口不收数据时那个调用 + 不看写超时，会把夹爪节点的回调一直堵住。2026-10-20 合爪：命令 10:40:16 + 已经交给话题，节点 120s 后才打出 ``ctrl pos=0``，流水线 8s 时手指还停在 400。 + """ + while True: + if int(queued()) == 0: + return True + if time.monotonic() >= deadline: + return False + pause(0.01) + + def latched_position_break(command: int, current: int, target: int) -> Optional[int]: """命令寄存器已经等于目标、手指却不在目标上时，返回要先写入的中间位置。 @@ -384,16 +403,16 @@ class DhPgeaGripper: time.sleep(2.0 * bit_s) self._ser.reset_input_buffer() self._ser.write(frame) - self._ser.flush() + # 不用 flush/tcdrain。发送缓冲排不空就在 timeout_s 内失败，回调才能返回。 + # 读应答另给一个 timeout_s，不能和排空共用同一个截止时间。 + if not wait_tx_empty(self._tx_queued, time.monotonic() + self.timeout_s): + raise DhPgeaError( + f"{self.port} 串口发送缓冲 {self.timeout_s:.2f}s 内没排空" + ) if self.rs485_rts: time.sleep(len(frame) * bit_s + 0.0008) self._set_tx(False) - deadline = time.monotonic() + self.timeout_s - buf = bytearray() - while time.monotonic() < deadline and len(buf) < expect: - chunk = self._ser.read(expect - len(buf)) - if chunk: - buf.extend(chunk) + buf = self._read_reply(expect, time.monotonic() + self.timeout_s) except OSError as exc: raise DhPgeaBusError( f"{self.port} 串口读写失败（{exc}）。" @@ -401,6 +420,34 @@ class DhPgeaGripper: ) from exc return bytes(buf) if buf else None + def _tx_queued(self) -> int: + """内核发送缓冲里还没推出去的字节数。""" + raw = fcntl.ioctl(self._ser.fileno(), termios.TIOCOUTQ, struct.pack("I", 0)) + return struct.unpack("I", raw)[0] + + def _read_reply(self, expect: int, deadline: float) -> bytes: + """读应答。单次 ``read`` 不许越过 ``deadline``。 + + pyserial 的超时在 USB 卡住时不可靠；``select`` 到点就停。 + """ + fd = self._ser.fileno() + buf = bytearray() + while time.monotonic() < deadline and len(buf) < expect: + remain = deadline - time.monotonic() + ready, _, _ = select.select([fd], [], [], remain) + if not ready: + break + old = self._ser.timeout + self._ser.timeout = 0 + try: + chunk = self._ser.read(expect - len(buf)) + finally: + self._ser.timeout = old + if not chunk: + break + buf.extend(chunk) + return bytes(buf) + def initialization(self) -> bool: return self.write_register(REG_INIT, INIT_CMD) diff --git a/src/arm_application/arm_application/dh_pgea_node.py b/src/arm_application/arm_application/dh_pgea_node.py index 990e398..89297f2 100644 --- a/src/arm_application/arm_application/dh_pgea_node.py +++ b/src/arm_application/arm_application/dh_pgea_node.py @@ -154,6 +154,11 @@ class DhPgeaNode(TolerantNode): def _on_ctrl(self, msg: Float32MultiArray) -> None: initialize, position, force, speed = _as_ctrl_array(msg) + # 写在执行之前。串口堵住时，只有这一行能看出命令是何时进来的。 + self.get_logger().info( + f"收到夹爪命令 initialize={initialize:.0f} pos={position:.0f} " + f"force={force:.0f} speed={speed:.0f}" + ) try: if initialize > 0.5: self._grip.initialize() diff --git a/src/arm_application/arm_application/pipeline_node.py b/src/arm_application/arm_application/pipeline_node.py index 374d37b..d0caa61 100644 --- a/src/arm_application/arm_application/pipeline_node.py +++ b/src/arm_application/arm_application/pipeline_node.py @@ -75,6 +75,27 @@ def a7_move_duration_s(delta_rad: float, speed_deg_s: float) -> float: # 合爪位置命令是 0。和 ``pos`` 的到位带同一宽度。 CLOSE_POS_TOL = 40.0 +#: 首轮咬合结束之后，第三步整段重复的次数。 +#: +#: 一轮 = 张开到 ``unwind_open_permille``、微抬 ``unwind_lift_mm``、逆时针到限位、 +#: 按本轮实测上抬量降回、合爪、顺时针到限位。抬和降都在轮内，不许拿到循环外面 +#: 只做一次。固定 4，不进 site.yaml：这是工序次数，不是现场标定值。 +UNWIND_ROUNDS = 4 + + +def unwind_round_label(index: int, total: int = UNWIND_ROUNDS) -> str: + """第几轮的日志前缀。``index`` 从 1 计到 ``total``。 + + 越界直接抛错。打出「第 0/4 轮」或「第 5/4 轮」会让人以为流程多走或少走了。 + """ + n = int(total) + i = int(index) + if n != UNWIND_ROUNDS or not 1 <= i <= n: + raise ValueError( + f"空回轮次只能是 1–{UNWIND_ROUNDS}（固定 {UNWIND_ROUNDS} 轮），" + f"收到 index={index} total={total}" + ) + return f"空回第{i}/{n}轮" def close_ready(grip: int, pos: float, tol: float = CLOSE_POS_TOL) -> bool: @@ -461,26 +482,35 @@ class AssemblyPipeline(Stage3Search): return False script = ros2_root() / "scripts" / "run_gripper.sh" cmd = ["bash", str(script), "--arm", short, "--cli", action, *extra] - self.get_logger().info("夹爪：" + " ".join(cmd)) - try: - proc = subprocess.run( - cmd, capture_output=True, text=True, timeout=30, check=False - ) - except subprocess.TimeoutExpired: - self.get_logger().error(f"夹爪 {action} 30s 没有返回，A7 不再转") - return False - text = ((proc.stdout or "") + "\n" + (proc.stderr or "")).strip() - if text: - tail = " | ".join(text.splitlines()[-4:]) - self.get_logger().info(f"夹爪脚本：{tail}") - if proc.returncode != 0: - self.get_logger().error( - f"夹爪 {action} 失败 exit={proc.returncode}，A7 不再转" - ) - return False - return self._wait_gripper(action, value) + # 第一次状态没跟上就再发一次。2026-10-20 合爪命令已发布，节点被串口 + # flush 堵住 120s 才执行，8s 时手指还停在张开位，这一轮就停了。 + for attempt in (1, 2): + self.get_logger().info("夹爪：" + " ".join(cmd)) + try: + proc = subprocess.run( + cmd, capture_output=True, text=True, timeout=30, check=False + ) + except subprocess.TimeoutExpired: + self.get_logger().error(f"夹爪 {action} 30s 没有返回，A7 不再转") + return False + text = ((proc.stdout or "") + "\n" + (proc.stderr or "")).strip() + if text: + tail = " | ".join(text.splitlines()[-4:]) + self.get_logger().info(f"夹爪脚本：{tail}") + if proc.returncode != 0: + self.get_logger().error( + f"夹爪 {action} 失败 exit={proc.returncode}，A7 不再转" + ) + return False + if self._wait_gripper(action, value, final=(attempt == 2)): + return True + if attempt == 1: + self.get_logger().warn(f"夹爪 {action} 状态没跟上，再发一次命令") + return False - def _wait_gripper(self, action: str, value: str = "", timeout_s: float = 8.0) -> bool: + def _wait_gripper( + self, action: str, value: str = "", timeout_s: float = 8.0, *, final: bool = True + ) -> bool: """``/gripper/states``：``pos`` 要停在目标附近；合爪见 :func:`close_ready`。 离线帧（第 6 位为 0，前 5 个是上一次的值）不算数：读不到总线时 @@ -524,11 +554,13 @@ class AssemblyPipeline(Stage3Search): return True finally: self.destroy_subscription(sub) - self.get_logger().error( - gripper_wait_timeout_reason( - action, timeout_s, last, saw_any=saw_any, saw_offline=saw_offline - ) + reason = gripper_wait_timeout_reason( + action, timeout_s, last, saw_any=saw_any, saw_offline=saw_offline ) + if final: + self.get_logger().error(reason) + else: + self.get_logger().warn(reason) return False def _a7_to_limit( @@ -546,10 +578,11 @@ class AssemblyPipeline(Stage3Search): return self._send_a7_only(float(target[6]), speed, tag) def _run_a7_seat(self) -> bool: - """贴面后咬螺纹。只转 A7，不改法兰位姿。 + """贴面后咬螺纹。只转 A7，法兰 XY 不动。 前两步是首轮：逆时针到限位，再顺时针到限位。顺时针到位即首轮咬合结束。 - 第三步松爪、逆时针回限位、合爪、再顺时针到限位。 + 第三步整段固定重复 :data:`UNWIND_ROUNDS` 轮。每一轮自己抬、自己记住、 + 自己降回；某一轮失败就停，后面的轮次不再做。 """ limits = self._a7_limits() if limits is None: @@ -560,6 +593,7 @@ class AssemblyPipeline(Stage3Search): ccw = int(ga["extreme_direction"]) lift_mm = float(ga["unwind_lift_mm"]) open_permille = int(ga["unwind_open_permille"]) + seat_cw_deg_s = float(ga["extreme_speed_deg_s"]) except (KeyError, TypeError, ValueError) as exc: self.get_logger().error( f"site.yaml gauge_assembly 缺 A7 旋转参数（{exc}），拒绝旋转" @@ -580,51 +614,82 @@ class AssemblyPipeline(Stage3Search): f"unwind_open_permille 只接受 1–1000，收到 {open_permille}，拒绝旋转" ) return False - # extreme_direction 是逆时针的符号。第一步出孔，第二步顺时针进孔。 - # 四段 A7 都走接口上限。样条峰速由 a7_move_duration_s 压在 0.90 rad/s。 - first = ( - (ccw, "A7 逆时针至极限（出孔）"), - (-ccw, "A7 顺时针至极限（进孔）"), - ) - for direction, tag in first: - if not self._a7_to_limit(limits, direction, margin, A7_MAX_SPEED_DEG_S, tag): - return False + if not (0.0 < seat_cw_deg_s <= 90.0): + self.get_logger().error( + f"extreme_speed_deg_s 只接受 0–90°/s，收到 {seat_cw_deg_s}，拒绝旋转" + ) + return False + # extreme_direction 是逆时针的符号。第一步出孔走接口上限，第二步顺时针 + # 进孔用 extreme_speed_deg_s（现场 10°/s）。空回轮次里的正反转仍是上限。 + if not self._a7_to_limit( + limits, ccw, margin, A7_MAX_SPEED_DEG_S, "A7 逆时针至极限（出孔）" + ): + return False + if not self._a7_to_limit( + limits, -ccw, margin, seat_cw_deg_s, "A7 顺时针至极限（进孔）" + ): + return False self._state("首轮咬合结束") self.get_logger().info("首轮咬合结束：A7 已顺时针转到限位。") - # 第三步：只张开到 unwind_open_permille，微抬 Z，最快逆时针空回，再降回同一段 Z，合爪，最快顺时针进。 - if not self._gripper_cli("pos", str(open_permille)): - return False - dz_up = self._shift_flange_z(lift_mm / 1000.0, "空回前微抬") - if dz_up is None: - return False - if dz_up < (lift_mm / 1000.0) * 0.6: - self.get_logger().error( - f"空回前微抬只做到 {dz_up * 1000:.2f}mm，不到指令 {lift_mm:.1f}mm 的六成，不空转" - ) - return False self.get_logger().info( - f"记住上抬 {dz_up * 1000:.2f}mm。空回后按这一段下降，XY 不动。" + f"第三步空回固定重复 {UNWIND_ROUNDS} 轮。每一轮：张开到 {open_permille}、" + f"微抬 {lift_mm:.1f}mm、逆时针到限位、按本轮记住的上抬量降回、合爪、顺时针到限位。" ) - if not self._a7_to_limit( - limits, ccw, margin, A7_MAX_SPEED_DEG_S, "A7 逆时针至极限（松爪回转）" - ): - return False - dz_down = self._shift_flange_z(-dz_up, "空回后降回") - if dz_down is None: - return False - if abs(dz_down + dz_up) > 0.0005: - self.get_logger().error( - f"下降 {dz_down * 1000:.2f}mm 和记住的上抬 {dz_up * 1000:.2f}mm 不一致，不合爪" + # 抬和降必须在轮内：每轮的下降量用这一轮实测的上抬，不能用上一轮的数。 + for rnd in range(1, UNWIND_ROUNDS + 1): + label = unwind_round_label(rnd) + self._state(label) + self.get_logger().info(f"{label} 开始") + if not self._gripper_cli("pos", str(open_permille)): + self.get_logger().error(f"{label} 没走完，后面的轮次不再做") + return False + dz_up = self._shift_flange_z(lift_mm / 1000.0, f"{label} 空回前微抬") + if dz_up is None: + self.get_logger().error(f"{label} 没走完，后面的轮次不再做") + return False + if dz_up < (lift_mm / 1000.0) * 0.6: + self.get_logger().error( + f"{label} 空回前微抬只做到 {dz_up * 1000:.2f}mm，" + f"不到指令 {lift_mm:.1f}mm 的六成，不空转" + ) + self.get_logger().error(f"{label} 没走完，后面的轮次不再做") + return False + self.get_logger().info( + f"{label} 记住上抬 {dz_up * 1000:.2f}mm。本轮空回后按这一段下降，XY 不动。" + ) + if not self._a7_to_limit( + limits, ccw, margin, A7_MAX_SPEED_DEG_S, + f"{label} A7 逆时针至极限（松爪回转）", + ): + self.get_logger().error(f"{label} 没走完，后面的轮次不再做") + return False + dz_down = self._shift_flange_z(-dz_up, f"{label} 空回后降回") + if dz_down is None: + self.get_logger().error(f"{label} 没走完，后面的轮次不再做") + return False + if abs(dz_down + dz_up) > 0.0005: + self.get_logger().error( + f"{label} 下降 {dz_down * 1000:.2f}mm 和记住的上抬 " + f"{dz_up * 1000:.2f}mm 不一致，不合爪" + ) + self.get_logger().error(f"{label} 没走完，后面的轮次不再做") + return False + if not self._gripper_cli("close"): + self.get_logger().error(f"{label} 没走完，后面的轮次不再做") + return False + if not self._a7_to_limit( + limits, -ccw, margin, A7_MAX_SPEED_DEG_S, + f"{label} A7 顺时针至极限（夹紧再进）", + ): + self.get_logger().error(f"{label} 没走完，后面的轮次不再做") + return False + self.get_logger().info( + f"{label} 完成：张开到 {open_permille}、上抬 {dz_up * 1000:.2f}mm、" + f"下降 {dz_down * 1000:.2f}mm、合爪、顺时针到限位。" + f"进度 {rnd}/{UNWIND_ROUNDS}。" ) - return False - if not self._gripper_cli("close"): - return False - if not self._a7_to_limit( - limits, -ccw, margin, A7_MAX_SPEED_DEG_S, "A7 顺时针至极限（夹紧再进）" - ): - return False self.get_logger().info( - f"A7 第三步完成：张开到 {open_permille}、微抬、最快逆时针回限位、降回同一段、合爪、顺时针到限位。" + f"空回 {UNWIND_ROUNDS}/{UNWIND_ROUNDS} 轮全部完成。" ) return True @@ -855,9 +920,12 @@ class AssemblyPipeline(Stage3Search): self._pin(q) self.get_logger().info( "插孔已停在贴面高度，不进搜孔、不抬回悬高。" - "接着只转 A7：先逆时针到限位，再顺时针到限位（首轮咬合），都用接口最快。" - "然后平行爪只张开到指定开度、微抬 Z、最快逆时针回限位、" - "按记住的上抬量降回、合爪、再最快顺时针到限位。" + "接着只转 A7：先逆时针到限位（接口最快），再顺时针到限位" + "（首轮咬合，用 extreme_speed_deg_s）。" + f"然后下面这一整段固定重复 {UNWIND_ROUNDS} 轮：" + "平行爪只张开到指定开度、微抬 Z、最快逆时针回限位、" + "按本轮记住的上抬量降回、合爪、再最快顺时针到限位。" + "某一轮没走完就停，后面的轮次不再做。" ) if not self._run_a7_seat(): self.get_logger().error( diff --git a/src/arm_application/test/test_dh_pgea_latch.py b/src/arm_application/test/test_dh_pgea_latch.py index 4fbebf0..82f4e76 100644 --- a/src/arm_application/test/test_dh_pgea_latch.py +++ b/src/arm_application/test/test_dh_pgea_latch.py @@ -26,6 +26,7 @@ from arm_application.dh_pgea import ( DhPgeaGripper, grip_motion_settled, latched_position_break, + wait_tx_empty, ) @@ -121,6 +122,39 @@ class _DeadSerial: raise OSError(5, "Input/output error") +def test_tx_drain_stops_at_the_deadline(): + """发送缓冲一直不空时必须在截止时间返回，不能改回会堵住 120s 的 tcdrain。""" + import time + + t0 = time.monotonic() + ok = wait_tx_empty(lambda: 8, time.monotonic() + 0.05, pause=lambda _s: None) + assert ok is False + assert time.monotonic() - t0 < 0.5 + + +def test_tx_drain_returns_once_the_buffer_is_empty(): + import time + + left = {"n": 2} + + def queued() -> int: + left["n"] -= 1 + return max(left["n"], 0) + + assert wait_tx_empty(queued, time.monotonic() + 1.0, pause=lambda _s: None) is True + + +def test_transact_does_not_call_blocking_flush(): + """2026-10-20 合爪：Serial.flush/tcdrain 堵住 120s，流水线 8s 时手指还在 400。""" + src = ( + Path(__file__).resolve().parents[1] / "arm_application" / "dh_pgea.py" + ).read_text(encoding="utf-8") + body = src[src.find("def _transact") : src.find("def initialization")] + assert ".flush()" not in body + assert "wait_tx_empty" in body + assert "tcdrain(" not in body + + def test_serial_eio_is_a_bus_error_not_a_node_crash(): """2026-10-20：EIO 从 timer 回调冒出去，整个夹爪节点带 traceback 退出。 diff --git a/src/arm_application/test/test_pipeline_stage1_diag.py b/src/arm_application/test/test_pipeline_stage1_diag.py index eb091af..d1eae1f 100644 --- a/src/arm_application/test/test_pipeline_stage1_diag.py +++ b/src/arm_application/test/test_pipeline_stage1_diag.py @@ -18,6 +18,7 @@ import pytest from arm_application.overhead_oneshot import D435_MOVEL_GAVE_UP from arm_application.pipeline_node import ( + UNWIND_ROUNDS, close_ready, gripper_wait_timeout_reason, hold_at_insert_z, @@ -25,6 +26,7 @@ from arm_application.pipeline_node import ( stage1_log_fatal, stage1_timeout_reason, track_lock_z, + unwind_round_label, ) @@ -81,6 +83,22 @@ def test_fast_a7_duration_keeps_spline_peak_under_the_drive_cap(): assert a7_move_duration_s(slow, 10.0) == pytest.approx(abs(slow) / math.radians(10.0)) +def test_unwind_is_four_rounds_and_labels_cannot_lie(): + """首轮咬合之后第三步整段固定 4 轮。轮次标签越界必须红，不能打出第 0 轮或第 5 轮。""" + assert UNWIND_ROUNDS == 4 + assert unwind_round_label(1) == "空回第1/4轮" + assert unwind_round_label(4) == "空回第4/4轮" + for bad in (0, 5, -1): + with pytest.raises(ValueError): + unwind_round_label(bad) + with pytest.raises(ValueError): + unwind_round_label(1, total=3) + root = Path(__file__).resolve().parents[3] + hb = (root / "docs" / "HANDBOOK.md").read_text(encoding="utf-8") + assert f"固定重复 {UNWIND_ROUNDS} 轮" in hb + assert "UNWIND_ROUNDS" in hb + + def test_insert_hold_does_not_return_to_hover(): """贴面后停住。三阶段开局会抬回悬高；没有传感器，不另设一个 Z。""" assert hold_at_insert_z({"insert_held": True}, skip3=False) is True @@ -116,7 +134,28 @@ def test_insert_hold_does_not_return_to_hover(): assert all(i >= 0 for i in pos) assert pos == sorted(pos) assert seat.count("A7_MAX_SPEED_DEG_S") == 3 - assert "extreme_speed_deg_s" not in seat + # 首轮：出孔用上限，顺时针进孔用 site 的 extreme_speed_deg_s（现场 10°/s）。 + # 空回循环里的正反转仍是上限，不能把 10°/s 写进循环。 + assert 'ga["extreme_speed_deg_s"]' in seat + first = seat[ + seat.find("第一步出孔走接口上限") : seat.find("首轮咬合结束：A7 已顺时针转到限位") + ] + assert "A7_MAX_SPEED_DEG_S" in first + assert "seat_cw_deg_s" in first + assert first.find("A7_MAX_SPEED_DEG_S") < first.find("seat_cw_deg_s") + loop = seat[seat.find("for rnd in range(1, UNWIND_ROUNDS + 1)") :] + assert "seat_cw_deg_s" not in loop + assert loop.count("A7_MAX_SPEED_DEG_S") == 2 + # 抬和降写在循环里面，各一处。循环外再写一份就会有的轮次不抬。 + loop_at = seat.find("for rnd in range(1, UNWIND_ROUNDS + 1)") + assert loop_at > seat.find("首轮咬合结束") + # 抬、降、开、合都在循环里，而且各只调用一次（靠循环重复，不在循环外再写一份）。 + assert seat.find("_shift_flange_z(lift_mm") > loop_at + assert seat.count("_shift_flange_z(lift_mm") == 1 + assert seat.count('_shift_flange_z(-dz_up') == 1 + assert seat.count('_gripper_cli("pos"') == 1 + assert seat.count('_gripper_cli("close")') == 1 + assert "没走完，后面的轮次不再做" in seat grip = pipe[pipe.find("def _gripper_cli") : pipe.find("def _wait_gripper")] assert "run_gripper.sh" in grip assert '"--cli"' in grip