仿真夹爪适配器：服务接口对齐真机，开口按正运动学换算成真毫米
为什么
------
应用层只认 arm_msgs/srv/GripperExecute（开口毫米），真机与仿真必须同一套接口。
但 GN01 的 URDF 里 <mimic> 是**线性近似**：直接按线性系数下发 finger_width_joint，
"命令开 20 mm"在 Gazebo 里量出来是 21.75 mm —— 抓不住工件，而且看起来像算法问题。
实测（2026-09-24，Gazebo 内量两指尖间距）：
线性下发 20 mm → 实际 21.74 mm（偏差 1.74 mm，超 2 mm 抓取容差的前半步）
走 FK 反解 → 实际 20.00 mm（≤0.5 mm）
怎么改
------
- 新增 sim_gripper_kin.py：解析 URDF 正运动学，算出"主关节值 ↔ 真实开口 mm"的
单调曲线，反解用二分。曲线非单调/零行程时**主动报错**，不静默退化成线性。
- sim_gripper.py：开口换算全部走该曲线；容差统一改成毫米。
- sim_gripper_adapter.py：反馈改取**从动关节**（见下）；is_gripped 按毫米判定。
- 新增 check_sim_gripper.py：三层验收（服务/开口一致/指尖真实几何行程 + 对称性）。
踩到的坑
--------
- finger_width_joint 挂在无质量的 unused_finger_width_link 上，与手指**没有机械
连接**：命令到 0.100，它的 /joint_states 恒为 1.4e-08，6 个从动关节才准到
0.7854。反馈若取它，/gripper/status 会永远报"开口 0 mm"。
- 残留进程会伪造出"代码写错了"的现场：旧 sim_gripper_adapter 与新实例抢话题，
验收读到"指尖间距 0.03 mm"（真实 127 mm）；孤儿 static_transform_publisher
用 latched /tf_static 把旧相机外参按在新仿真上。
→ 新增 scripts/lib/sim_procs.sh；run_sim_gz.sh 用 setsid 独立进程组，
PGID 记进 log_sim/sim.pgid，stop 时整组 kill；启动前预检拒绝带病启动。
- 验收脚本自己也错过两次（把 TF 的米当毫米比 → 127 mm 显示成 0.13 mm；
用会转动指垫的中点漂移判对称 → 必然位移 16 mm）。判据改成"两侧各走多远"，
单位只在 _dist_mm 里换算一次。
验证
----
python3 -m pytest src/arm_sim_gazebo/test/ -q → 307 passed
python3 scripts/check_sim_gripper.py → 三层全过
第 2 层：请求/反馈开口最大偏差 < 0.5 mm
第 3 层：期望行程 vs 实际行程、左右行程差，均 < 1.5 mm
bash scripts/stop_sim_gz.sh --list → 整组列出（含 static tf）
文档
----
- 仿真手册：新增 §4.4（开口 mm 怎么换算）、坑 12（宽度关节不动、
反馈必须取从动关节）；停止一节改写成进程组语义。
- FRAMEWORK_RULES：新增 R4.11（残留进程会伪造现场）、R4.12（验收判据要挑
与实现无关的物理量）；R4.9 表补夹爪；清单补 13/14 两条。
- HANDBOOK：新增 §3.4「在仿真里开合夹爪」，补 §4.4 的两条新教训。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -0,0 +1,411 @@
|
||||
#!/usr/bin/env python3
|
||||
"""仿真夹爪的验收：**几何**行程对不对，不是"服务有返回"。
|
||||
|
||||
为什么需要它
|
||||
------------
|
||||
夹爪出问题的方式几乎全是静默的：
|
||||
|
||||
* 耦合链只套了一层 multiplier —— 夹爪会动、角度也是那个量级，就是夹不准；
|
||||
* 命令数组顺序与控制器不一致 —— 角度发给了别的关节，夹爪"动得很奇怪"；
|
||||
* 毫米与米混用（差 1000 倍） —— 夹爪开到别的地方；
|
||||
* 夹爪适配器根本没起来 —— 服务不存在，但**话题照旧**、控制器照旧 active。
|
||||
|
||||
前三条都能通过"调服务有 success=true、status 里有数"这种验收。
|
||||
所以这里分三层，只有第二、三层算数：
|
||||
|
||||
第 1 层 /gripper/* 服务在、/gripper/status 有数据 （不算验收）
|
||||
第 2 层 请求开口 mm == 关节反馈换回来的开口 mm （证明下发链路）
|
||||
第 3 层 两个指尖在 world 系里的**实际间距**随请求变化，
|
||||
且"合拢→张开"的行程 == 请求的开口 （证明耦合与几何）
|
||||
|
||||
第 3 层是关键：第 2 层只说明"关节值到位了"。如果 mimic 的 multiplier 写错，
|
||||
手指根本不会按预期张开，而第 2 层完全看不出来。也正因为这一层是**独立**的，
|
||||
它才逼出了下面两个真实故障 —— 都是"验收脚本自己错"，比被测对象错更常见：
|
||||
|
||||
⚠️ 三个踩过的坑（都让本脚本先给出过错误结论）
|
||||
--------------------------------------------
|
||||
1. **米 / 毫米**。TF 查出来的坐标是米，而本脚本的阈值与期望值全是毫米。
|
||||
漏掉 ×1000 时，127 mm 会显示成 0.13 mm —— 恰好"看起来像夹爪没张开"，
|
||||
于是跑去查耦合公式，而真正的问题就在本文件里。（判据全部走 _dist_mm。）
|
||||
|
||||
2. **残留进程**。旧会话没停干净时，会有两个 sim_gripper_adapter 同时订阅
|
||||
/joint_states、同时发布 /gripper/status，服务调用还会被随机分给其中一个；
|
||||
残留的 static_transform_publisher 更阴 —— 它发 latched 的 /tf_static，
|
||||
会把**旧相机外参**按在新仿真的 TF 树上。症状一样是"读数全乱"。
|
||||
→ 所以本脚本第一步查重名节点（`:meth:`GripperCheck._duplicate_nodes`），
|
||||
启动侧也加了闸门（scripts/run_sim_gz.sh 的预检 + setsid 进程组）。
|
||||
|
||||
3. **判据要与机构无关**。两侧"对称"不能用"两指中点漂移"来判：tip frame 是
|
||||
刚性固连在**会转动**的指垫上的（指垫既平移又转，指面才保持平行），所以
|
||||
frame 走的是弧线，中点必然位移 16 mm 左右。真正与方向无关的判据是
|
||||
"两侧各自走了多远"（实测 52.56 / 52.56 mm）。
|
||||
|
||||
⚠️ 指尖坐标系原点的固有间距
|
||||
实测两个 `*_finger_tip` frame 在完全合拢时仍然相距 27.09 mm（tip frame 不在
|
||||
夹持面上，而且会随手指转动）。所以**不要**拿"指尖间距"直接当开口看；
|
||||
本脚本用它的**变化量**（行程）判据 —— 那个变化量与开口是 1:1 的
|
||||
（实测：请求 8 / 20 / 50 mm → 间距增加 8.00 / 20.00 / 50.00 mm）。
|
||||
|
||||
用法
|
||||
----
|
||||
source scripts/sim_env.sh
|
||||
python3 scripts/check_sim_gripper.py # 需要 Gazebo 仿真已在跑
|
||||
python3 scripts/check_sim_gripper.py --verbose # 另打印每档的指尖原始坐标
|
||||
|
||||
退出码：0 = 通过；1 = 未通过；2 = 环境不可用（服务不在 / 有重名节点）。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import math
|
||||
import sys
|
||||
import time
|
||||
from typing import Dict, List, Optional, Tuple
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from tf2_ros import Buffer, TransformListener
|
||||
|
||||
try:
|
||||
from arm_msgs.msg import GripperStatus
|
||||
from arm_msgs.srv import GripperExecute
|
||||
except ModuleNotFoundError as exc: # pragma: no cover - 只在环境没 source 时触发
|
||||
# 直接让 Python 抛"找不到模块"很难定位：真正的原因通常是**忘了 source 环境**，
|
||||
# 而不是 arm_msgs 没编译。所以这里把原因和修法一次说清。
|
||||
sys.exit(
|
||||
f"找不到 {exc.name}：请先 source 仿真环境（它把 install_sim/ 加进 PYTHONPATH）：\n"
|
||||
f" source scripts/sim_env.sh\n"
|
||||
f"再执行本脚本。若仍失败，说明 install_sim/ 还没构建：\n"
|
||||
f" bash scripts/build_sim.sh"
|
||||
)
|
||||
|
||||
#: 指尖坐标系的**固有**间距不影响行程判据，但打印出来省得有人误读。
|
||||
TIP_LEFT = "sim_left_finger_tip"
|
||||
TIP_RIGHT = "sim_right_finger_tip"
|
||||
WORLD = "world"
|
||||
|
||||
#: 每个开口请求之后等多久再量（位置控制器 + 物理稳定）。
|
||||
SETTLE_S = 2.5
|
||||
|
||||
#: 第 2 层容差（毫米）：请求开口 vs 关节反馈换回的开口。
|
||||
POSITION_TOL_MM = 0.5
|
||||
#: 第 3 层容差（毫米）：指尖行程 vs 期望行程。
|
||||
TRAVEL_TOL_MM = 1.5
|
||||
#: 左右对称性容差（毫米）：两侧位移量之差。
|
||||
SYMMETRY_TOL_MM = 1.5
|
||||
|
||||
|
||||
def _spin(node: Node, seconds: float) -> None:
|
||||
deadline = time.monotonic() + seconds
|
||||
while time.monotonic() < deadline:
|
||||
rclpy.spin_once(node, timeout_sec=0.05)
|
||||
|
||||
|
||||
def _tip_positions(buf: Buffer, node: Node, attempts: int = 40):
|
||||
"""取两个指尖在 world 系里的位置；查不到返回 None。"""
|
||||
for _ in range(attempts):
|
||||
try:
|
||||
left = buf.lookup_transform(WORLD, TIP_LEFT, rclpy.time.Time())
|
||||
right = buf.lookup_transform(WORLD, TIP_RIGHT, rclpy.time.Time())
|
||||
return (
|
||||
(left.transform.translation.x, left.transform.translation.y, left.transform.translation.z),
|
||||
(right.transform.translation.x, right.transform.translation.y, right.transform.translation.z),
|
||||
)
|
||||
except Exception:
|
||||
_spin(node, 0.2)
|
||||
return None
|
||||
|
||||
|
||||
def _dist_pts(a, b) -> float:
|
||||
"""两点距离，**米**（原始量纲，调用方自己决定怎么用）。"""
|
||||
return math.sqrt(sum((x - y) ** 2 for x, y in zip(a, b)))
|
||||
|
||||
|
||||
def _dist_mm(a, b) -> float:
|
||||
"""两个位置之间的距离，**毫米**。
|
||||
|
||||
⚠️ 这个换算漏过一次，症状很典型：TF 查出来的坐标是**米**，而本脚本里所有
|
||||
阈值/期望值都是**毫米**（POSITION_TOL_MM、TRAVEL_TOL_MM、请求开口…）。
|
||||
漏掉 *1000 的话，127 mm 会显示成 0.13 —— 恰好"看起来像夹爪没张开"，
|
||||
于是跑去看耦合公式，而真正的问题只在本文件这一行。
|
||||
（实测：同一份数据下，指尖坐标是对的，只是判据全被缩小了 1000 倍。）
|
||||
"""
|
||||
return 1000.0 * _dist_pts(a, b)
|
||||
|
||||
|
||||
# ⚠️ 属性名都要避开 rclpy.Node 自己的内部属性：`self._clients` 是 Node 的私有
|
||||
# 成员（一个 list），覆盖它会直接在 create_client 里崩（实测踩过）。
|
||||
# 所以这里统一用 `_grip_clients` / `_last_status` / `_tf_buf`。
|
||||
class GripperCheck(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim_gripper_check")
|
||||
self._tf_buf = Buffer()
|
||||
TransformListener(self._tf_buf, self)
|
||||
self._last_status: Optional[GripperStatus] = None
|
||||
self.create_subscription(GripperStatus, "/gripper/status", self._on_status, 10)
|
||||
self._grip_clients: Dict[str, object] = {}
|
||||
for verb in ("open", "close", "move"):
|
||||
self._grip_clients[verb] = self.create_client(GripperExecute, f"/gripper/{verb}")
|
||||
|
||||
def _on_status(self, msg: GripperStatus) -> None:
|
||||
self._last_status = msg
|
||||
|
||||
# --- 基础设施 ---------------------------------------------------------
|
||||
def wait_ready(self, timeout: float = 30.0) -> List[str]:
|
||||
"""等服务和状态都就绪。返回缺失项（空列表=就绪）。"""
|
||||
missing = []
|
||||
for verb, cli in self._grip_clients.items():
|
||||
if not cli.wait_for_service(timeout_sec=timeout): # type: ignore[attr-defined]
|
||||
missing.append(f"/gripper/{verb}")
|
||||
if self._last_status is None:
|
||||
_spin(self, 3.0)
|
||||
if self._last_status is None:
|
||||
missing.append("/gripper/status（话题无数据）")
|
||||
return missing
|
||||
|
||||
def call(self, verb: str, position: float = 0.0) -> Tuple[bool, str]:
|
||||
cli = self._grip_clients[verb]
|
||||
req = GripperExecute.Request()
|
||||
req.command.command = verb
|
||||
req.command.position = float(position)
|
||||
fut = cli.call_async(req) # type: ignore[attr-defined]
|
||||
while not fut.done():
|
||||
_spin(self, 0.05)
|
||||
res = fut.result()
|
||||
if res is None:
|
||||
return False, "服务没有返回"
|
||||
return bool(res.success), str(res.message)
|
||||
|
||||
def movement(self, verb: str, position: float = 0.0):
|
||||
"""下发一条命令，等稳定，返回 (请求说明, 状态开口, 两指尖, 是否成功)。"""
|
||||
ok, msg = self.call(verb, position)
|
||||
_spin(self, SETTLE_S)
|
||||
tips = _tip_positions(self._tf_buf, self)
|
||||
aperture = float(self._last_status.position) if self._last_status else None
|
||||
return ok, msg, aperture, tips
|
||||
|
||||
# --- 三层验收 ---------------------------------------------------------
|
||||
def _duplicate_nodes(self) -> List[str]:
|
||||
"""查出"同名节点有两个"的情况。
|
||||
|
||||
⚠️ 这是踩出来的：残留的旧会话与新会话同时活着时，两个
|
||||
``sim_gripper_adapter`` 会同时订阅 /joint_states、同时发布
|
||||
/gripper/status，服务调用还会被随机分给其中一个 ——
|
||||
验收脚本读到的开口恒为 0 mm、指尖间距 0.03 mm（真实值 127 mm），
|
||||
看起来完全像夹爪换算写错了。
|
||||
|
||||
所以验收第一步就是确认"图里只有一个我"。启动侧的那道闸门在
|
||||
scripts/run_sim_gz.sh 的预检里（拒绝起第二个仿真），这里是第二道。
|
||||
"""
|
||||
names = self.get_node_names()
|
||||
dupes = []
|
||||
for known in (
|
||||
"sim_gripper_adapter",
|
||||
"sim_camera_adapter",
|
||||
"sim_tactile_adapter",
|
||||
"robot_state_publisher",
|
||||
"joint_state_broadcaster",
|
||||
"gripper_controller",
|
||||
):
|
||||
n = sum(1 for name in names if name.strip("/") == known)
|
||||
if n > 1:
|
||||
dupes.append(f"{known} × {n}")
|
||||
return dupes
|
||||
|
||||
def check(self, verbose: bool) -> int:
|
||||
dupes = self._duplicate_nodes()
|
||||
if dupes:
|
||||
print("环境不干净：仿真节点有重名（说明有残留的旧会话在跑）：", file=sys.stderr)
|
||||
for d in dupes:
|
||||
print(f" - {d}", file=sys.stderr)
|
||||
print(
|
||||
"\n两个同名节点会抢话题/服务/TF，读数全是乱的。先停干净：\n"
|
||||
" bash scripts/stop_sim_gz.sh\n"
|
||||
"再重启（run_sim_gz.sh 自己也会拒绝在残留状态下启动）。",
|
||||
file=sys.stderr,
|
||||
)
|
||||
return 2
|
||||
|
||||
missing = self.wait_ready()
|
||||
if missing:
|
||||
print("环境不可用，缺：", file=sys.stderr)
|
||||
for m in missing:
|
||||
print(f" - {m}", file=sys.stderr)
|
||||
print(
|
||||
"\n先确认仿真在跑（bash scripts/run_sim_gz.sh），"
|
||||
"且夹爪适配器起来了（ros2 node list | grep sim_gripper_adapter）。",
|
||||
file=sys.stderr,
|
||||
)
|
||||
return 2
|
||||
|
||||
print("第 1 层：服务与状态")
|
||||
print(f" /gripper/{{open,close,move}} 可用；/gripper/status 有数据（开口 {self._last_status.position:.2f} mm）")
|
||||
|
||||
# --- 第 2、3 层：走一遍开口序列 -----------------------------------
|
||||
requests: List[Tuple[str, float]] = [
|
||||
("close", 0.0),
|
||||
("move", 8.0),
|
||||
("move", 20.0),
|
||||
("move", 50.0),
|
||||
("open", 0.0),
|
||||
]
|
||||
rows = []
|
||||
for verb, pos in requests:
|
||||
ok, msg, aperture, tips = self.movement(verb, pos)
|
||||
label = "全开" if verb == "open" else ("全闭合" if verb == "close" else f"{pos:.0f} mm")
|
||||
rows.append((label, float(pos) if verb == "move" else None, ok, msg, aperture, tips))
|
||||
|
||||
print("\n第 2 层：请求开口 vs 关节反馈换回的开口（下发链路）")
|
||||
print(f" {'命令':<10}{'期望开口':>10}{'状态开口':>10}{'差(mm)':>10} ")
|
||||
l2_fail = []
|
||||
# "期望开口"对 open 来说是仿真行程，从状态里推出来（open 时的读数）。
|
||||
open_row = rows[-1]
|
||||
stroke = open_row[4]
|
||||
for label, want, ok, msg, aperture, tips in rows:
|
||||
expect = stroke if label == "全开" else (0.0 if label == "全闭合" else want)
|
||||
if aperture is None:
|
||||
l2_fail.append(f"{label}: 拿不到状态开口")
|
||||
print(f" {label:<10}{expect:>10.2f}{'--':>10}{'--':>10} 状态缺失")
|
||||
continue
|
||||
diff = aperture - (expect or 0.0)
|
||||
bad = abs(diff) > POSITION_TOL_MM
|
||||
if bad:
|
||||
l2_fail.append(f"{label}: 状态开口 {aperture:.2f} != 期望 {expect:.2f}")
|
||||
if not ok:
|
||||
l2_fail.append(f"{label}: 服务返回失败（{msg}）")
|
||||
print(f" {label:<10}{expect:>10.2f}{aperture:>10.2f}{diff:>10.2f} {'✗' if bad else '✓'} {msg}")
|
||||
|
||||
print("\n第 3 层：指尖的**实际**位移（真几何，这一层才证明耦合是对的）")
|
||||
tips_ok = all(r[5] is not None for r in rows)
|
||||
if not tips_ok:
|
||||
print(" 查不到指尖 TF，无法验证几何 —— 这不算通过。", file=sys.stderr)
|
||||
return 1
|
||||
|
||||
# ⚠️ 这一层**故意不用 status 的开口读数**当基准：开口反馈取自从动关节，
|
||||
# 而第 2 层已经在验那条链路。这里只认"我请求了多少毫米"，
|
||||
# 几何摆在那里，跟任何软件读数无关 —— 这才叫独立验证。
|
||||
closed_tips = rows[0][5]
|
||||
open_tips = rows[-1][5]
|
||||
span_closed = _dist_mm(*closed_tips)
|
||||
span_open = _dist_mm(*open_tips)
|
||||
spans = [_dist_mm(*r[5]) for r in rows]
|
||||
|
||||
print(f" 合拢：指尖间距 {span_closed:7.2f} mm（tip frame 固有间距，不是开口）")
|
||||
print(f" 全开：指尖间距 {span_open:7.2f} mm")
|
||||
|
||||
# 逐档对比"请求开口"与"实测间距增量"：仿真里 1 mm 开口 ≙ 1 mm 间距，
|
||||
# 所以两者必须相等（探针实测 27.02→127.02 mm，正好 1.000）。
|
||||
travel_bad = []
|
||||
for label, want, ok, msg, aperture, tips in rows:
|
||||
if want is None:
|
||||
continue # close / open 的 request 不是毫米数，单独判
|
||||
actual = _dist_mm(*tips) - span_closed
|
||||
err = actual - want
|
||||
bad = abs(err) > TRAVEL_TOL_MM
|
||||
if bad:
|
||||
travel_bad.append(
|
||||
f"请求 {want:.0f} mm，指尖实际张开 {actual:.2f} mm（差 {err:+.2f}）"
|
||||
)
|
||||
print(
|
||||
f" 请求 {want:5.1f} mm：指尖张开 {actual:7.2f} mm "
|
||||
f"差 {err:+6.2f} mm {'✗' if bad else '✓'}"
|
||||
)
|
||||
|
||||
# 全开那一档用**几何自洽**来判：必须比 50 mm 那档还大，且与第 2 层的
|
||||
# 开口读数一致（两者都错成一样是不可能的）。
|
||||
if stroke:
|
||||
err = (span_open - span_closed) - stroke
|
||||
if abs(err) > TRAVEL_TOL_MM:
|
||||
travel_bad.append(
|
||||
f"全开行程 {span_open - span_closed:.2f} mm != 开口读数 {stroke:.2f} mm"
|
||||
)
|
||||
print(
|
||||
f" 全开行程 {span_open - span_closed:7.2f} mm 期望 {stroke:.2f} mm "
|
||||
f"差 {err:+.2f} mm {'✓' if abs(err) <= TRAVEL_TOL_MM else '✗'}"
|
||||
)
|
||||
|
||||
# 单调性：请求越大，间距必须越大（方向错了会在这里露出来）。
|
||||
monotonic = all(b > a for a, b in zip(spans, spans[1:]))
|
||||
print(f" 单调递增：{'✓' if monotonic else '✗ ' + ' → '.join(f'{s:.2f}' for s in spans)}")
|
||||
|
||||
# 对称性：两侧**行程相等**才算这条耦合链是对的。
|
||||
#
|
||||
# ⚠️ 这里换过两次判据，两次都是"判据本身错了"，值得记下来：
|
||||
# 1) 最初把两侧位移投影到"左指的张开方向"再比大小 → 一个完全
|
||||
# 对称的夹爪被判成不对称（左 +52.6 / 右 -42.5）。因为左指的
|
||||
# 位移方向自己带着 x 分量，拿它当基准去投影右侧必然偏。
|
||||
# 2) 改成"两指中点漂移" → 又误判（漂移 16.29 mm，而两侧各走
|
||||
# 52.56 / 52.55 mm，好得不能再好）。原因：tip frame 是刚性固连
|
||||
# 在**会旋转**的指垫上的（指垫既平移又转动，指面才保持平行），
|
||||
# 所以 frame 走的是弧线 —— 两侧弧线的"共同分量"让中点发生位移，
|
||||
# 这跟"左右不对称"是两回事。
|
||||
# 真正与方向无关、又能反映耦合好坏的量只有一个：**两侧各自走了多远**。
|
||||
left_move = 1000.0 * _dist_pts(closed_tips[0], open_tips[0])
|
||||
right_move = 1000.0 * _dist_pts(closed_tips[1], open_tips[1])
|
||||
asym = abs(left_move - right_move)
|
||||
asym_bad = asym > SYMMETRY_TOL_MM
|
||||
mid_closed = tuple((a + b) / 2.0 for a, b in zip(*closed_tips))
|
||||
mid_open = tuple((a + b) / 2.0 for a, b in zip(*open_tips))
|
||||
mid_drift = 1000.0 * _dist_pts(mid_closed, mid_open)
|
||||
print(
|
||||
f" 对称性：左右各走 {left_move:.2f} / {right_move:.2f} mm，"
|
||||
f"差 {asym:.2f} mm {'✗' if asym_bad else '✓'}"
|
||||
)
|
||||
print(
|
||||
f" （参考：两指中点整体位移 {mid_drift:.2f} mm。tip frame 固连在"
|
||||
f"会转动的指垫上，会画弧，所以这个数不为 0 是正常的，不参与判据。）"
|
||||
)
|
||||
|
||||
if verbose:
|
||||
print("\n明细：")
|
||||
for label, _want, ok, msg, aperture, tips in rows:
|
||||
d = "--" if tips is None else f"{_dist_mm(*tips):.2f}"
|
||||
print(f" {label:<10} ok={ok} 开口={aperture} 指尖间距={d} {msg}")
|
||||
# 原始坐标也打出来：上面那一列数字是"间距"，看着对/错时都需要
|
||||
# 回头核对它到底取到了哪两个点（历史上这里出过取错帧的假象）。
|
||||
print("\n 指尖坐标（world 系，米）：")
|
||||
for label, _want, _ok, _msg, _ap, tips in rows:
|
||||
if tips is None:
|
||||
print(f" {label:<10} --")
|
||||
continue
|
||||
lx, rx = tips
|
||||
print(
|
||||
f" {label:<10} 左=({lx[0]:+.4f},{lx[1]:+.4f},{lx[2]:+.4f}) "
|
||||
f"右=({rx[0]:+.4f},{rx[1]:+.4f},{rx[2]:+.4f})"
|
||||
)
|
||||
|
||||
problems = list(l2_fail)
|
||||
problems.extend(travel_bad)
|
||||
if not monotonic:
|
||||
problems.append("开口不是单调递增 —— 方向或顺序反了")
|
||||
if asym_bad:
|
||||
problems.append(
|
||||
f"左右不对称（左右行程差 {asym:.2f} mm）—— 某一侧耦合没生效"
|
||||
)
|
||||
|
||||
print()
|
||||
if problems:
|
||||
print("结论：不通过", file=sys.stderr)
|
||||
for p in problems:
|
||||
print(f" - {p}", file=sys.stderr)
|
||||
return 1
|
||||
print("结论：通过（下发链路 + 耦合几何 + 方向 + 对称性）")
|
||||
return 0
|
||||
|
||||
|
||||
def main() -> int:
|
||||
ap = argparse.ArgumentParser(description=__doc__.splitlines()[0])
|
||||
ap.add_argument("--verbose", action="store_true", help="打印每一档的明细")
|
||||
args = ap.parse_args()
|
||||
|
||||
rclpy.init()
|
||||
node = GripperCheck()
|
||||
try:
|
||||
return node.check(args.verbose)
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,174 @@
|
||||
#!/usr/bin/env bash
|
||||
# =============================================================================
|
||||
# scripts/lib/sim_procs.sh —— 仿真进程扫描（被 stop_sim_gz.sh / run_sim_gz.sh 共用）
|
||||
#
|
||||
# 为什么单独抽一层
|
||||
# ----------------
|
||||
# "哪些进程算仿真栈"这件事以前只写在 stop_sim_gz.sh 里。新增一个仿真节点
|
||||
# （sim_gripper_adapter）时**忘了同步那张模式表**，于是：
|
||||
#
|
||||
# 脚本说"仿真已停止" → 其实残留了一个夹爪适配器
|
||||
# → 下一次启动后**两个适配器同时订阅 /joint_states、同时发布 /gripper/status**
|
||||
# → 服务调用被负载均衡到任意一个；TF 也有两份来源
|
||||
# → 验收脚本读到的开口/指尖间距全是乱的（实测：开口恒为 0 mm，
|
||||
# 指尖间距 0.03 mm，而实际是 127 mm）
|
||||
#
|
||||
# 这个坑的可怕之处在于**它看起来像夹爪代码写错了**。所以这里做三件事：
|
||||
#
|
||||
# 1. 两份脚本共用同一个扫描实现 —— 不再各写一份会漂移的模式表；
|
||||
# 2. 停止的主通道改成**进程组**：仿真整栈用 setsid 起在自己的会话里，
|
||||
# PGID 记在 log_sim/sim.pgid，停止时 kill 整组，不依赖模式匹配；
|
||||
# 3. 模式扫描当兜底（对付 ign 自己 fork 的进程与历史孤儿）。扫描本身是
|
||||
# "点名表 + install_sim/ 兜底"：凡命令行含 ``<仓库>/install_sim/``
|
||||
# 的进程都算仿真进程，所以**以后再加新节点，不改这个文件也能被扫到**。
|
||||
# 点名表只用于少数"不在 install_sim 里"的进程（Gazebo 本体、桥接、
|
||||
# robot_state_publisher、static_transform_publisher）。
|
||||
#
|
||||
# 用法（在 `set -uo pipefail` 之后 source）：
|
||||
# source "$(dirname "$0")/lib/sim_procs.sh"
|
||||
# mapfile -t targets < <(sim_proc_targets) # 只输出 PID（已排除自身/祖先）
|
||||
# sim_proc_scan # 输出 "PID<TAB>命令行"
|
||||
# sim_pgid_read / sim_pgid_leader_is_ours / sim_pgid_stop
|
||||
# =============================================================================
|
||||
|
||||
# 兜底模式：仿真安装空间。用绝对路径前缀，避免误伤 install/（真机那边）。
|
||||
# ⚠️ 路径要按 awk 正则转义：路径里可能有 . + - 之类字符，不转义的话
|
||||
# "." 会匹配任意字符（匹配面变大 = 误杀无关进程）。
|
||||
_sim_install_dir() {
|
||||
printf '%s/install_sim/' "$(_sim_repo_root)"
|
||||
}
|
||||
|
||||
_sim_regex_escape() {
|
||||
printf '%s' "$1" | sed -e 's/[][\.^$*+?(){}|\\]/\\&/g'
|
||||
}
|
||||
|
||||
# 点名表：列**不在 install_sim/ 下**、但属于仿真栈的进程。
|
||||
# ign gazebo / gz sim Gazebo 服务端与 GUI（conda Fortress 里可执行名是 ign gazebo）
|
||||
# parameter_bridge 相机/触觉/clock 的 ros_gz_bridge（在 conda env 的 lib 下）
|
||||
# robot_state_publisher 发 TF 与 /robot_description（同上，不在 install_sim）
|
||||
# static_transform_publisher 相机的静态外参（同上是 conda env 里的可执行文件）
|
||||
# controller_manager.*spawner / gz_ros2_control 控制器与它的 spawner
|
||||
#
|
||||
# ⚠️ 这几个必须**点名**：它们的可执行文件来自 conda 环境或 ROS 发行版，
|
||||
# 命令行里没有 install_sim/ 字样，兜底规则扫不到。
|
||||
# 实测教训：只留兜底规则时 robot_state_publisher 会漏，而它的症状是
|
||||
# "脚本说已停止，它还在发上一份 TF 与 /robot_description"。
|
||||
# 更阴的是 static_transform_publisher：它发的是**latched**的 /tf_static，
|
||||
# 残留进程会一直发**旧的外参**，表现成"改了相机安装位置却不生效"。
|
||||
#
|
||||
# 有意**不**包含 rviz2：headless 模式不起 RViz，用户可能开着 RViz 看别的东西，
|
||||
# 写进来会误杀无关进程 —— 误杀比漏杀更糟。
|
||||
SIM_PROC_NAMED='ign gazebo|gz sim|parameter_bridge|ros_gz_bridge|robot_state_publisher|static_transform_publisher|controller_manager.*spawner|gz_ros2_control'
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 进程组：精确停止的主通道
|
||||
# ---------------------------------------------------------------------------
|
||||
# 模式匹配永远是"猜"，上面那张表就漏过好几次。更可靠的做法是让**整个仿真栈
|
||||
# 待在自己的进程组里**：启动时用 setsid 新开一个会话，PID 即 PGID，写进
|
||||
# log_sim/sim.pgid；停止时 `kill -TERM -<PGID>` 一次性带走整组，
|
||||
# 不依赖任何模式串。模式扫描退化成兜底（对付 ign 自己 fork 出去的进程、
|
||||
# 以及历史遗留的孤儿）。
|
||||
#
|
||||
# ⚠️ 文件里的 PGID 可能是**过时的**（那次仿真的进程早退出了，PID 被别人复用）。
|
||||
# 所以用之前必须确认那个进程确实是我们的仿真栈 —— 见 sim_pgid_leader_is_ours。
|
||||
sim_pgid_file() {
|
||||
printf '%s/log_sim/sim.pgid' "$(_sim_repo_root)"
|
||||
}
|
||||
|
||||
_sim_repo_root() {
|
||||
cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd
|
||||
}
|
||||
|
||||
# 读 pgid 文件里的 PGID；没有/格式不对 → 输出空。
|
||||
sim_pgid_read() {
|
||||
local f pgid
|
||||
f="$(sim_pgid_file)"
|
||||
[ -f "$f" ] || return 0
|
||||
read -r pgid <"$f" 2>/dev/null || return 0
|
||||
case "${pgid:-}" in
|
||||
''|*[!0-9]*) return 0 ;; # 不是纯数字 → 当作无效
|
||||
esac
|
||||
printf '%s' "$pgid"
|
||||
}
|
||||
|
||||
# 该 PGID 的组长进程是否确实是仿真栈（防止 PID 复用导致误杀无关进程组）。
|
||||
sim_pgid_leader_is_ours() {
|
||||
local pgid="$1" args
|
||||
args="$(ps -o args= -p "$pgid" 2>/dev/null || true)"
|
||||
[ -n "$args" ] || return 1
|
||||
printf '%s' "$args" | grep -Eq 'arm_sim_gazebo|gazebo_sim|ign gazebo|gz sim|ros2 launch|ros2 run'
|
||||
}
|
||||
|
||||
# 组是否还活着（任一组员存活即可）。
|
||||
sim_pgid_alive() {
|
||||
local pgid="$1"
|
||||
kill -0 -- "-$pgid" 2>/dev/null
|
||||
}
|
||||
|
||||
# 停止整组：先 TERM 等一下，仍在就 KILL。成功输出 "stopped"。
|
||||
sim_pgid_stop() {
|
||||
local pgid="$1" i
|
||||
if ! sim_pgid_alive "$pgid"; then
|
||||
rm -f "$(sim_pgid_file)"
|
||||
return 0
|
||||
fi
|
||||
kill -TERM -- "-$pgid" 2>/dev/null
|
||||
for i in $(seq 1 20); do
|
||||
sim_pgid_alive "$pgid" || break
|
||||
sleep 0.5
|
||||
done
|
||||
if sim_pgid_alive "$pgid"; then
|
||||
echo "进程组 $pgid 没响应 TERM，用 KILL" >&2
|
||||
kill -KILL -- "-$pgid" 2>/dev/null
|
||||
sleep 0.5
|
||||
fi
|
||||
rm -f "$(sim_pgid_file)"
|
||||
}
|
||||
|
||||
sim_proc_patterns() {
|
||||
printf '%s|%s' "$SIM_PROC_NAMED" "$(_sim_regex_escape "$(_sim_install_dir)")"
|
||||
}
|
||||
|
||||
# 扫描：输出 "PID<TAB>命令行"。排除本进程/祖先/扫描用的 awk 自身。
|
||||
#
|
||||
# ⚠️ 两个必须踩过的坑（原 stop_sim_gz.sh 的注释，这里保留）：
|
||||
# 1. `pkill -f <模式>` 会匹配到自己所在的 shell（命令行里含那个模式），
|
||||
# 表现成"命令立刻返回、什么都没杀"。
|
||||
# 2. 模式串用**环境变量**传给 awk，不用 `-v pat=...`：用了 -v 的话 awk 自己的
|
||||
# 命令行里就含整串模式，于是它匹配到自己，复查时多报一个幽灵 PID。
|
||||
sim_proc_scan() {
|
||||
export PAT="$(sim_proc_patterns)"
|
||||
export ME="${SIM_PROC_SELF:-$$}"
|
||||
ps -eo pid=,ppid=,args= | awk '
|
||||
BEGIN { pat = ENVIRON["PAT"]; me = ENVIRON["ME"] }
|
||||
$0 ~ pat {
|
||||
if (NF < 3) next # 没有 args 字段（僵尸/内核线程）
|
||||
pid = $1; ppid = $2
|
||||
args = substr($0, index($0, $3))
|
||||
if (ppid == me) next # 本脚本的直接子进程（如 $(...) 的 fork）
|
||||
if (args ~ /sim_procs\.sh/) next # 本库自己（含 grep/sed 的读数）
|
||||
if (args ~ /stop_sim_gz\.sh/) next
|
||||
if (args ~ /run_sim_gz\.sh/) next
|
||||
if (args ~ /^\s*awk/) next
|
||||
print pid "\t" args
|
||||
}'
|
||||
}
|
||||
|
||||
# 排除自己与全部祖先，只输出 PID。
|
||||
sim_proc_targets() {
|
||||
local exclude=() p=$$ pid _rest e skip
|
||||
while [ "$p" -gt 1 ] 2>/dev/null; do
|
||||
exclude+=("$p")
|
||||
p=$(ps -o ppid= -p "$p" 2>/dev/null | tr -d ' ')
|
||||
[ -z "$p" ] && break
|
||||
done
|
||||
while read -r pid _rest; do
|
||||
[ -z "${pid:-}" ] && continue
|
||||
skip=0
|
||||
for e in "${exclude[@]}"; do
|
||||
[ "$pid" = "$e" ] && skip=1 && break
|
||||
done
|
||||
[ "$skip" = "1" ] && continue
|
||||
echo "$pid"
|
||||
done < <(sim_proc_scan)
|
||||
}
|
||||
+59
-1
@@ -67,9 +67,37 @@ fi
|
||||
|
||||
if [[ "${PASS_ONLY}" -eq 1 ]]; then
|
||||
# 这些选项不启动任何东西，只准备描述，直接转发。
|
||||
# （所以放在"残留进程"预检**之前**：手上有仿真在跑时，照样应该能 --print-urdf。）
|
||||
exec ros2 run arm_sim_gazebo gazebo_sim_launcher "${LAUNCH_ARGS[@]}"
|
||||
fi
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 预检：不许在"上一轮仿真还有残留"的情况下启动
|
||||
# ---------------------------------------------------------------------------
|
||||
# ⚠️ 这不是洁癖。实测过的最坏情况：残留了一个 sim_gripper_adapter，新起一个之后
|
||||
# 两个适配器**同时**订阅 /joint_states、同时发布 /gripper/status，
|
||||
# 服务调用还会被随机分给其中一个 —— 验收脚本读到的开口恒为 0 mm、
|
||||
# 指尖间距 0.03 mm（真实值 127 mm），看起来完全像夹爪换算写错了。
|
||||
# 残留的 static_transform_publisher 更阴：它发 latched 的 /tf_static，
|
||||
# 会一直把**旧外参**按在新仿真的 TF 树上。
|
||||
# 所以这里宁可拒绝启动，也不给你一个"看起来能跑、数据是错的"仿真。
|
||||
# shellcheck source=lib/sim_procs.sh
|
||||
source "${ROOT}/scripts/lib/sim_procs.sh"
|
||||
|
||||
mapfile -t running < <(sim_proc_targets)
|
||||
if [[ "${#running[@]}" != "0" ]]; then
|
||||
{
|
||||
echo "已有仿真相关进程在跑，拒绝启动第二个："
|
||||
for pid in "${running[@]}"; do
|
||||
printf ' %-7s %s\n' "$pid" "$(ps -o args= -p "$pid" 2>/dev/null | cut -c1-120)"
|
||||
done
|
||||
echo
|
||||
echo "先停掉它们： bash scripts/stop_sim_gz.sh"
|
||||
echo "只想看看残留是谁、不动手：bash scripts/stop_sim_gz.sh --list"
|
||||
} >&2
|
||||
exit 3
|
||||
fi
|
||||
|
||||
echo "=== 启动 Gazebo 仿真 ==="
|
||||
echo " 环境 : ${CONDA_DEFAULT_ENV:-?} ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-?}"
|
||||
echo " 退出方式 : Ctrl-C"
|
||||
@@ -78,4 +106,34 @@ echo " 首次启动约需 10~20 秒（Gazebo 初始化 + spawn + 拉控制器
|
||||
echo " 验收: ros2 control list_controllers 应看到两个 active"
|
||||
echo
|
||||
|
||||
exec ros2 run arm_sim_gazebo gazebo_sim_launcher "${LAUNCH_ARGS[@]}"
|
||||
# ---------------------------------------------------------------------------
|
||||
# 起在自己的**会话/进程组**里，并把 PGID 记下来
|
||||
# ---------------------------------------------------------------------------
|
||||
# 这样 stop_sim_gz.sh 可以用 `kill -TERM -PGID` 一次性带走整栈（含
|
||||
# robot_state_publisher / static_transform_publisher 这些"不在 install_sim 下"
|
||||
# 的节点），不必依赖模式匹配 —— 模式表漏一个就留一个孤儿的教训见脚本注释。
|
||||
#
|
||||
# 代价：进程组不在前台，终端里的 Ctrl-C **不会**传到仿真。所以这里自己接
|
||||
# SIGINT/SIGTERM，转发成组信号，行为与以前"Ctrl-C 停仿真"一致。
|
||||
PGID_FILE="$(sim_pgid_file)"
|
||||
mkdir -p "$(dirname "${PGID_FILE}")"
|
||||
|
||||
setsid bash -c 'echo $$ > "$1"; shift; exec "$@"' _ "${PGID_FILE}" \
|
||||
ros2 run arm_sim_gazebo gazebo_sim_launcher "${LAUNCH_ARGS[@]}" &
|
||||
SIM_PID=$!
|
||||
|
||||
_forward_signal() {
|
||||
local pgid
|
||||
pgid="$(sim_pgid_read)"
|
||||
if [[ -n "${pgid}" ]]; then
|
||||
echo
|
||||
echo "收到中断信号，正在停止整栈（进程组 ${pgid}）…" >&2
|
||||
sim_pgid_stop "${pgid}"
|
||||
fi
|
||||
}
|
||||
trap _forward_signal INT TERM
|
||||
|
||||
wait "${SIM_PID}"
|
||||
status=$?
|
||||
rm -f "${PGID_FILE}"
|
||||
exit "${status}"
|
||||
|
||||
+53
-74
@@ -22,83 +22,61 @@ set -uo pipefail
|
||||
LIST_ONLY=0
|
||||
[ "${1:-}" = "--list" ] && LIST_ONLY=1
|
||||
|
||||
# 这些模式覆盖仿真栈的全部进程：
|
||||
# gz sim / ign gazebo 服务端与 GUI（conda 的 Fortress 里可执行名是 ign gazebo）
|
||||
# gazebo_sim.launch.py ros2 launch 的壳
|
||||
# gazebo_sim_launcher 入口可执行文件（展开描述后转交 launch）
|
||||
# parameter_bridge 相机/触觉/clock 的桥接
|
||||
# sim_camera_adapter / sim_tactile_adapter 两个适配器
|
||||
# robot_state_publisher 发 TF 与 /robot_description
|
||||
# controller_manager.*spawner / gz_ros2_control 控制器
|
||||
# \"哪些进程算仿真栈\"这张表**不在本文件里**，在 scripts/lib/sim_procs.sh：
|
||||
# 它同时被 run_sim_gz.sh（启动前的预检）使用，两处共用一份实现，不会漂移。
|
||||
#
|
||||
# ⚠️ 这里漏过一个（robot_state_publisher），症状是"脚本说已停止，但它还在跑"：
|
||||
# 下一次启动时它继续发上一份 /robot_description 与 TF，而两个进程的
|
||||
# 参数可能不同 —— 表现成"改了描述却不生效"，极难往"有个残留进程"上想。
|
||||
# 所以脚本末尾加了一次**复查**：杀完再扫一遍，有残留就报出来并非零退出。
|
||||
# 加进新进程时，务必确认复查也能扫到它。
|
||||
# 历史教训（都真实发生过，所以规则才写这么细）：
|
||||
# - 漏过 robot_state_publisher：脚本说"已停止"，它还在跑；下一次启动时
|
||||
# 继续发上一份 /robot_description 与 TF，表现成"改了描述却不生效"。
|
||||
# - 漏过 static_transform_publisher：更阴 —— 它发的是 latched 的 /tf_static，
|
||||
# 残留进程会一直把**旧相机外参**按在新仿真的 TF 树上。
|
||||
# - 漏过 sim_gripper_adapter：残留的适配器与新适配器**同时**订阅 /joint_states、
|
||||
# 同时发布 /gripper/status，服务调用还会被随机分给其中一个。
|
||||
# 验收脚本读到的开口恒为 0 mm、指尖间距 0.03 mm（实际 127 mm），
|
||||
# 看起来就像夹爪代码算错了 —— 这是最难查的一类残留。
|
||||
#
|
||||
# 有意**不**包含 rviz2：本仿真栈的 headless 模式不起 RViz，而用户可能同时
|
||||
# 开着 RViz 看别的东西。把它写进来会误杀无关进程，比漏杀更糟。
|
||||
PATTERNS='ign gazebo|gz sim|gazebo_sim\.launch\.py|gazebo_sim_launcher|parameter_bridge|sim_camera_adapter|sim_tactile_adapter|robot_state_publisher|controller_manager.*spawner|gz_ros2_control'
|
||||
# 所以停止分两步：
|
||||
# ① 主通道：run_sim_gz.sh 用 setsid 起仿真，PGID 记在 log_sim/sim.pgid，
|
||||
# `kill -TERM -PGID` 一次带走整组（**不依赖任何模式串**）；
|
||||
# ② 兜底：模式扫描（点名表 + install_sim/ 兜底），收拾历史孤儿与
|
||||
# ign 自己 fork 出去的进程；杀完再扫一遍，有残留就报出来并非零退出。
|
||||
# shellcheck source=lib/sim_procs.sh
|
||||
source "$(dirname "${BASH_SOURCE[0]}")/lib/sim_procs.sh"
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 扫描：输出 "PID<TAB>命令行"，供两次调用（收集 / 复查）共用。
|
||||
#
|
||||
# 两处必须共用同一个实现，否则"复查用的模式"和"杀的时候用的模式"会各自漂移，
|
||||
# 而漂移的表现就是"说已停止、其实有残留"——正是这个脚本要防的事。
|
||||
#
|
||||
# ⚠️ 模式串用**环境变量**传给 awk，不用 `-v pat=...`：
|
||||
# 用了 -v 的话，awk 自己的命令行里就含整串模式，于是它匹配到自己。
|
||||
# 这是 pkill -f 那个坑的同款，只是换了个位置 —— 实测表现为每次复查都会
|
||||
# 多报出一个"没有命令行"的幽灵 PID（awk 已经退出，ps 查不到它了）。
|
||||
#
|
||||
# 另外用 ps 而不是 pgrep -f：ps 的输出里可以用 awk 精确控制字段与排除规则。
|
||||
scan_pids() {
|
||||
# ⚠️ 必须 export：`VAR=x ps ... | awk` 只会把 VAR 给 ps，awk 看不到。
|
||||
export PAT="$PATTERNS"
|
||||
ps -eo pid=,ppid=,args= | awk '
|
||||
BEGIN { pat = ENVIRON["PAT"]; me = ENVIRON["ME"] }
|
||||
$0 ~ pat {
|
||||
if (NF < 3) next # 没有 args 字段（僵尸/内核线程）：跳过
|
||||
pid = $1; ppid = $2
|
||||
args = substr($0, index($0, $3))
|
||||
if (ppid == me) next # 本脚本的直接子进程（进程替换 fork 出的子 shell）
|
||||
if (args ~ /stop_sim_gz\.sh/) next # 本脚本自己 / 调用它的壳的文本
|
||||
if (args ~ /^\s*awk/) next # 扫描用的 awk 本身
|
||||
print pid "\t" args
|
||||
}'
|
||||
}
|
||||
export ME=$$
|
||||
PGID="$(sim_pgid_read)"
|
||||
if [ -n "$PGID" ] && sim_pgid_alive "$PGID"; then
|
||||
if ! sim_pgid_leader_is_ours "$PGID"; then
|
||||
echo "⚠ log_sim/sim.pgid 里的进程组 $PGID 看起来不是仿真栈（PID 可能被复用），忽略它。"
|
||||
PGID=""
|
||||
fi
|
||||
else
|
||||
PGID=""
|
||||
fi
|
||||
|
||||
# 排除自己与全部祖先 —— 这是本脚本存在的核心理由（见文件头）。
|
||||
exclude=()
|
||||
p=$$
|
||||
while [ "$p" -gt 1 ] 2>/dev/null; do
|
||||
exclude+=("$p")
|
||||
p=$(ps -o ppid= -p "$p" 2>/dev/null | tr -d ' ')
|
||||
[ -z "$p" ] && break
|
||||
done
|
||||
if [ "$LIST_ONLY" = "1" ]; then
|
||||
if [ -n "$PGID" ]; then
|
||||
echo "仿真的进程组：$PGID（kill -TERM -$PGID 可整组停掉）"
|
||||
ps -o pid=,args= -g "$PGID" 2>/dev/null | cut -c1-150
|
||||
fi
|
||||
fi
|
||||
|
||||
# 从扫描结果里剔掉自己/祖先，只输出 PID。
|
||||
# 用 `cut -f1` 而不是 read 到数组再拼接，避免命令行里再出现长的进程描述。
|
||||
filter_targets() {
|
||||
local pid
|
||||
while read -r pid _rest; do
|
||||
[ -z "$pid" ] && continue
|
||||
local skip=0
|
||||
for e in "${exclude[@]}"; do
|
||||
[ "$pid" = "$e" ] && skip=1 && break
|
||||
done
|
||||
[ "$skip" = "1" ] && continue
|
||||
echo "$pid"
|
||||
done
|
||||
}
|
||||
# 模式扫描的候选（--list 时先打印，再决定动不动手）。
|
||||
mapfile -t targets < <(sim_proc_targets)
|
||||
|
||||
mapfile -t SCAN < <(scan_pids)
|
||||
mapfile -t targets < <(printf '%s\n' "${SCAN[@]:-}" | filter_targets)
|
||||
if [ "${#targets[@]}" = "0" ] && [ -z "$PGID" ]; then
|
||||
echo "没有正在运行的仿真进程（已排除本脚本自身与调用它的 shell）。"
|
||||
exit 0
|
||||
fi
|
||||
|
||||
if [ -n "$PGID" ] && [ "$LIST_ONLY" != "1" ]; then
|
||||
echo "停止仿真进程组 $PGID（整组，含不在 install_sim/ 下的节点）…"
|
||||
sim_pgid_stop "$PGID"
|
||||
# 组信号发完后重新扫一遍：可能还有孤儿。
|
||||
mapfile -t targets < <(sim_proc_targets)
|
||||
fi
|
||||
|
||||
if [ "${#targets[@]}" = "0" ]; then
|
||||
echo "没有正在运行的仿真进程（已排除本脚本自身与调用它的 shell）。"
|
||||
echo "仿真已停止。"
|
||||
exit 0
|
||||
fi
|
||||
|
||||
@@ -136,15 +114,16 @@ done
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 复查：杀完再扫一遍。
|
||||
# 这一步是为了让"模式漏了某个进程"变成**看得见的告警**，而不是
|
||||
# "脚本说已停止、实际有残留"。robot_state_publisher 就这么漏过一次。
|
||||
# 这一步是为了让"表漏了某个进程"变成**看得见的告警**，而不是
|
||||
# "脚本说已停止、实际有残留"。robot_state_publisher 与 sim_gripper_adapter
|
||||
# 都这么漏过，而后者导致的假象是"夹爪算错了"。
|
||||
# ---------------------------------------------------------------------------
|
||||
sleep 0.5
|
||||
mapfile -t SCAN2 < <(scan_pids)
|
||||
mapfile -t leftover < <(printf '%s\n' "${SCAN2[@]:-}" | filter_targets)
|
||||
mapfile -t leftover < <(sim_proc_targets)
|
||||
|
||||
if [ "${#leftover[@]}" != "0" ]; then
|
||||
echo "⚠ 仍有仿真相关进程存活（说明上面的模式没覆盖它，请把它加进 PATTERNS）："
|
||||
echo "⚠ 仍有仿真相关进程存活（说明它既不在 lib/sim_procs.sh 的点名表里，"
|
||||
echo " 也不在 install_sim/ 下 —— 请把它加进点名表，或让它从 install_sim/ 启动）："
|
||||
for pid in "${leftover[@]}"; do
|
||||
printf ' %-7s %s\n' "$pid" "$(ps -o args= -p "$pid" 2>/dev/null | cut -c1-140)"
|
||||
done
|
||||
|
||||
Reference in New Issue
Block a user