diff --git a/docs/GRIPPER.md b/docs/GRIPPER.md index f1f84c7..0eae70f 100644 --- a/docs/GRIPPER.md +++ b/docs/GRIPPER.md @@ -67,4 +67,17 @@ ros2 service call /dual_gripper_driver/right/execute arm_msgs/srv/GripperExecute USB 未插齐时节点仍可起来（`allow_offline:=true`），对应侧 `status.error_msg` 会写连接失败。 +## `/gripper/states` 的离线帧 + +`data` 是 6 个元素： +`[initialized, grip_state, position, target_pos, target_force, online]`。 + +`online=0` 表示这一帧**没读到总线**，前 5 个是上一次读到的值，不是现在的状态。老节点只发 5 个元素（那时它只在读成功时才发），5 元素帧按有效帧处理。 + +读 `0x0201` 无应答时节点照样发离线帧，`/gripper/joint_states` **不发**：关节状态没有离线位，宁可不发，也不要把上一次的开口当成现在的。 + +流水线 `_wait_gripper` 只认有效帧；超时没等到有效帧时报「读不到状态（只有离线帧，Modbus 无应答），不是夹爪没张开」，不能再报成「夹爪没到位」——2026-10-20 读 `0x0201` 一连 264 次无应答，实测在张开后停住、微抬没有下发，日志却只写「未到位」。 + +串口 `EIO`（USB 掉线）抛 `DhPgeaBusError`，节点先关串口再重开（1 秒最多一次），不再整进程带 traceback 退出。没有应答（`DhPgeaError`）先继续重试；连续 `BUS_REOPEN_AFTER_FAILURES=30` 拍都读不到也重开一次——2026-10-20 那次就是写有回显、读一直无应答，重启节点立刻恢复，说明坏的是串口会话。 + 用户需在 `dialout`：`sudo usermod -aG dialout $USER`。 diff --git a/src/arm_application/arm_application/dh_pgea.py b/src/arm_application/arm_application/dh_pgea.py index 117d46a..6c437ea 100644 --- a/src/arm_application/arm_application/dh_pgea.py +++ b/src/arm_application/arm_application/dh_pgea.py @@ -204,6 +204,22 @@ class DhPgeaError(RuntimeError): pass +class DhPgeaBusError(DhPgeaError): + """串口本身坏了（EIO / 设备被拔 / USB 掉线），不是夹爪没应答。 + + 2026-10-20 之前 ``_transact`` 让 ``termios.error: (5, 'Input/output error')`` + 直接冒到 timer 回调外，整个夹爪节点带着 traceback 退出。这两种失败必须分开： + 没应答可以继续重试；串口坏了要先关再重开。 + """ + + +#: 连续这么多拍读不到状态就重开串口。 +#: 2026-10-20 实测：写命令有回显（``ctrl pos=400`` 有记录、手指也动了）， +#: 读 ``0x0201`` 却连续 264 次无应答；重启节点立刻恢复。说明串口会话坏了， +#: 光在同一个 fd 上重试没用。按失败拍数（不是秒）计数，是因为每拍耗时随重试变长。 +BUS_REOPEN_AFTER_FAILURES = 30 + + WIRING_HINT = ( "PC 已发出 Modbus，夹爪 0 字节返回。不是力/速度参数问题。按顺序查：\n" " 1) 夹爪 24 V DC 已上电（指示灯亮）\n" @@ -362,21 +378,27 @@ class DhPgeaGripper: if self._ser is None or not self._ser.is_open: raise DhPgeaError("串口未打开") bit_s = 10.0 / max(1, self.baudrate) - self._set_tx(True) - if self.rs485_rts: - time.sleep(2.0 * bit_s) - self._ser.reset_input_buffer() - self._ser.write(frame) - self._ser.flush() - 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) + try: + self._set_tx(True) + if self.rs485_rts: + time.sleep(2.0 * bit_s) + self._ser.reset_input_buffer() + self._ser.write(frame) + self._ser.flush() + 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) + except OSError as exc: + raise DhPgeaBusError( + f"{self.port} 串口读写失败（{exc}）。" + "设备被拔掉或 USB 掉线时要先关串口再重开，不是夹爪没应答。" + ) from exc return bytes(buf) if buf else None def initialization(self) -> bool: diff --git a/src/arm_application/arm_application/dh_pgea_node.py b/src/arm_application/arm_application/dh_pgea_node.py index 5c43870..990e398 100644 --- a/src/arm_application/arm_application/dh_pgea_node.py +++ b/src/arm_application/arm_application/dh_pgea_node.py @@ -6,8 +6,9 @@ data: [initialize, position, force, speed] initialize>0.5 → 写 0xA5；否则设置力/速度/位置 /gripper/states std_msgs/Float32MultiArray - data: [initialized, grip_state, position, target_pos, target_force] - /gripper/joint_states sensor_msgs/JointState + data: [initialized, grip_state, position, target_pos, target_force, online] + online=1 这一帧刚从总线读回来；0=读不到（前 5 个是上一次的值） + /gripper/joint_states sensor_msgs/JointState（只在读到状态时发） 服务（std_srvs/Trigger）： /gripper/initialize /gripper/open /gripper/close @@ -18,6 +19,7 @@ from __future__ import annotations import threading +import time from typing import Optional import rclpy @@ -28,11 +30,14 @@ from std_msgs.msg import Float32, Float32MultiArray, MultiArrayDimension, MultiA from std_srvs.srv import Trigger from arm_application.dh_pgea import ( + BUS_REOPEN_AFTER_FAILURES, + DhPgeaBusError, DhPgeaError, DhPgeaGripper, GripperState, WIRING_HINT, ) +from arm_application.gauge_gripper import STATES_ONLINE_INDEX from arm_application.ros_node import TolerantNode, param_as_str @@ -84,6 +89,8 @@ class DhPgeaNode(TolerantNode): ) self._state = GripperState(stroke_mm=self._stroke) self._cmd_lock = threading.Lock() + self._last_reopen = 0.0 + self._fail_streak = 0 self._pub_state = self.create_publisher(Float32MultiArray, "/gripper/states", 10) self._pub_js = self.create_publisher(JointState, "/gripper/joint_states", 10) @@ -157,14 +164,14 @@ class DhPgeaNode(TolerantNode): info = self._apply_move(int(position), f, s) self.get_logger().info(f"ctrl {info}") except DhPgeaError as exc: - self.get_logger().error(str(exc)) + self._note_bus_error(exc) def _on_set_pos(self, msg: Float32) -> None: try: info = self._apply_move(int(msg.data)) self.get_logger().info(f"set_position {info}") except DhPgeaError as exc: - self.get_logger().error(str(exc)) + self._note_bus_error(exc) def _srv_init(self, _req, resp): try: @@ -172,6 +179,7 @@ class DhPgeaNode(TolerantNode): resp.success = True resp.message = self._grip.get_run_states().summary() except DhPgeaError as exc: + self._note_bus_error(exc) resp.success = False resp.message = str(exc) return resp @@ -182,6 +190,7 @@ class DhPgeaNode(TolerantNode): resp.success = True resp.message = info except DhPgeaError as exc: + self._note_bus_error(exc) resp.success = False resp.message = str(exc) return resp @@ -192,30 +201,87 @@ class DhPgeaNode(TolerantNode): resp.success = True resp.message = info except DhPgeaError as exc: + self._note_bus_error(exc) resp.success = False resp.message = str(exc) return resp + def _reopen_serial(self) -> None: + """串口 EIO 后先关再重开。1 秒最多一次，避免把 USB 打爆。""" + now = time.monotonic() + if now - self._last_reopen < 1.0: + return + self._last_reopen = now + try: + self._grip.close() + except Exception: + pass + try: + self._grip.open() + self._grip.probe() + except (DhPgeaError, OSError) as exc: + self.get_logger().error( + f"夹爪串口重开失败：{exc}", throttle_duration_sec=10.0 + ) + return + self._online = True + self.get_logger().info("夹爪串口已重开，总线恢复应答") + + def _note_bus_error(self, exc: Exception) -> None: + """命令回调里的失败也走同一套：标离线，串口坏了就重开。""" + self._online = False + self.get_logger().error(str(exc)) + if isinstance(exc, DhPgeaBusError): + self._reopen_serial() + def _on_timer(self) -> None: + online = True try: st = self._grip.get_run_states(retries=2 if self._online else 1) self._state = st + self._fail_streak = 0 if not self._online: self._online = True self.get_logger().info("夹爪总线已恢复") + except DhPgeaBusError as exc: + self._online = False + self._fail_streak = 0 + self.get_logger().warn(str(exc), throttle_duration_sec=5.0) + self._reopen_serial() + online = False except DhPgeaError as exc: self._online = False + self._fail_streak += 1 self.get_logger().warn(str(exc), throttle_duration_sec=5.0) - return + if self._fail_streak >= BUS_REOPEN_AFTER_FAILURES: + # 写有回显、读一直无应答时，重试同一个串口会话没用，必须重开。 + self.get_logger().warn( + f"连续 {self._fail_streak} 拍读不到状态，先重开串口再继续重试" + ) + self._fail_streak = 0 + self._reopen_serial() + online = False + self._publish_state(online) + def _publish_state(self, online: bool) -> None: + """离线也要发一帧，前 5 个是上一次的值、最后一位是 0。 + + 不发的话上层只看到"没有 /gripper/states"，会把"总线读不到"当成 + "夹爪没到位"。只有 5 个元素的老消费者照旧能用。 + """ + st = self._state + n = STATES_ONLINE_INDEX + 1 arr = Float32MultiArray() arr.layout = MultiArrayLayout( - dim=[MultiArrayDimension(label="dh_pgea", size=5, stride=5)], + dim=[MultiArrayDimension(label="dh_pgea", size=n, stride=n)], data_offset=0, ) - arr.data = list(st.as_tuple()) + arr.data = list(st.as_tuple()) + [1.0 if online else 0.0] self._pub_state.publish(arr) + # 关节状态没有"离线"位：宁可不发，也不要把上一次的开口当成现在的。 + if not online: + return js = JointState() js.header.stamp = self.get_clock().now().to_msg() js.name = ["gripper_finger1_joint"] diff --git a/src/arm_application/arm_application/gauge_config.py b/src/arm_application/arm_application/gauge_config.py index 5bc0bea..ed06698 100644 --- a/src/arm_application/arm_application/gauge_config.py +++ b/src/arm_application/arm_application/gauge_config.py @@ -52,6 +52,7 @@ _UNIT_SUFFIX: Dict[str, str] = { "m": "_m", "px": "_px", "pct": "_pct", + "permille": "_permille", "au": "_delta", # 腕部 FT 的**真牛顿**。与 au 分开一个后缀是刻意的：两个口径混用不会报错， # 只会"判据不触发"或"一碰就炸"，看起来像算法问题（见 site.yaml 的注释）。 @@ -119,6 +120,9 @@ _TYPES: Dict[str, type] = { "holes_total": int, "dry_run": bool, "reload_timeout_s": float, + # 首次咬合之后的空回：松爪开度 + 微抬量（见 pipeline_node._run_a7_seat） + "unwind_open_permille": int, + "unwind_lift_mm": float, } #: 每个键的单位。``""`` 表示无量纲（须同时登记在 :data:`_UNITLESS`）。 @@ -168,6 +172,8 @@ _UNITS: Dict[str, str] = { "holes_total": "", "dry_run": "", "reload_timeout_s": "s", + "unwind_open_permille": "permille", + "unwind_lift_mm": "mm", } #: 键名后缀与单位必须一致（防"填毫米当米用"）。``contact_speed_m_s`` 这类 @@ -273,6 +279,10 @@ DEFAULTS: Dict[str, Any] = { # E1（掉件）重放：等操作员放好并确认 /gauge/reload_ok 的最长时间。 # 超时 = 中止（臂在 home，人不在旁边时"继续等"比"停"更危险）。 "reload_timeout_s": 180.0, + # 首轮咬合之后的空回：只张开到这个开度（约 8mm），微抬这么多 Z 再转回去。 + # 实际下发在 pipeline_node._run_a7_seat（0–5mm、1–1000‰ 由那边也会再挡一次）。 + "unwind_open_permille": 400, + "unwind_lift_mm": 3.0, } @@ -321,6 +331,8 @@ class GaugeConfig: holes_total: int dry_run: bool reload_timeout_s: float + unwind_open_permille: int + unwind_lift_mm: float # -- 派生量（不占 YAML 键，免得两处各写一份，规则 M2）------------------- @property @@ -378,6 +390,8 @@ class GaugeConfig: f"{self.total_screw_deg:.0f}°；停止判据「切向 >+{self.screw_shear_delta:.0f} 且 " f"法向 >+{self.screw_normal_delta:.0f}」", f" S09 拧出 {self.unscrew_cycles} 轮（比拧入多 {self.unscrew_extra_cycles} 轮）", + f" 首轮咬合后空回：松爪到 {self.unwind_open_permille}‰、微抬 Z " + f"{self.unwind_lift_mm:.1f}mm，转回后按实测降回", ] @@ -511,6 +525,16 @@ def _sanity(cfg: GaugeConfig) -> None: ) if cfg.snap_tol_px <= 0.0: raise GaugeConfigError(f"gauge_assembly.snap_tol_px 必须为正，收到 {cfg.snap_tol_px}") + if not 0.0 <= cfg.unwind_lift_mm <= 5.0: + raise GaugeConfigError( + f"gauge_assembly.unwind_lift_mm 只接受 0–5mm 的微抬，" + f"收到 {cfg.unwind_lift_mm}。空回只需要让规离开孔口，抬多了会撞上方" + ) + if not 1 <= cfg.unwind_open_permille <= 1000: + raise GaugeConfigError( + f"gauge_assembly.unwind_open_permille 只接受 1–1000，" + f"收到 {cfg.unwind_open_permille}（0=合，1000=全开）" + ) if cfg.contact_relief_mm < 0.0: raise GaugeConfigError( f"gauge_assembly.contact_relief_mm 不能为负（0 = 接触到之后不卸载），" diff --git a/src/arm_application/arm_application/gauge_gripper.py b/src/arm_application/arm_application/gauge_gripper.py index 116875c..efcf87c 100644 --- a/src/arm_application/arm_application/gauge_gripper.py +++ b/src/arm_application/arm_application/gauge_gripper.py @@ -45,6 +45,13 @@ CONTRACT_CLOSE = "/gripper/close" CONTRACT_STATUS_TOPIC = "/gripper/status" LEGACY_STATUS_TOPIC = "/gripper/states" +#: ``/gripper/states`` 第 6 个元素：1=这一帧刚从总线读回来；0=总线无应答， +#: 前 5 个是上一次读到的值。老节点只发 5 个元素 —— 那时它只在读成功才发， +#: 所以 5 元素帧按有效帧处理。 +#: 为什么要这一位：2026-10-20 读 0x0201 一连 264 次无应答，节点干脆不发状态， +#: 流水线只看到「没有 /gripper/states」，把「总线读不到」显示成「夹爪没到位」。 +STATES_ONLINE_INDEX = 5 + #: DH 状态字（与 ``arm_msgs/GripperStatus.msg`` 里的注释一致）。 GRIP_STATE_MOVING = 0 GRIP_STATE_REACHED = 1 @@ -199,14 +206,30 @@ def is_dropped_from_state(grip_state: int, was_gripped: bool) -> bool: return was_gripped and grip_state == GRIP_STATE_DROPPED -def legacy_states_to_fields(data: Sequence[float]) -> Dict[str, float]: - """``/gripper/states`` 的 5 元素数组 → 具名字段。 +def states_frame_online(data: Sequence[float]) -> bool: + """这一帧是不是刚从总线读回来的。 - 超出长度或为空时返回空字典（而不是抛异常）：这是**状态话题**， - 收到畸形数据时应该让上层走"不知道"分支，而不是让整个节点崩掉。 + 5 元素帧（老节点）视为有效：老节点只在读成功时发。6 元素帧看最后一位。 + """ + if len(data) <= STATES_ONLINE_INDEX: + return True + try: + return float(data[STATES_ONLINE_INDEX]) >= 0.5 + except (TypeError, ValueError): + return False + + +def legacy_states_to_fields(data: Sequence[float]) -> Dict[str, float]: + """``/gripper/states`` 的 6 元素数组 → 具名字段。 + + 超出长度、为空、或这一帧是离线帧时返回空字典（而不是抛异常）：这是 + **状态话题**，收到畸形数据时应该让上层走"不知道"分支，而不是让整个 + 节点崩掉。离线帧里的前 5 个是上一次的值，不能当现在的状态用。 """ if not data or len(data) < 5: return {} + if not states_frame_online(data): + return {} return { "initialized": float(data[0]), "grip_state": float(data[1]), diff --git a/src/arm_application/arm_application/operator_gripper_state.py b/src/arm_application/arm_application/operator_gripper_state.py index b31d263..5cd8ade 100644 --- a/src/arm_application/arm_application/operator_gripper_state.py +++ b/src/arm_application/arm_application/operator_gripper_state.py @@ -392,8 +392,15 @@ def _parse_states_echo(text: str) -> dict | None: init, grip, pos, _target, force = (float(v) for v in raw[:5]) except (TypeError, ValueError): continue + # 第 6 位是"这一帧刚从总线读回来吗"。老 5 元素帧只在读成功时发，按在线算。 + online = True + if len(raw) > 5: + try: + online = float(raw[5]) >= 0.5 + except (TypeError, ValueError): + online = False return { - "connected": True, + "connected": online, "initialized": init >= 0.5, "permille": pos, "rotation_deg": 0.0, @@ -401,9 +408,9 @@ def _parse_states_echo(text: str) -> dict | None: "speed": None, "grip_state": int(grip), "rotation_state": None, - "moving": int(grip) == 0, - "gripped": int(grip) == 2, - "error_msg": "", + "moving": online and int(grip) == 0, + "gripped": online and int(grip) == 2, + "error_msg": "" if online else "夹爪总线无应答：读不到状态，这里是上一次的值", } return None diff --git a/src/arm_application/arm_application/pipeline_node.py b/src/arm_application/arm_application/pipeline_node.py index 79d21ab..374d37b 100644 --- a/src/arm_application/arm_application/pipeline_node.py +++ b/src/arm_application/arm_application/pipeline_node.py @@ -26,6 +26,7 @@ from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPo from std_msgs.msg import Bool, Float32MultiArray, String from trajectory_msgs.msg import JointTrajectory +from arm_application.gauge_gripper import states_frame_online from arm_application.gauge_kin import ( a7_limit_target, match_limits, @@ -89,6 +90,40 @@ def close_ready(grip: int, pos: float, tol: float = CLOSE_POS_TOL) -> bool: return g == 1 and float(pos) <= float(tol) +def gripper_wait_timeout_reason( + action: str, + timeout_s: float, + last: Optional[Tuple[int, float]], + *, + saw_any: bool, + saw_offline: bool, +) -> str: + """等夹爪状态超时后，到底该报哪一句。 + + 2026-10-20 三连实测都卡在这里：读 ``0x0201`` 一连 264 次无应答，节点 + 不再发状态，流水线把「总线读不到」显示成「夹爪没到位」，而臂停在原地 + 没做微抬。三种情况必须分开说，否则查的方向完全相反： + + * ``last`` 有值 → 真收到过有效帧，就是没到位； + * 只有离线帧 → 总线读不回来，不是没张开； + * 一帧都没有 → 节点没在发（崩了 / 没起 / 话题名不对）。 + """ + if last is not None: + return ( + f"夹爪 {action} 后 {timeout_s:.0f}s 未到位" + f"（grip={last[0]} pos={last[1]:.0f}），A7 不再转" + ) + if saw_offline: + return ( + f"夹爪 {action} 后 {timeout_s:.0f}s 读不到状态（/gripper/states " + "只有离线帧，Modbus 无应答），不是夹爪没张开。A7 不再转" + ) + return ( + f"夹爪 {action} 后 {timeout_s:.0f}s 一帧 /gripper/states 都没收到" + f"（节点没在发，saw_any={saw_any}），A7 不再转" + ) + + def init_z_refusal( z_m: float, tool_len_m: float, @@ -446,7 +481,11 @@ class AssemblyPipeline(Stage3Search): return self._wait_gripper(action, value) def _wait_gripper(self, action: str, value: str = "", timeout_s: float = 8.0) -> bool: - """``/gripper/states``：``pos`` 要停在目标附近；合爪见 :func:`close_ready`。""" + """``/gripper/states``：``pos`` 要停在目标附近；合爪见 :func:`close_ready`。 + + 离线帧（第 6 位为 0，前 5 个是上一次的值）不算数：读不到总线时 + 必须报"读不到状态"，不能报成"夹爪没到位"。 + """ box: dict = {"data": None} def _on_state(msg: Float32MultiArray) -> None: @@ -455,12 +494,18 @@ class AssemblyPipeline(Stage3Search): sub = self.create_subscription(Float32MultiArray, "/gripper/states", _on_state, 10) t0 = time.monotonic() last = None + saw_any = False + saw_offline = False try: while time.monotonic() - t0 < timeout_s: self._spin(0.05) data = box["data"] if not data or len(data) < 3: continue + saw_any = True + if not states_frame_online(data): + saw_offline = True + continue grip = int(data[1]) pos = float(data[2]) last = (grip, pos) @@ -479,9 +524,10 @@ class AssemblyPipeline(Stage3Search): return True finally: self.destroy_subscription(sub) - shown = f"grip={last[0]} pos={last[1]:.0f}" if last else "没有 /gripper/states" self.get_logger().error( - f"夹爪 {action} 后 {timeout_s:.0f}s 未到位（{shown}），A7 不再转" + gripper_wait_timeout_reason( + action, timeout_s, last, saw_any=saw_any, saw_offline=saw_offline + ) ) return False diff --git a/src/arm_application/arm_application/site_io.py b/src/arm_application/arm_application/site_io.py index 9243123..c16cb57 100644 --- a/src/arm_application/arm_application/site_io.py +++ b/src/arm_application/arm_application/site_io.py @@ -10,13 +10,83 @@ import yaml def ros2_root() -> Path: + """仓库根目录。先看环境变量，再从本文件向上找 ``config/site.yaml``。 + + 不能默认 ``~/ros2``：别人把仓库克隆到别的目录时，写死的家目录路径是空的， + 启动会报文件不存在。 + """ for key in ("SITE_ROS2_ROOT", "ROS2_WS"): v = os.environ.get(key, "").strip() if v: return Path(v).expanduser() + here = Path(__file__).resolve() + for parent in here.parents: + if (parent / "config" / "site.yaml").is_file() and (parent / "scripts" / "env.sh").is_file(): + return parent return Path.home() / "ros2" +_PATH_KEYS = frozenset({ + "named_poses_file", + "handeye_file", + "handeye_waypoints", + "handeye_workdir", + "handeye_calibrate", + "yolo_weights", + "hole_weights", + "weights", + "workpiece_weights", + "config_file", + "append_poses_file", +}) + + +def resolve_repo_path(value: str, root: Path | None = None) -> str: + """仓库内的相对路径展开成绝对路径。已经是绝对路径的原样保留。""" + text = str(value or "").strip() + if not text: + return "" + path = Path(text).expanduser() + if path.is_absolute(): + return str(path) + base = root or ros2_root() + return str((base / path).resolve()) + + +def _is_path_key(key: str) -> bool: + return ( + key in _PATH_KEYS + or key.startswith("handeye_file") + or key.startswith("handeye_waypoints") + or key.endswith("_weights") + or key.endswith("_file") + or key.endswith("_workdir") + ) + + +def absolutize_site_paths(data: Any, root: Path | None = None) -> Any: + """把 site 里的仓库相对路径展开。只动路径字段，不动别的字符串。""" + base = root or ros2_root() + if isinstance(data, dict): + out: dict[Any, Any] = {} + for key, val in data.items(): + name = str(key) + if isinstance(val, str) and _is_path_key(name): + out[key] = resolve_repo_path(val, base) + elif ( + name == "config" + and isinstance(val, str) + and (val.endswith((".yaml", ".yml", ".json")) or "/" in val) + ): + out[key] = resolve_repo_path(val, base) + else: + out[key] = absolutize_site_paths(val, base) + return out + if isinstance(data, list): + return [absolutize_site_paths(item, base) for item in data] + return data + + def site_path() -> Path: env = os.environ.get("SITE_CONFIG", "").strip() if env: @@ -24,12 +94,90 @@ def site_path() -> Path: return ros2_root() / "config" / "site.yaml" +def arm_short_id(sn: str) -> str: + """序列号末两位就是臂号（``Rizon4-063989`` → ``89``）。 + + 和 ``scripts/lib/export_site_env.arm_short_id`` 同一条规则。换一台臂 + 只改 ``site.yaml`` 的 SN，不要在界面里再写一张本机对照表。 + """ + tail = str(sn or "").strip().rsplit("-", 1)[-1] + return tail[-2:] if tail.isdigit() and len(tail) >= 2 else "" + + +def listed_arms(data: dict[str, Any] | None) -> list[dict[str, Any]]: + """``robots[]`` 里的真机臂。``sn: sim`` 不进控制。 + + 旋转爪只看夹爪型号里有没有 ``RGI``。右臂也可以是平行爪，不能靠臂号猜。 + """ + robots = ((data or {}).get("robot") or {}).get("robots") or [] + out: list[dict[str, Any]] = [] + seen: set[str] = set() + for robot in robots: + if not isinstance(robot, dict): + continue + sn = str(robot.get("sn") or "").strip() + if not sn or sn.lower() == "sim": + continue + arm_no = arm_short_id(sn) + if not arm_no: + continue + if arm_no in seen: + raise ValueError(f"两台臂的序列号末两位都是 {arm_no}，--arm 分不清") + seen.add(arm_no) + side = str(robot.get("arm") or "").strip().lower() + grip = robot.get("gripper") if isinstance(robot.get("gripper"), dict) else {} + model = str(grip.get("model") or "") + shared = (data or {}).get("gripper") if isinstance((data or {}).get("gripper"), dict) else {} + stroke = grip.get("stroke_mm") + if stroke is None: + stroke = shared.get("stroke_mm") or 20.0 + out.append({ + "id": arm_no, + "label": str(robot.get("label") or "").strip() or f"臂{arm_no}", + "sn": sn, + "side": side if side in ("left", "right") else "", + "tactile": bool(robot.get("tactile")), + "camera": str((robot.get("camera") or {}).get("name") or "") if isinstance(robot.get("camera"), dict) else "", + "gripper": model, + "rotary": "RGI" in model.upper(), + "iface": str(robot.get("iface") or ""), + "pc_ip": str(robot.get("pc_ip") or ""), + "port": str(grip.get("port") or ""), + "stroke_mm": float(stroke), + }) + return out + + +def usb_expectations(data: dict[str, Any] | None) -> dict[str, int]: + """设备页上「应有几台」跟 ``site.yaml`` 走，不写死本机的 2+1+2。""" + cameras = (data or {}).get("cameras") or {} + d405 = 0 + d435 = 0 + if isinstance(cameras, dict): + for key, val in cameras.items(): + model = str((val or {}).get("model") or "") if isinstance(val, dict) else "" + token = f"{key} {model}".lower() + if "d405" in token: + d405 += 1 + elif "d435" in token: + d435 += 1 + tactile = (data or {}).get("tactile") or {} + orisys = 0 + if isinstance(tactile, dict): + orisys = sum(1 for key in ("left_video", "right_video") if tactile.get(key) is not None) + return {"d405": d405, "d435": d435, "orisys": orisys} + + def load_site_dict(path: str | Path | None = None) -> dict[str, Any]: p = Path(path).expanduser() if path else site_path() if not p.is_file(): return {} data = yaml.safe_load(p.read_text(encoding="utf-8")) or {} - return data if isinstance(data, dict) else {} + if not isinstance(data, dict): + return {} + resolved = p.resolve() + root = resolved.parent.parent if resolved.parent.name == "config" else ros2_root() + return absolutize_site_paths(data, root) def deep_merge(base: dict[str, Any], overlay: dict[str, Any]) -> dict[str, Any]: diff --git a/src/arm_application/test/test_dh_pgea_latch.py b/src/arm_application/test/test_dh_pgea_latch.py index bd13fef..4fbebf0 100644 --- a/src/arm_application/test/test_dh_pgea_latch.py +++ b/src/arm_application/test/test_dh_pgea_latch.py @@ -16,11 +16,13 @@ from pathlib import Path from arm_application.dh_pgea import ( ctrl_values, open_failure_text, + REG_INIT_STATE, REG_POSITION, S_GRIP_ARRIVED, S_GRIP_CAUGHT, S_INIT_DOING, S_INIT_FINISHED, + DhPgeaBusError, DhPgeaGripper, grip_motion_settled, latched_position_break, @@ -110,6 +112,31 @@ def test_node_owned_port_uses_ctrl_topic_not_a_second_open() -> None: raise AssertionError("超范围位置应当失败") +class _DeadSerial: + """模拟 USB 掉线：读写抛 termios EIO，而不是"没有应答"。""" + + is_open = True + + def reset_input_buffer(self) -> None: + raise OSError(5, "Input/output error") + + +def test_serial_eio_is_a_bus_error_not_a_node_crash(): + """2026-10-20：EIO 从 timer 回调冒出去，整个夹爪节点带 traceback 退出。 + + 没应答（DhPgeaError）可以继续重试；串口坏了（DhPgeaBusError）必须先关了 + 重开。两者混在一起时，节点只会崩，流水线只看到"没有 /gripper/states"。 + """ + grip = DhPgeaGripper(port="/dev/null") + grip._ser = _DeadSerial() # type: ignore[assignment] + try: + grip.read_register(REG_INIT_STATE, retries=1) + except DhPgeaBusError as exc: + assert "串口" in str(exc) + else: + raise AssertionError("EIO 必须变成 DhPgeaBusError") + + def test_exclusive_lock_is_not_called_a_power_failure(): exc = OSError(11, "Could not exclusively lock port /dev/ttyUSB0: Resource temporarily unavailable") text = open_failure_text("/dev/ttyUSB0", exc) diff --git a/src/arm_application/test/test_gauge_gripper.py b/src/arm_application/test/test_gauge_gripper.py index 9af824e..7c21c35 100644 --- a/src/arm_application/test/test_gauge_gripper.py +++ b/src/arm_application/test/test_gauge_gripper.py @@ -27,6 +27,7 @@ from arm_application.gauge_gripper import ( is_gripped_from_state, legacy_states_to_fields, pick_backend, + states_frame_online, ) @@ -166,6 +167,19 @@ def test_legacy_states_parsing_is_field_named_and_defensive(): assert legacy_states_to_fields([]) == {} assert legacy_states_to_fields([1.0, 2.0]) == {} + # 离线帧（第 6 位 0）里的前 5 个是上一次的值，不能当现在的状态用。 + # 2026-10-20 读 0x0201 连 264 次无应答，这就是流水线把"读不到"说成 + # "夹爪没到位"的入口，必须有负向测试。 + assert legacy_states_to_fields([1.0, 2.0, 500.0, 0.0, 50.0, 0.0]) == {} + + +def test_states_online_marks_only_fresh_frames(): + assert states_frame_online([1.0, 2.0, 500.0, 0.0, 50.0]) is True + assert states_frame_online([1.0, 2.0, 500.0, 0.0, 50.0, 1.0]) is True + assert states_frame_online([1.0, 2.0, 500.0, 0.0, 50.0, 0.0]) is False + # 第 6 位是垃圾 → 当离线，不许当有效帧 + assert states_frame_online([1.0, 2.0, 500.0, 0.0, 50.0, "x"]) is False + def test_describe_mentions_the_unit_trap_for_legacy(): """legacy 的"开口"是千分比不是毫米，日志里必须写明。 diff --git a/src/arm_application/test/test_operator_gripper_state.py b/src/arm_application/test/test_operator_gripper_state.py index 63b87b8..85eda57 100644 --- a/src/arm_application/test/test_operator_gripper_state.py +++ b/src/arm_application/test/test_operator_gripper_state.py @@ -104,6 +104,21 @@ def test_parallel_topic_array() -> None: assert parsed["permille"] == 250.0 assert parsed["gripped"] is True assert parsed["initialized"] is True + # 老 5 元素帧：节点只在读成功时发，按在线算 + assert parsed["connected"] is True + + +def test_offline_topic_frame_is_not_shown_as_live_state() -> None: + """第 6 位是 0 时前 5 个是上一次的值，页面上不能显示成"夹住"。""" + text = "data:\n- 1.0\n- 2.0\n- 250.0\n- 250.0\n- 40.0\n- 0.0\n---\n" + parsed = _parse_states_echo(text) + assert parsed is not None + assert parsed["connected"] is False + assert "总线无应答" in parsed["error_msg"] + row = present("89", rotary=False, stroke_mm=20.0, reading=parsed) + assert row["connected"] is False + assert row["gripped"] is False + assert "总线无应答" in row["summary"] def test_rotary_topic_yaml() -> None: diff --git a/src/arm_application/test/test_pipeline_stage1_diag.py b/src/arm_application/test/test_pipeline_stage1_diag.py index fb43ded..eb091af 100644 --- a/src/arm_application/test/test_pipeline_stage1_diag.py +++ b/src/arm_application/test/test_pipeline_stage1_diag.py @@ -19,6 +19,7 @@ import pytest from arm_application.overhead_oneshot import D435_MOVEL_GAVE_UP from arm_application.pipeline_node import ( close_ready, + gripper_wait_timeout_reason, hold_at_insert_z, init_z_refusal, stage1_log_fatal, @@ -27,6 +28,26 @@ from arm_application.pipeline_node import ( ) +def test_gripper_timeout_does_not_call_a_dead_bus_not_in_position(): + """2026-10-20：读 0x0201 无应答时只发离线帧，不能报成"夹爪没到位"。 + + 三种情况说法必须不同，否则会去查夹爪开合，而真正的问题是总线读不回来。 + """ + no_frame = gripper_wait_timeout_reason( + "pos", 8.0, None, saw_any=False, saw_offline=False + ) + assert "都没收到" in no_frame and "节点没在发" in no_frame + offline = gripper_wait_timeout_reason( + "pos", 8.0, None, saw_any=True, saw_offline=True + ) + assert "读不到状态" in offline and "不是夹爪没张开" in offline + assert "未到位" not in offline + late = gripper_wait_timeout_reason( + "pos", 8.0, (0, 120.0), saw_any=True, saw_offline=False + ) + assert "未到位" in late and "grip=0 pos=120" in late + + def test_close_at_zero_continues_even_when_firmware_says_arrived(): """2026-10-19：合到 0 时规还在，固件写 grip=1。只认 grip=2 就永远不顺时针。