把当前控制栈收成一份可克隆的源码：腕部看不见时用顶视一次定到基座，二阶段对孔和手外标定跟着进同一份树。
现场 site.yaml、权重和编译产物仍不入库；模板只留占位符。 Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
+402
-25
@@ -272,7 +272,12 @@ def camera_ns(data: dict) -> str:
|
||||
|
||||
|
||||
def camera_frames(data: dict) -> tuple[str, str, str]:
|
||||
"""当前机械臂的（命名空间, 光学坐标系, 深度话题）。"""
|
||||
"""当前机械臂**腕部**相机的（命名空间, 光学坐标系, 深度话题）。
|
||||
|
||||
一阶段跟踪用哪台相机见 ``track_camera_frames`` —— 不许改本函数去跟
|
||||
``perception.track_camera``：``SITE_CAM_NAME`` / 二阶段 / 入孔检测都走这里，
|
||||
切跟踪相机不该把精拍那路也切走。
|
||||
"""
|
||||
ns = camera_ns(data)
|
||||
yolo = data.get("yolo") or {}
|
||||
frame = str(yolo.get("camera_frame") or "").strip()
|
||||
@@ -284,6 +289,218 @@ def camera_frames(data: dict) -> tuple[str, str, str]:
|
||||
return ns, frame, depth
|
||||
|
||||
|
||||
#: 顶视跟踪必须自己写的深度量程。缺了就借腕部 0.55m 上限 = D435 的点全丢。
|
||||
_OVERHEAD_RANGE_REQUIRED = ("min_depth_m", "max_depth_m", "feature_min_range_m")
|
||||
#: D405 主跟踪切 D435 一次性时必须显式写的键。缺了就静默不切 = 臂停在半空。
|
||||
_ONESHOT_REQUIRED = (
|
||||
"oneshot_conf",
|
||||
"cooldown_s",
|
||||
"frame_max_age_s",
|
||||
"oneshot_hold_s",
|
||||
)
|
||||
|
||||
|
||||
def track_camera_frames(data: dict) -> tuple[str, str, str, str]:
|
||||
"""一阶段跟踪用的（命名空间, 光学坐标系, 深度话题, 手眼模式）。
|
||||
|
||||
``perception.track_camera``：
|
||||
* ``wrist`` → 腕部相机 + ``eye_in_hand``
|
||||
* ``overhead`` → 手外相机 + ``eye_to_hand``
|
||||
"""
|
||||
data, robot = apply_active_arm(data)
|
||||
sio = _load_site_io()
|
||||
role = sio.track_camera_role(data)
|
||||
mode = sio.track_handeye_mode(role)
|
||||
if role == "overhead":
|
||||
over = overhead_camera(data, robot)
|
||||
ns = str(over.get("name") or "").strip("/")
|
||||
if not ns:
|
||||
raise RuntimeError(
|
||||
"perception.track_camera=overhead，但 cameras 里没有 "
|
||||
"role 以 overhead 开头的相机"
|
||||
)
|
||||
return ns, f"{ns}_color_optical_frame", f"/{ns}/aligned_depth_to_color/image_raw", mode
|
||||
ns, frame, depth = camera_frames(data)
|
||||
return ns, frame, depth, mode
|
||||
|
||||
|
||||
def _track_handeye_file(data: dict, robot: dict, mode: str) -> str:
|
||||
"""跟踪用的手眼文件。overhead 不许去读 ``paths.handeye_file``（那是腕部的）。"""
|
||||
if mode != "eye_to_hand":
|
||||
return str((data.get("paths") or {}).get("handeye_file") or "")
|
||||
over = overhead_camera(data, robot)
|
||||
cam = str(over.get("name") or "").strip("/")
|
||||
short = arm_short_id(str(robot.get("sn") or ""))
|
||||
name = _load_site_io().eye_to_hand_result_name(short, cam)
|
||||
return str(_root() / "handeye" / "results" / name)
|
||||
|
||||
|
||||
def _apply_overhead_range(data: dict, params: dict) -> None:
|
||||
"""把 ``perception.overhead`` 的深度量程盖到感知参数上。缺键就红。"""
|
||||
oh = (data.get("perception") or {}).get("overhead")
|
||||
if not isinstance(oh, dict):
|
||||
raise RuntimeError(
|
||||
"perception.track_camera=overhead 必须在 perception.overhead 写顶视"
|
||||
"深度量程（min_depth_m / max_depth_m / feature_min_range_m）。"
|
||||
"腕部那组 0.55m 上限会把 D435 的点全丢掉，看起来像检测失败。"
|
||||
)
|
||||
missing = [k for k in _OVERHEAD_RANGE_REQUIRED if k not in oh]
|
||||
if missing:
|
||||
raise RuntimeError(
|
||||
f"perception.overhead 缺 {missing} —— 不许借腕部量程。"
|
||||
)
|
||||
params["min_depth_m"] = float(oh["min_depth_m"])
|
||||
params["max_depth_m"] = float(oh["max_depth_m"])
|
||||
params["feature_min_range_m"] = float(oh["feature_min_range_m"])
|
||||
if "work_z_max_m" in oh:
|
||||
params["work_z_max_m"] = float(oh["work_z_max_m"])
|
||||
if "z_ref_m" in oh:
|
||||
params["z_ref_m"] = float(oh["z_ref_m"])
|
||||
if "feature_mode" in oh:
|
||||
params["feature_mode"] = str(oh["feature_mode"])
|
||||
if "hold_point_on_miss" in oh:
|
||||
params["hold_point_on_miss"] = bool(oh["hold_point_on_miss"])
|
||||
|
||||
|
||||
def _apply_wrist_hybrid(data: dict, robot: dict, params: dict) -> None:
|
||||
"""腕部主跟踪 + D435 一次性：参数接到感知节点，**不许盖掉腕部量程**。
|
||||
|
||||
顶视量程写进 ``overhead_*`` 键。若直接复用 ``min_depth_m``，D405 会按
|
||||
0.25–1.80 m 收点，近处工件全丢，看起来像检测失败。
|
||||
"""
|
||||
perc = data.get("perception") or {}
|
||||
if "wrist_handoff_conf" not in perc:
|
||||
raise RuntimeError(
|
||||
"perception.track_camera=wrist 的混合跟踪必须写 "
|
||||
"perception.wrist_handoff_conf（画面内最高 conf 低于此才切 D435）。"
|
||||
"不许在代码里写死 0.5 再抄一份到 yaml。"
|
||||
)
|
||||
params["wrist_handoff_conf"] = float(perc["wrist_handoff_conf"])
|
||||
oh = perc.get("overhead")
|
||||
if not isinstance(oh, dict):
|
||||
raise RuntimeError(
|
||||
"wrist 混合跟踪必须在 perception.overhead 写顶视量程和一次性参数"
|
||||
"（min_depth_m / max_depth_m / feature_min_range_m / oneshot_conf / "
|
||||
"cooldown_s / frame_max_age_s / oneshot_hold_s）。"
|
||||
)
|
||||
missing = [k for k in _OVERHEAD_RANGE_REQUIRED if k not in oh]
|
||||
if missing:
|
||||
raise RuntimeError(
|
||||
f"perception.overhead 缺 {missing} —— 不许借腕部量程做 D435 一次性。"
|
||||
)
|
||||
miss2 = [k for k in _ONESHOT_REQUIRED if k not in oh]
|
||||
if miss2:
|
||||
raise RuntimeError(f"perception.overhead 缺一次性键 {miss2} —— 不许用代码默认值。")
|
||||
over = overhead_camera(data, robot)
|
||||
ns = str(over.get("name") or "").strip("/")
|
||||
if not ns:
|
||||
raise RuntimeError(
|
||||
"wrist 混合跟踪需要 cameras 里 role 以 overhead 开头的相机"
|
||||
)
|
||||
e2h = _track_handeye_file(data, robot, "eye_to_hand")
|
||||
params["overhead_oneshot_enable"] = True
|
||||
params["overhead_min_depth_m"] = float(oh["min_depth_m"])
|
||||
params["overhead_max_depth_m"] = float(oh["max_depth_m"])
|
||||
params["overhead_feature_min_range_m"] = float(oh["feature_min_range_m"])
|
||||
params["overhead_oneshot_conf"] = float(oh["oneshot_conf"])
|
||||
params["overhead_cooldown_s"] = float(oh["cooldown_s"])
|
||||
params["overhead_frame_max_age_s"] = float(oh["frame_max_age_s"])
|
||||
params["overhead_hold_s"] = float(oh["oneshot_hold_s"])
|
||||
params["overhead_handeye_file"] = e2h
|
||||
params["overhead_camera_source"] = ns
|
||||
params["overhead_camera_frame"] = f"{ns}_color_optical_frame"
|
||||
params["overhead_image_topic"] = f"/{ns}/color/image_raw"
|
||||
params["overhead_info_topic"] = f"/{ns}/color/camera_info"
|
||||
params["overhead_depth_topic"] = f"/{ns}/aligned_depth_to_color/image_raw"
|
||||
|
||||
|
||||
#: 手眼标定的两种装法。写在这里当"合法值清单"，避免各处各写一遍字符串。
|
||||
HANDEYE_MODES = ("eye_in_hand", "eye_to_hand")
|
||||
|
||||
|
||||
def handeye_board(he: dict, mode: str) -> tuple[dict, str]:
|
||||
"""取某个模式用的标定板尺寸 → ``({cols, rows, square_size_m}, 来源)``。
|
||||
|
||||
权威只有一处：``handeye.boards.<mode>``。为什么按模式分开——两种装法用的是
|
||||
**两块不同的物理板**（本项目：眼在手上 A5 / 眼在手外 A4），方格边长不一样。
|
||||
合成一组键的后果不是报错，而是"跑另一种模式时静默用错边长"，结果只是系统性偏，
|
||||
现场几乎联想不到板尺寸。
|
||||
|
||||
兼容：没有 ``boards`` 段时，回落到旧的顶层 ``board_cols/rows/square_size_m``。
|
||||
返回的第二个值是来源（``boards`` / ``legacy`` / ``default``），调用方据此决定
|
||||
要不要拦下——``default`` 意味着**没人配过这块板**，此时开标定等于赌一个尺寸。
|
||||
"""
|
||||
boards = he.get("boards") if isinstance(he.get("boards"), dict) else {}
|
||||
node = boards.get(mode) if isinstance(boards, dict) else None
|
||||
src = "boards"
|
||||
if not isinstance(node, dict):
|
||||
node = he
|
||||
src = "legacy"
|
||||
if mode not in HANDEYE_MODES:
|
||||
# 不认识的模式：不猜板子，交给调用方报错（猜错就是静默用错尺寸）。
|
||||
node, src = {}, "default"
|
||||
|
||||
def pick(key: str, cast, default):
|
||||
v = node.get(key) if isinstance(node, dict) else None
|
||||
if v is None:
|
||||
return default, False
|
||||
try:
|
||||
return cast(v), True
|
||||
except (TypeError, ValueError):
|
||||
return default, False
|
||||
|
||||
# boards 段用 cols/rows；旧的顶层键是 board_cols/board_rows。两种都认，
|
||||
# 免得"兼容旧配置"其实读不到旧键，悄悄退回默认 8×11。
|
||||
cols, ok_c = pick("cols", int, 8)
|
||||
if not ok_c:
|
||||
cols, ok_c = pick("board_cols", int, 8)
|
||||
rows, ok_r = pick("rows", int, 11)
|
||||
if not ok_r:
|
||||
rows, ok_r = pick("board_rows", int, 11)
|
||||
sq, ok_s = pick("square_size_m", float, 0.015)
|
||||
if src == "legacy" and not (ok_c or ok_r or ok_s):
|
||||
# 顶层键一个都没有 ⇒ 真的没人配过，不要把"内置默认值"伪装成"配置"。
|
||||
src = "default"
|
||||
return {"cols": cols, "rows": rows, "square_size_m": sq}, src
|
||||
|
||||
|
||||
def overhead_camera(data: dict, robot: dict | None = None) -> dict:
|
||||
"""手外（固定）相机：``cameras.*`` 里 ``role`` 以 ``overhead`` 开头的那台。
|
||||
|
||||
**规则只有一份**，在 ``arm_application.site_io.overhead_camera``——S03 粗定位
|
||||
取命名空间走的也是它。这里若自己再写一遍"按 role 找 + 按 arm 收敛"，两份判据
|
||||
必然漂移，症状是"标定用的相机"和"推理用的相机"不是同一台，且错而不报。
|
||||
所以这里只做一层薄封装（从源码树 import），拿到配置字典。
|
||||
"""
|
||||
fn = _load_overhead_camera_fn()
|
||||
cam, _others = fn(data, robot)
|
||||
return cam
|
||||
|
||||
|
||||
def _load_site_io():
|
||||
"""从 ``src/arm_application`` 取 ``site_io`` 模块。
|
||||
|
||||
为什么从源码树而不是已安装包：``export_site_env.py`` 在 ``colcon build``
|
||||
之前就被调用（``load_site_config.sh`` 的第一件事），那时 install 里可能还没有
|
||||
这个包。源码树在本仓库里恒定存在（``scripts/`` 与 ``src/`` 同根）。
|
||||
"""
|
||||
src = _root() / "src" / "arm_application"
|
||||
if not (src / "arm_application" / "site_io.py").is_file():
|
||||
raise RuntimeError(
|
||||
f"找不到 {src}/arm_application/site_io.py —— 手外相机 / 结果文件名的规则"
|
||||
"只有那一处，不接受本文件自己再实现一份（两份判据必然漂移）。"
|
||||
)
|
||||
if str(src) not in sys.path:
|
||||
sys.path.insert(0, str(src))
|
||||
from arm_application import site_io as _mod # noqa: PLC0415
|
||||
|
||||
return _mod
|
||||
|
||||
|
||||
def _load_overhead_camera_fn():
|
||||
return _load_site_io().overhead_camera
|
||||
|
||||
|
||||
def flatten(data: dict) -> dict:
|
||||
"""供 bash 脚本使用的扁平 ``SITE_*`` 环境变量映射。"""
|
||||
data, robot = apply_active_arm(data)
|
||||
@@ -313,7 +530,59 @@ def flatten(data: dict) -> dict:
|
||||
# 两边会共用同一个手眼文件，表现是"标定做了但不生效"。
|
||||
short = arm_short_id(str(robot.get("sn") or ""))
|
||||
he_mode = str(he.get("mode") or "eye_in_hand")
|
||||
he_stem = f"{short}_{he_mode}" if short else he_mode
|
||||
over = overhead_camera(data, robot)
|
||||
over_name = str(over.get("name") or "").strip("/")
|
||||
root = _root()
|
||||
|
||||
# 手眼标定的"按模式"派生物：结果文件 / 示教点 / 标定板。
|
||||
#
|
||||
# 为什么一切都要按模式分开（eye_in_hand / eye_to_hand）：
|
||||
# 1) 结果文件：眼在手外的产物是"**这台相机的光心 → 这条臂的基座**"，换条臂
|
||||
# （基座不同）或换台相机都不通用 ⇒ 名字要同时带臂号与相机名
|
||||
# （<臂号>_<相机>_eye_to_hand）。曾经只有 <臂号>_<模式>，于是一个双臂共用
|
||||
# 的手外相机只能存一份外参，第二条臂拿到的必然错；
|
||||
# 2) 标定板：两种装法用两块不同的物理板（A5 / A4），方格边长不同 ⇒ 见
|
||||
# handeye_board() 的说明；
|
||||
# 3) 示教点：两种模式采的关节角完全不同，共用一份会把上一模式的点位当成本
|
||||
# 模式的（merge 之后才发现在动错的姿态）。
|
||||
he_modes_out: dict[str, str] = {}
|
||||
_e2h_name = _load_site_io().eye_to_hand_result_name
|
||||
for _m in HANDEYE_MODES:
|
||||
_tag = _m.upper()
|
||||
_pose_stem = f"{short}_{_m}" if short else _m
|
||||
# 结果文件名：眼在手外走 site_io.eye_to_hand_result_name（唯一口径）；
|
||||
# 眼在手上仍是 <臂号>_eye_in_hand.yaml。
|
||||
if _m == "eye_to_hand":
|
||||
_result_name = _e2h_name(short, over_name)
|
||||
else:
|
||||
_result_name = f"{_pose_stem}.yaml"
|
||||
# eye_to_hand 允许用显式路径覆盖（与 eye_in_hand 的 paths.handeye_file 对称）；
|
||||
# 不写就用派生的名字。
|
||||
if _m == "eye_in_hand":
|
||||
_file = str(
|
||||
paths.get("handeye_file")
|
||||
or root / "handeye" / "results" / _result_name
|
||||
)
|
||||
_wps = str(
|
||||
paths.get("handeye_waypoints")
|
||||
or root / "handeye" / "poses" / f"{_pose_stem}.yaml"
|
||||
)
|
||||
else:
|
||||
_file = str(
|
||||
paths.get(f"handeye_file_{_m}")
|
||||
or root / "handeye" / "results" / _result_name
|
||||
)
|
||||
_wps = str(
|
||||
paths.get(f"handeye_waypoints_{_m}")
|
||||
or root / "handeye" / "poses" / f"{_pose_stem}.yaml"
|
||||
)
|
||||
_board, _src = handeye_board(he, _m)
|
||||
he_modes_out[f"SITE_HANDEYE_FILE_{_tag}"] = _file
|
||||
he_modes_out[f"SITE_HANDEYE_WAYPOINTS_{_tag}"] = _wps
|
||||
he_modes_out[f"SITE_HANDEYE_BOARD_COLS_{_tag}"] = str(_board["cols"])
|
||||
he_modes_out[f"SITE_HANDEYE_BOARD_ROWS_{_tag}"] = str(_board["rows"])
|
||||
he_modes_out[f"SITE_HANDEYE_SQUARE_{_tag}"] = str(_board["square_size_m"])
|
||||
he_modes_out[f"SITE_HANDEYE_BOARD_SRC_{_tag}"] = _src
|
||||
|
||||
out = {
|
||||
"SITE_CONFIG": str(site_path()),
|
||||
@@ -339,10 +608,7 @@ def flatten(data: dict) -> dict:
|
||||
),
|
||||
"SITE_YOLO_WEIGHTS": weights,
|
||||
"SITE_YOLO_WEIGHTS_RESOLVED": weights_resolved,
|
||||
"SITE_HANDEYE_WAYPOINTS": str(
|
||||
paths.get("handeye_waypoints")
|
||||
or str(_root() / "handeye" / "poses" / f"{he_stem}.yaml")
|
||||
),
|
||||
"SITE_HANDEYE_WAYPOINTS": he_modes_out["SITE_HANDEYE_WAYPOINTS_EYE_IN_HAND"],
|
||||
"SITE_HANDEYE_CALIBRATE": str(
|
||||
paths.get("handeye_calibrate")
|
||||
or str(_root() / "handeye" / "calibrate.yaml")
|
||||
@@ -355,18 +621,24 @@ def flatten(data: dict) -> dict:
|
||||
paths.get("handeye_workdir")
|
||||
or str(_root() / "handeye" / "tmp" / short)
|
||||
),
|
||||
"SITE_HANDEYE_FILE": str(
|
||||
paths.get("handeye_file")
|
||||
or str(_root() / "handeye" / "results" / f"{he_stem}.yaml")
|
||||
),
|
||||
# 不带后缀的两个名字保持旧语义（= 腕部 eye-in-hand 的产物），因为
|
||||
# launch / 其它脚本一直按这两个名字读；换成"当前模式"会让 site.yaml 里
|
||||
# 一改 handeye.mode 就把 S05 精拍定位的 TF 换掉，那是另一个功能的静默失效。
|
||||
# 两个模式各自的名字在 *_EYE_IN_HAND / *_EYE_TO_HAND 里。
|
||||
"SITE_HANDEYE_FILE": he_modes_out["SITE_HANDEYE_FILE_EYE_IN_HAND"],
|
||||
"SITE_HANDEYE_MODE": he_mode,
|
||||
# 板尺寸默认值必须与 site.yaml / 结果文件头里的实际板子一致。
|
||||
# 这里原先是 11/16/0.011（一张早期用过、现已不存在的板），与 site.yaml 的
|
||||
# 8/11/0.015 冲突：一旦 site.yaml 缺这几个键就会静默用错尺寸，
|
||||
# 表现为"标定能跑完但结果明显不对"。
|
||||
"SITE_HANDEYE_BOARD_COLS": str(int(he.get("board_cols") or 8)),
|
||||
"SITE_HANDEYE_BOARD_ROWS": str(int(he.get("board_rows") or 11)),
|
||||
"SITE_HANDEYE_SQUARE_M": str(float(he.get("square_size_m") or 0.015)),
|
||||
**he_modes_out,
|
||||
# 不带后缀的板尺寸 = **当前模式**那一组，供 `show` 之类只看当前模式的调用方；
|
||||
# 标定脚本用带后缀的那三组（按 --mode 选），不依赖这个。
|
||||
"SITE_HANDEYE_BOARD_COLS": he_modes_out[
|
||||
f"SITE_HANDEYE_BOARD_COLS_{he_mode.upper()}"
|
||||
],
|
||||
"SITE_HANDEYE_BOARD_ROWS": he_modes_out[
|
||||
f"SITE_HANDEYE_BOARD_ROWS_{he_mode.upper()}"
|
||||
],
|
||||
"SITE_HANDEYE_SQUARE_M": he_modes_out[
|
||||
f"SITE_HANDEYE_SQUARE_{he_mode.upper()}"
|
||||
],
|
||||
"SITE_HANDEYE_VEL_SCALE": str(float(he.get("velocity_scale") or 0.05)),
|
||||
"SITE_HANDEYE_MOVE_T": str(float(he.get("move_time_sec") or 8.0)),
|
||||
"SITE_HANDEYE_SETTLE_SEC": str(float(he.get("settle_sec") or 1.5)),
|
||||
@@ -396,6 +668,21 @@ def flatten(data: dict) -> dict:
|
||||
"SITE_CAM_LASER_POWER": str(float(cam.get("laser_power") if cam.get("laser_power") is not None else 0)),
|
||||
"SITE_CAM_EXPOSURE_US": str(float(cam.get("exposure_us") or 0)),
|
||||
"SITE_CAM_GAIN": str(float(cam.get("gain") or 0)),
|
||||
# 手外（固定）相机的**按角色**导出。eye-to-hand 标定要用它：标定时得订阅
|
||||
# 这台固定相机，而不是腕部相机（run_handeye.sh 原来恒用 SITE_CAM_NAME，
|
||||
# 于是 `--mode eye_to_hand` 会去开腕部相机 —— 标出一个"看着成功"的错结果）。
|
||||
"SITE_OVERHEAD_CAM_NAME": over_name,
|
||||
"SITE_OVERHEAD_CAM_SERIAL": str(over.get("serial") or ""),
|
||||
"SITE_OVERHEAD_CAM_WIDTH": str(int(over.get("width") or 1280)),
|
||||
"SITE_OVERHEAD_CAM_HEIGHT": str(int(over.get("height") or 720)),
|
||||
"SITE_OVERHEAD_CAM_FPS": str(int(over.get("fps") or 30)),
|
||||
"SITE_OVERHEAD_CAM_AUTO_EXPOSURE": "1" if over.get("auto_exposure", True) else "0",
|
||||
"SITE_OVERHEAD_CAM_AE_LIMIT_US": str(
|
||||
float(over.get("ae_limit_us") if over.get("ae_limit_us") is not None else 8000)
|
||||
),
|
||||
"SITE_OVERHEAD_CAM_EMITTER": "1" if over.get("emitter_enabled", True) else "0",
|
||||
"SITE_OVERHEAD_CAM_EXPOSURE_US": str(float(over.get("exposure_us") or 0)),
|
||||
"SITE_OVERHEAD_CAM_GAIN": str(float(over.get("gain") or 0)),
|
||||
"SITE_D435_SERIAL": str(((data.get("cameras") or {}).get("d435") or {}).get("serial") or ""),
|
||||
"SITE_D435_WIDTH": str(int(((data.get("cameras") or {}).get("d435") or {}).get("width") or 1280)),
|
||||
"SITE_D435_HEIGHT": str(int(((data.get("cameras") or {}).get("d435") or {}).get("height") or 720)),
|
||||
@@ -417,6 +704,14 @@ def flatten(data: dict) -> dict:
|
||||
"SITE_EE_Z_M": str(float(servo.get("ee_z_m") or 0.24)),
|
||||
"SITE_SERVO_WATCHDOG": str(servo.get("watchdog_timeout_sec") or 0.3),
|
||||
"SITE_CAMERA_FRAME": camera_frames(data)[1],
|
||||
# 一阶段跟踪用的相机/手眼（可与腕部精拍不同）。SITE_CAM_NAME 仍是腕部。
|
||||
"SITE_TRACK_CAMERA": _load_site_io().track_camera_role(data),
|
||||
"SITE_TRACK_CAM_NAME": track_camera_frames(data)[0],
|
||||
"SITE_TRACK_CAMERA_FRAME": track_camera_frames(data)[1],
|
||||
"SITE_TRACK_HANDEYE_MODE": track_camera_frames(data)[3],
|
||||
"SITE_TRACK_HANDEYE_FILE": _track_handeye_file(
|
||||
data, robot, track_camera_frames(data)[3]
|
||||
),
|
||||
"SITE_GRIP_VENDOR": str(grip.get("vendor") or "dh"),
|
||||
"SITE_GRIP_MODEL": str(grip.get("model") or "PGEA-100-20-W"),
|
||||
"SITE_GRIP_SN": str(grip.get("sn") or ""),
|
||||
@@ -490,14 +785,14 @@ def perception_overrides(data: dict) -> dict:
|
||||
data, _robot = apply_active_arm(data)
|
||||
paths = data.get("paths") or {}
|
||||
yolo = data.get("yolo") or {}
|
||||
cam_ns, cam_frame, depth_topic = camera_frames(data)
|
||||
cam_ns, cam_frame, depth_topic, _he_mode = track_camera_frames(data)
|
||||
obj = str(yolo.get("object_frame") or "").strip()
|
||||
if not obj or obj == "tracked_object":
|
||||
# 跟踪目标的坐标系按**臂号**派生（SN 末两位），两侧互不干扰。
|
||||
# 以前是从 SN 里找 "063989"/"063991" 子串，换个站点就失去意义了。
|
||||
short = arm_short_id(str((_robot or {}).get("sn") or ""))
|
||||
obj = f"tracked_object_{short}" if short else "tracked_object"
|
||||
return {
|
||||
out = {
|
||||
"weights": str(paths.get("yolo_weights") or ""),
|
||||
"conf": float(yolo.get("conf") or 0.10),
|
||||
"iou": float(yolo.get("iou") or 0.45),
|
||||
@@ -528,10 +823,36 @@ def perception_overrides(data: dict) -> dict:
|
||||
"object_frame": obj,
|
||||
"depth_topic": depth_topic,
|
||||
"camera_source": cam_ns,
|
||||
"base_frame": str((_robot or {}).get("base_frame") or ""),
|
||||
"feature_mode": str(yolo.get("feature_mode") or "plane"),
|
||||
# feature_mode=keypoint 时用：哪个关键点当跟踪点（k0）、可信度门槛。
|
||||
"keypoint_index": int(yolo.get("keypoint_index") if yolo.get("keypoint_index") is not None else 0),
|
||||
"keypoint_min_conf": float(yolo.get("keypoint_min_conf") or 0.30),
|
||||
"keypoint_win_px": int(yolo.get("keypoint_win_px") if yolo.get("keypoint_win_px") is not None else 6),
|
||||
# 三条关键点量纲守卫（2026-10-08 真机一阶段被假 k0 带走，见
|
||||
# feature_guard.py 的注释）。**必须在这里透传**：节点里声明的默认值
|
||||
# 恰好等于 site.yaml 当前值，漏了不会报错，只会让以后改配置毫无反应
|
||||
# （R1.1 的"声明了却没接线"）。
|
||||
"keypoint_border_px": float(yolo.get("keypoint_border_px") if yolo.get("keypoint_border_px") is not None else 6.0),
|
||||
"track_max_step_m": float(yolo.get("track_max_step_m") if yolo.get("track_max_step_m") is not None else 0.04),
|
||||
"reacquire_far_px": float(yolo.get("reacquire_far_px") if yolo.get("reacquire_far_px") is not None else 120.0),
|
||||
"reacquire_confirm_frames": int(yolo.get("reacquire_confirm_frames") if yolo.get("reacquire_confirm_frames") is not None else 3),
|
||||
"plane_inlier_m": float(yolo.get("plane_inlier_m") or 0.008),
|
||||
"plane_band_m": float(yolo.get("plane_band_m") or 0.06),
|
||||
}
|
||||
sio = _load_site_io()
|
||||
role = sio.track_camera_role(data)
|
||||
perc = data.get("perception") or {}
|
||||
if role == "overhead":
|
||||
_apply_overhead_range(data, out)
|
||||
out["overhead_oneshot_enable"] = False
|
||||
elif perc.get("track_camera") or "wrist_handoff_conf" in perc:
|
||||
# 显式 wrist / 写了混合键 → 必须接全套。缺键就红，不许用代码默认。
|
||||
# 没写 perception 段的探针测试保持旧行为（不加一次性）。
|
||||
_apply_wrist_hybrid(data, _robot, out)
|
||||
else:
|
||||
out["overhead_oneshot_enable"] = False
|
||||
return out
|
||||
|
||||
|
||||
def servo_overrides(data: dict) -> dict:
|
||||
@@ -540,12 +861,20 @@ def servo_overrides(data: dict) -> dict:
|
||||
paths = data.get("paths") or {}
|
||||
yolo = data.get("yolo") or {}
|
||||
servo = data.get("servo") or {}
|
||||
_ns, cam_frame, _depth = camera_frames(data)
|
||||
_ns, cam_frame, _depth, he_mode = track_camera_frames(data)
|
||||
he_file = _track_handeye_file(data, robot, he_mode)
|
||||
xy_track = str(servo.get("xy_track") or "hover").strip().lower()
|
||||
if he_mode == "eye_to_hand" and xy_track == "lookat":
|
||||
raise RuntimeError(
|
||||
"perception.track_camera=overhead 时相机固定，lookat（把目标对准画面中心）"
|
||||
"没有意义。site.yaml 的 servo.xy_track 必须是 hover。"
|
||||
)
|
||||
out = {
|
||||
"joint_prefix": str(robot.get("joint_prefix") or ""),
|
||||
"base_frame": str(robot.get("base_frame") or ""),
|
||||
"ee_frame": str(robot.get("ee_frame") or ""),
|
||||
"handeye_file": str(paths.get("handeye_file") or ""),
|
||||
"handeye_file": he_file,
|
||||
"handeye_mode": he_mode,
|
||||
"camera_frame": cam_frame,
|
||||
"handeye_child_frame": cam_frame,
|
||||
"arm_label": str(robot.get("label") or robot.get("arm") or ""),
|
||||
@@ -555,14 +884,19 @@ def servo_overrides(data: dict) -> dict:
|
||||
keys = [
|
||||
"control_hz", "watchdog_timeout_sec", "lost_timeout_sec", "stale_hold_sec",
|
||||
"max_extrapolate_sec", "predict_enable", "predict_sec",
|
||||
"predict_v_max", "predict_jump_m",
|
||||
"predict_v_max", "predict_jump_m", "jump_confirm_frames", "feat_ws_margin_m",
|
||||
"kp_xy", "ki_xy", "kd_xy", "kp_z", "ki_z", "kd_z", "i_sep_pos_m",
|
||||
"deadzone_m", "deadzone_z_m", "v_max_xy", "v_max_z", "v_approach_z",
|
||||
# 远处限速（见 _slow_xy_limit）：漏了这两个键，site.yaml 改了也不生效，
|
||||
# 节点会静默退回声明默认值（R1.1）。
|
||||
"slow_far_m", "v_slow_xy",
|
||||
"z_min_m", "approach_xy_gate_m", "min_ee_z_m", "e_xy_clip_m",
|
||||
"ee_x_m", "ee_y_m", "ee_z_m", "ee_hold_xy", "ee_vertical_down",
|
||||
"xy_track", "hover_offset_ee_xy_m", "arm_label",
|
||||
"ee_hold_kp", "ee_w_max",
|
||||
"lock_hold_sec",
|
||||
"lock_still_sec",
|
||||
"lock_still_move_m",
|
||||
"a_max", "max_qdot", "damping",
|
||||
"vel_lpf_alpha", "dq_window_size", "traj_stream_hz", "exec_horizon_sec",
|
||||
"min_cmd_delta_rad", "point_topic", "traj_topic",
|
||||
@@ -572,6 +906,38 @@ def servo_overrides(data: dict) -> dict:
|
||||
for k in keys:
|
||||
if k in servo:
|
||||
out[k] = servo[k]
|
||||
sio = _load_site_io()
|
||||
perc = data.get("perception") or {}
|
||||
if sio.track_camera_role(data) == "wrist" and (
|
||||
perc.get("track_camera") or "wrist_handoff_conf" in perc
|
||||
):
|
||||
if "wrist_handoff_conf" not in perc:
|
||||
raise RuntimeError(
|
||||
"servo 混合跟踪必须读 perception.wrist_handoff_conf "
|
||||
"（与感知同一门槛，不许两边各写一个数）。"
|
||||
)
|
||||
out["wrist_handoff_conf"] = float(perc["wrist_handoff_conf"])
|
||||
oh = perc.get("overhead") if isinstance(perc.get("overhead"), dict) else {}
|
||||
if "oneshot_hold_s" not in oh:
|
||||
raise RuntimeError(
|
||||
"servo 混合跟踪必须写 perception.overhead.oneshot_hold_s "
|
||||
"（D435 一次性设定保持多久）。"
|
||||
)
|
||||
out["overhead_hold_s"] = float(oh["oneshot_hold_s"])
|
||||
if "lock_still_sec" not in servo:
|
||||
raise RuntimeError(
|
||||
"servo.lock_still_sec 必须显式写"
|
||||
"（D405 跟住后法兰静止多久才进二阶段）。不许用代码默认 3。"
|
||||
)
|
||||
if "lock_still_move_m" not in servo:
|
||||
raise RuntimeError(
|
||||
"servo.lock_still_move_m 必须显式写"
|
||||
"（静止判据的法兰 XY 位移上限）。不许用代码默认 15mm。"
|
||||
)
|
||||
out["lock_still_sec"] = float(servo["lock_still_sec"])
|
||||
out["lock_still_move_m"] = float(servo["lock_still_move_m"])
|
||||
out["overhead_k0_topic"] = "visual_servo/overhead_k0"
|
||||
out["wrist_conf_topic"] = "visual_servo/wrist_conf"
|
||||
return out
|
||||
|
||||
|
||||
@@ -620,10 +986,21 @@ def main() -> int:
|
||||
print(f" 手眼路点 : {flat['SITE_HANDEYE_WAYPOINTS']}")
|
||||
print(f" 手眼目录 : {flat['SITE_HANDEYE_WORKDIR']}")
|
||||
print(f" 命名位姿 : {flat.get('SITE_NAMED_POSES_FILE')}")
|
||||
print(f" 眼在手外结果 : {flat['SITE_HANDEYE_FILE_EYE_TO_HAND']}")
|
||||
print(f" 眼在手外路点 : {flat['SITE_HANDEYE_WAYPOINTS_EYE_TO_HAND']}")
|
||||
for _m in ("eye_in_hand", "eye_to_hand"):
|
||||
_t = _m.upper()
|
||||
_src = flat.get(f"SITE_HANDEYE_BOARD_SRC_{_t}")
|
||||
print(
|
||||
f" 标定板[{_m:12s}]: "
|
||||
f"{flat.get(f'SITE_HANDEYE_BOARD_COLS_{_t}')}x"
|
||||
f"{flat.get(f'SITE_HANDEYE_BOARD_ROWS_{_t}')} 内角点, "
|
||||
f"square={flat.get(f'SITE_HANDEYE_SQUARE_{_t}')} m"
|
||||
+ (" ← 没配过（标定会被拦下）" if _src == "default" else "")
|
||||
)
|
||||
print(
|
||||
" 标定板 : "
|
||||
f"{flat['SITE_HANDEYE_BOARD_COLS']}x{flat['SITE_HANDEYE_BOARD_ROWS']} "
|
||||
f"内角点, square={flat['SITE_HANDEYE_SQUARE_M']} m"
|
||||
f" 手外相机 : {flat.get('SITE_OVERHEAD_CAM_NAME') or '（未登记）'} "
|
||||
f"serial={flat.get('SITE_OVERHEAD_CAM_SERIAL') or '（空）'}"
|
||||
)
|
||||
print(
|
||||
f" 相机 : serial={flat['SITE_CAM_SERIAL'] or '（空）'} "
|
||||
|
||||
Reference in New Issue
Block a user