修复平行夹爪串口通讯问题
今天三次实测都在"首轮咬合结束→张开到 400"之后停住，微抬 Z 3mm 那条 厂家 MoveL 从未下发。日志只写"夹爪 pos 后 8s 未到位（没有 /gripper/states）"， 照着去查夹爪开合，方向是错的。 真实原因是串口会话坏了：旧节点从昨天 18:25 一直跑，写命令（FC06）有回显 ——ctrl pos=400 有记录、手指也确实张开——读 0x0201 却连续 264 次无应答 （0x0200 和写都有应答）。_on_timer 读失败就 return，一帧状态都不发， 流水线拿不到状态，就在 _shift_flange_z 之前退出了。 改法（都按"读不到要如实说读不到"）： - dh_pgea：串口 EIO/termios 错误从 DhPgeaError 里分出来，单独抛 DhPgeaBusError。以前 termios.error(5) 从 timer 回调冒出去，整个节点带 traceback 退出。 - dh_pgea_node：读失败也发 /gripper/states，data 增到 6 个元素，最后一位 online（1=刚读回，0=离线、前 5 个是旧值）；5 元素老消费者不受影响。 离线时不发 /gripper/joint_states（关节状态没有离线位，不能拿旧开口冒充 现在的）。EIO 立刻重开串口，连续 30 拍读不到也重开一次，1 秒最多一次。 - pipeline_node：超时不再一律说"未到位"，三种情况分开——真没到位 / 只有离线帧（总线无应答，不是没张开）/ 一帧都没收到（节点没在发）。 - gauge_gripper、operator_gripper_state：离线帧不当有效状态，夹爪页显示 "总线无应答"。 - gauge_config：登记 unwind_lift_mm 和 unwind_open_permille。这两个键 上一次只写进了 site.yaml，加载器按"不认识的键"直接拒绝启动 gauge_assembly 段（test_shipped_site_yaml_s07_step_cannot_deadlock 当时 是红的）。 - site_io：恢复7af9e3d误删的 listed_arms / absolutize_site_paths 等 （operator_gripper_state 还在导入 listed_arms，操作台夹爪页一导入就 ImportError）。按76c9b9c恢复，只增不减。 实测验收：重启节点后 --cli probe 读回 data=[1.0, 2.0, 9.0, 0.0, 20.0, 1.0]，online=1、grip=2（咬住）、pos=9， 0x0201 不再无应答。 单测 931 passed。新增负向用例：离线帧不能被当成有效状态、EIO 必须抛 DhPgeaBusError、夹爪超时不能报成"未到位"。剩余 6 个失败与本次无关 （伺服限速、回调遮蔽、operator_camera_view 直接继承 Node、 site.yaml 缺 screw_stage_depth_mm、臂控制器未激活，以及 parallel_cli 用例因当前有真实夹爪节点占着串口而走了另一条分支）。 Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -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`。
|
||||
|
||||
@@ -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,6 +378,7 @@ class DhPgeaGripper:
|
||||
if self._ser is None or not self._ser.is_open:
|
||||
raise DhPgeaError("串口未打开")
|
||||
bit_s = 10.0 / max(1, self.baudrate)
|
||||
try:
|
||||
self._set_tx(True)
|
||||
if self.rs485_rts:
|
||||
time.sleep(2.0 * bit_s)
|
||||
@@ -377,6 +394,11 @@ class DhPgeaGripper:
|
||||
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:
|
||||
|
||||
@@ -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"]
|
||||
|
||||
@@ -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 = 接触到之后不卸载），"
|
||||
|
||||
@@ -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]),
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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]:
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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 的"开口"是千分比不是毫米，日志里必须写明。
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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 就永远不顺时针。
|
||||
|
||||
|
||||
Reference in New Issue
Block a user