orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
adc72955c788f2b9530e704642b9ef98b9d02416
ros_contorl/scripts/lib/export_site_env.py
T
arm_dev a3a589b9e3 P3a：手眼标定集中到 handeye/，修复三套板尺寸矛盾与从未生效的稳定性参数 
新建 handeye/ 专用文件夹（用户要求：专用文件夹 + 使用说明）
- handeye/README.md：从小白视角讲清楚两种模式怎么选、板尺寸怎么量、
  完整流程、出问题先看哪里。以前这些知识散在 docs/ 和代码注释里。
- 结构：calibrate.yaml（参数）/ poses/（采点输入）/ results/（结果输出）/ tmp/（临时，不进 git）
- 文件用 git mv 迁移，历史保留

命名约定改为 <臂号>_<模式>.yaml
- 如 89_eye_in_hand.yaml、91_eye_to_hand.yaml
- 臂号 = Flexiv 序列号后两位；模式 = eye_in_hand | eye_to_hand
- 换模式 = 改 site.yaml 里 paths.handeye_file 一行，不动代码
- 迁移只需改 site.yaml（4 个 launch + servo_node 都经 paths 读取，一行没改）

修复：三套互相矛盾的标定板尺寸（真实 bug）
同一个"板子多大"分散在三处，且值各不相同：
  handeye_calibrate.py 默认 9x6 / 0.025
  handeye/calibrate.yaml   11x16 / 0.011
  site.yaml + 结果文件头     8x11 / 0.015   ← 实测的真实板子
谁生效取决于覆盖顺序。症状是"标定流程跑得很顺、不报错，但结果明显不对"，
极难排查。已全部统一为 8x11 / 0.015，README 里写明换板必须三处同改。

修复：site.yaml 里五项稳定性配置从未生效
run_handeye.sh 一直在传 settle_qd_max / settle_stable_sec / settle_timeout_sec /
pin_holds / pin_hold_sec，但 handeye_calibrate.py 从未声明这些参数 ——
ROS 2 会静默忽略未声明的覆盖参数，所以"等机械臂停稳再拍照"这件事其实没实现，
实际只有一次固定 settle_sec。现已声明并实现：
- _wait_settled()：用关节速度 |qd|_max 判定"真的停了"，比固定 sleep 可靠
  （负载/姿态不同刹车距离差别很大；等太短会在臂还动时拍照导致角点抖动）
- _pin_hold()：静止后再保持几拍，抑制到位后的微小回弹

新增：自动标定进度条
一条可视化进度条（[ 4/12] [████░░░░]  33.3%），无人值守的长流程能一眼看出还剩多久。

清理重复文件（内容逐字节比对确认后删除）
- config/handeye_result.yaml 与 _89 逐字节相同 → 删
- config/handeye_poses.yaml 与 _89 逐字节相同 → 删
迁移后 src/arm_application/config/ 下已无 handeye 文件。

测试：224 passed（原 198，新增 26）
新增 src/arm_application/test/test_handeye_lib.py —— 手眼模块此前零测试。
关键用例是合成数据往返：用人为设定的 X_true 反推出每个视图的
(gripper→base, target→cam)，再让求解器还原。坐标链方向/求逆/转置
只要有一处写错就必然失败。
- 真值恢复（旋转+平移，atol 1e-6）
- 单独验证平移精确（旋转对而平移错是最易漏的一类 bug）
- 结果必须是 det=+1 的正交阵（排除镜像解）
- 退化输入（全同位姿）不得悄悄给出"看起来能用"的结果
- 稳定性判据：窗口未满不判稳、丢帧后重新计数、抖动超阈不判稳
- 写盘再读回不漂移
2026-09-23 11:46:03 +08:00

568 lines
25 KiB
Python
Raw Blame History

#!/usr/bin/env python3
"""Load ~/ros2/config/site.yaml and print bash export lines or JSON.
Usage:
eval "$(python3 ~/ros2/scripts/lib/export_site_env.py)"
python3 ~/ros2/scripts/lib/export_site_env.py --json
python3 ~/ros2/scripts/lib/export_site_env.py --print
"""
from __future__ import annotations
import argparse
import json
import os
import sys
from pathlib import Path
try:
import yaml
except ImportError:
yaml = None # type: ignore
def _root() -> Path:
return Path(__file__).resolve().parents[2]
def site_path() -> Path:
env = os.environ.get("SITE_CONFIG", "").strip()
if env:
return Path(env).expanduser()
return _root() / "config" / "site.yaml"
def load_site(path: Path | None = None) -> dict:
path = path or site_path()
if not path.is_file():
raise FileNotFoundError(f"site config not found: {path}")
text = path.read_text(encoding="utf-8")
if yaml is None:
raise RuntimeError("PyYAML required: pip/conda install pyyaml")
data = yaml.safe_load(text) or {}
if not isinstance(data, dict):
raise ValueError(f"invalid site.yaml: {path}")
return data
def list_robots(data: dict) -> list[dict]:
"""Catalog entries from robot.robots (list or SN-keyed map)."""
raw = (data.get("robot") or {}).get("robots") or []
out: list[dict] = []
if isinstance(raw, dict):
for sn, item in raw.items():
row = dict(item or {}) if isinstance(item, dict) else {}
row.setdefault("sn", str(sn))
out.append(row)
return out
if isinstance(raw, list):
for item in raw:
if isinstance(item, dict) and str(item.get("sn") or "").strip():
out.append(dict(item))
return out
# --arm 89|91|left|right 与 SN 别名。禁止把 89 的手眼/相机套到 91。
_ARM_ALIASES = {
"89": "Rizon4-063989",
"left": "Rizon4-063989",
"左": "Rizon4-063989",
"左臂": "Rizon4-063989",
"左臂89": "Rizon4-063989",
"91": "Rizon4-063991",
"right": "Rizon4-063991",
"右": "Rizon4-063991",
"右臂": "Rizon4-063991",
"右臂91": "Rizon4-063991",
}
_ARM_SECTION_KEYS = ("camera", "gripper", "paths", "servo", "handeye", "go_home", "yolo")
def _deep_merge(base: dict, overlay: dict) -> dict:
out = dict(base)
for k, v in overlay.items():
if isinstance(out.get(k), dict) and isinstance(v, dict):
out[k] = _deep_merge(out[k], v)
else:
out[k] = v
return out
def normalize_robot_sn(data: dict, want: str) -> str:
"""Map 89/91/left/right/label → full Flexiv SN."""
w = (want or "").strip()
if not w:
return w
mapped = _ARM_ALIASES.get(w) or _ARM_ALIASES.get(w.lower())
if mapped:
return mapped
for item in list_robots(data):
sn = str(item.get("sn") or "").strip()
label = str(item.get("label") or "").strip()
arm = str(item.get("arm") or "").strip().lower()
if w == sn or w == label or w.lower() == arm:
return sn
if sn.endswith(w) and w.isdigit() and len(w) >= 2:
return sn
return w
def resolve_robot(data: dict, sn: str | None = None) -> dict:
"""Active arm: ROBOT_SN env overrides robot.sn; catalog fills ip/iface/frames."""
robot = dict(data.get("robot") or {})
catalog = list_robots(data)
want = normalize_robot_sn(
data,
sn or os.environ.get("ROBOT_SN") or os.environ.get("ROBOT_ARM") or robot.get("sn") or "",
)
match: dict = {}
for item in catalog:
if str(item.get("sn") or "").strip() == want:
match = dict(item)
break
out = {k: v for k, v in robot.items() if k != "robots"}
out.update({k: v for k, v in match.items() if k != "robots"})
out["sn"] = want
if not str(out.get("arm") or "").strip():
if "063989" in want:
out["arm"] = "left"
elif "063991" in want:
out["arm"] = "right"
if not str(out.get("joint_prefix") or "").strip() and want:
out["joint_prefix"] = f"{want}_"
if not str(out.get("base_frame") or "").strip() and want:
out["base_frame"] = f"{want}_base_link"
if not str(out.get("ee_frame") or "").strip() and want:
out["ee_frame"] = f"{want}_flange"
net = data.get("network") or {}
if not str(out.get("iface") or "").strip():
out["iface"] = str(net.get("prefer_iface") or "")
if not str(out.get("ip") or "").strip():
out["ip"] = "192.168.100.1"
if not str(out.get("pc_ip") or "").strip():
out["pc_ip"] = "192.168.100.10"
return out
def apply_active_arm(data: dict, sn: str | None = None) -> tuple[dict, dict]:
"""Merge robots[] camera/gripper/paths/servo onto site defaults for the active SN."""
robot = resolve_robot(data, sn)
merged = dict(data)
for key in _ARM_SECTION_KEYS:
base = merged.get(key) if isinstance(merged.get(key), dict) else {}
ov = robot.get(key) if isinstance(robot.get(key), dict) else {}
merged[key] = _deep_merge(dict(base or {}), dict(ov or {}))
return merged, robot
def camera_ns(data: dict) -> str:
"""Topic namespace for the active wrist camera (d405_left / d405_right)."""
cam = data.get("camera") or {}
name = str(cam.get("name") or "d405").strip().strip("/")
return name or "d405"
def camera_frames(data: dict) -> tuple[str, str, str]:
"""(namespace, optical_frame, depth_topic) for the active arm."""
ns = camera_ns(data)
yolo = data.get("yolo") or {}
frame = str(yolo.get("camera_frame") or "").strip()
depth = str(yolo.get("depth_topic") or "").strip()
if frame in ("", "d405_color_optical_frame"):
frame = f"{ns}_color_optical_frame"
if depth in ("", "/d405/aligned_depth_to_color/image_raw"):
depth = f"/{ns}/aligned_depth_to_color/image_raw"
return ns, frame, depth
def flatten(data: dict) -> dict:
"""Flat SITE_* env map for bash scripts."""
data, robot = apply_active_arm(data)
paths = data.get("paths") or {}
cam = data.get("camera") or {}
yolo = data.get("yolo") or {}
servo = data.get("servo") or {}
home = data.get("go_home") or {}
net = data.get("network") or {}
he = data.get("handeye") or {}
grip = data.get("gripper") or {}
named_poses = str(
robot.get("named_poses_file")
or paths.get("named_poses_file")
or str(_root() / "src" / "arm_control" / "config" / "named_poses.yaml")
)
weights = str(paths.get("yolo_weights") or "")
# Resolve active.pt symlink to real path for display
try:
weights_resolved = str(Path(weights).expanduser().resolve())
except Exception:
weights_resolved = weights
out = {
"SITE_CONFIG": str(site_path()),
"SITE_ROS2_ROOT": str(_root()),
"SITE_ROBOT_SN": str(robot.get("sn") or "Rizon4-063989"),
"SITE_ROBOT_IP": str(robot.get("ip") or "192.168.100.1"),
"SITE_PC_IP": str(robot.get("pc_ip") or "192.168.100.10"),
"SITE_JOINT_PREFIX": str(robot.get("joint_prefix") or ""),
"SITE_BASE_FRAME": str(robot.get("base_frame") or ""),
"SITE_EE_FRAME": str(robot.get("ee_frame") or ""),
"SITE_ROBOT_TOOL_NAME": str(robot.get("tool_name") or ""),
"SITE_ROBOT_LABEL": str(robot.get("label") or ""),
"SITE_ROBOT_ARM": str(robot.get("arm") or ""),
"SITE_NAMED_POSES_FILE": named_poses,
"SITE_ROBOT_CATALOG": ",".join(
str(r.get("sn") or "") for r in list_robots(data) if r.get("sn")
),
"SITE_YOLO_WEIGHTS": weights,
"SITE_YOLO_WEIGHTS_RESOLVED": weights_resolved,
"SITE_HANDEYE_WAYPOINTS": str(
paths.get("handeye_waypoints")
or str(_root() / "handeye" / "poses" / "89_eye_in_hand.yaml")
),
"SITE_HANDEYE_CALIBRATE": str(
paths.get("handeye_calibrate")
or str(_root() / "handeye" / "calibrate.yaml")
),
"SITE_HANDEYE_TOUR": str(
paths.get("handeye_tour")
or str(_root() / "handeye" / "waypoints.yaml")
),
"SITE_HANDEYE_WORKDIR": str(
paths.get("handeye_workdir") or "/tmp/handeye_calib"
),
"SITE_HANDEYE_FILE": str(
paths.get("handeye_file")
or str(_root() / "handeye" / "results" / "89_eye_in_hand.yaml")
),
"SITE_HANDEYE_MODE": str(he.get("mode") or "eye_in_hand"),
# 板尺寸默认值必须与 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)),
"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)),
"SITE_HANDEYE_SETTLE_QD": str(float(he.get("settle_qd_max") or 0.015)),
"SITE_HANDEYE_SETTLE_STABLE": str(float(he.get("settle_stable_sec") or 0.6)),
"SITE_HANDEYE_SETTLE_TO": str(float(he.get("settle_timeout_sec") or 8.0)),
"SITE_HANDEYE_PIN_HOLDS": str(int(he.get("pin_holds") if he.get("pin_holds") is not None else 2)),
"SITE_HANDEYE_PIN_SEC": str(float(he.get("pin_hold_sec") or 0.45)),
"SITE_HANDEYE_SKIP_UNSTABLE": (
"true" if he.get("skip_unstable_poses", True) else "false"
),
"SITE_FASTDDS_PROFILE": str(paths.get("fastdds_profile") or ""),
"SITE_CAM_NAME": camera_ns(data),
"SITE_CAM_SERIAL": str(cam.get("serial") or ""),
"SITE_CAM_WIDTH": str(int(cam.get("width") or 848)),
"SITE_CAM_HEIGHT": str(int(cam.get("height") or 480)),
"SITE_CAM_FPS": str(int(cam.get("fps") or 30)),
"SITE_CAM_ENABLE_DEPTH": "1" if cam.get("enable_depth", True) else "0",
"SITE_CAM_ENABLE_INFRA": "1" if cam.get("enable_infra", True) else "0",
"SITE_CAM_PREVIEW": "1" if cam.get("preview", True) else "0",
"SITE_CAM_PREVIEW_STEREO": "1" if cam.get("preview_stereo", True) else "0",
"SITE_CAM_OVERLAY_HOLD": str(float(cam.get("overlay_hold_sec") or 1.5)),
"SITE_CAM_USE_SITE_RES": "1" if cam.get("use_site_resolution", True) else "0",
"SITE_CAM_AUTO_EXPOSURE": "1" if cam.get("auto_exposure", True) else "0",
"SITE_CAM_AE_LIMIT_US": str(float(cam.get("ae_limit_us") if cam.get("ae_limit_us") is not None else 4000)),
"SITE_CAM_EMITTER": "1" if cam.get("emitter_enabled", False) else "0",
"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)),
"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)),
"SITE_D435_FPS": str(int(((data.get("cameras") or {}).get("d435") or {}).get("fps") or 30)),
"SITE_D405_LEFT_SERIAL": str(((data.get("cameras") or {}).get("d405_left") or {}).get("serial") or "352122274408"),
"SITE_D405_RIGHT_SERIAL": str(((data.get("cameras") or {}).get("d405_right") or {}).get("serial") or "260522274442"),
"SITE_YOLO_CONF": str(yolo.get("conf") or 0.35),
"SITE_YOLO_IMGSZ": str(int(yolo.get("imgsz") or 416)),
"SITE_YOLO_DEVICE": str(yolo.get("device") or "0"),
"SITE_YOLO_MIN_CONF": str(yolo.get("min_conf") or 0.35),
"SITE_GO_HOME_POSE": str(home.get("pose") or "home"),
"SITE_GO_HOME_SPEED": str(home.get("speed") or 0.25),
"SITE_NET_IFACE": str(robot.get("iface") or net.get("prefer_iface") or ""),
"SITE_SERVO_Z_REF": str(servo.get("z_ref_m") or 0.40),
"SITE_EE_X_M": str(float(servo.get("ee_x_m") or 0.25)),
"SITE_EE_Y_M": str(float(servo.get("ee_y_m") or 0.0)),
"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_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 ""),
"SITE_GRIP_BACKEND": str(grip.get("backend") or "usb"),
"SITE_GRIP_DEVICE": str(grip.get("device_name") or "GripperDahuanModbus"),
"SITE_GRIP_FORCE_N": str(float(grip.get("force_n") or 50.0)),
"SITE_GRIP_VEL_MPS": str(float(grip.get("vel_mps") or 0.05)),
"SITE_GRIP_PORT": str(grip.get("port") or "/dev/ttyUSB0"),
"SITE_GRIP_BAUD": str(int(grip.get("baud") or 115200)),
"SITE_GRIP_SLAVE": str(int(grip.get("slave_id") or 1)),
"SITE_GRIP_STROKE_MM": str(float(grip.get("stroke_mm") or 20.0)),
"SITE_GRIP_FORCE": str(int(grip.get("force_pct") or 50)),
"SITE_GRIP_SPEED": str(int(grip.get("speed_pct") or 50)),
"SITE_GRIP_AUTO_INIT": "true" if grip.get("auto_init", True) else "false",
"SITE_GRIP_RS485_RTS": str(grip.get("rs485_rts") or ""),
"SITE_HOLE_BACKEND": str((data.get("hole_detect") or {}).get("backend") or "two_stage"),
"SITE_HOLE_WEIGHTS": str(
(data.get("hole_detect") or {}).get("weights")
or paths.get("hole_weights")
or str(_root() / "models" / "hole_insert" / "best.pt")
),
"SITE_HOLE_WORKPIECE_WEIGHTS": str(
(data.get("hole_detect") or {}).get("workpiece_weights")
or paths.get("hole_workpiece_weights")
or weights
),
"SITE_HOLE_CONFIG": str(
(data.get("hole_detect") or {}).get("config")
or (
str(_root() / "src" / "arm_application" / "config" / "hole_detect.yaml")
if str((data.get("hole_detect") or {}).get("backend") or "two_stage")
in ("two_stage", "hough_roi")
else str(_root() / "src" / "arm_application" / "config" / "hole_insert.yaml")
)
),
"SITE_TACTILE_BACKEND": str((data.get("tactile") or {}).get("backend") or "orisys"),
"SITE_TACTILE_CONFIG": str((data.get("tactile") or {}).get("config") or "dd03"),
"SITE_TACTILE_CONFIG_FILE": str(
(data.get("tactile") or {}).get("config_file")
or str(_root() / "src" / "arm_application" / "config" / "orisys" / "dd03.json")
),
"SITE_TACTILE_LEFT": str((data.get("tactile") or {}).get("left_video", 0)),
"SITE_TACTILE_RIGHT": str((data.get("tactile") or {}).get("right_video", 2)),
}
return out
def gripper_overrides(data: dict) -> dict:
"""ROS parameter dict for dh_pgea_node (DH Modbus, not Flexiv gripper)."""
data, _robot = apply_active_arm(data)
g = data.get("gripper") or {}
return {
"port": str(g.get("port") or "/dev/ttyUSB0"),
"baud": int(g.get("baud") or 115200),
"slave_id": int(g.get("slave_id") or 1),
"stroke_mm": float(g.get("stroke_mm") or 20.0),
"force_pct": int(g.get("force_pct") or 50),
"speed_pct": int(g.get("speed_pct") or 50),
"auto_init": bool(g.get("auto_init", True)),
"state_hz": float(g.get("state_hz") or 20.0),
"model": str(g.get("model") or "PGEA-100-20-W"),
"serial_number": str(g.get("sn") or ""),
"open_position": int(g.get("open_position") if g.get("open_position") is not None else 1000),
"close_position": int(g.get("close_position") if g.get("close_position") is not None else 0),
"rs485_rts": str(g.get("rs485_rts") or ""),
}
def perception_overrides(data: dict) -> dict:
"""ROS parameter dict for perception_node."""
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)
obj = str(yolo.get("object_frame") or "").strip()
if not obj or obj == "tracked_object":
arm = str((_robot or {}).get("arm") or "")
sn = str((_robot or {}).get("sn") or "")
if "063991" in sn or arm == "right":
obj = "tracked_object_91"
elif "063989" in sn or arm == "left":
obj = "tracked_object_89"
else:
obj = "tracked_object"
return {
"weights": str(paths.get("yolo_weights") or ""),
"conf": float(yolo.get("conf") or 0.10),
"iou": float(yolo.get("iou") or 0.45),
"imgsz": int(yolo.get("imgsz") or 416),
"device": str(yolo.get("device") or "0"),
"half_precision": bool(yolo.get("half_precision", True)),
"backend": str(yolo.get("backend") or "auto"),
"mp_infer": bool(yolo.get("mp_infer", True)),
"min_conf": float(yolo.get("min_conf") or 0.12),
"vision_hz": float(yolo.get("vision_hz") or 30.0),
"infer_max_width": int(yolo.get("infer_max_width") if yolo.get("infer_max_width") is not None else 0),
"box_ema_alpha": float(yolo.get("box_ema_alpha") or 0.85),
"miss_clear_frames": int(yolo.get("miss_clear_frames") or 12),
"overlay_hold_on_miss": bool(yolo.get("overlay_hold_on_miss", True)),
"jump_px": float(yolo.get("jump_px") or 280.0),
"jump_z_m": float(yolo.get("jump_z_m") or 0.20),
"z_ref_m": float(yolo.get("z_ref_m") or 0.22),
"min_depth_m": float(yolo.get("min_depth_m") or 0.04),
"max_depth_m": float(yolo.get("max_depth_m") or 0.55),
"work_z_max_m": float(yolo.get("work_z_max_m") or 0.50),
"hold_point_on_miss": bool(yolo.get("hold_point_on_miss", True)),
"miss_hold_point_frames": int(yolo.get("miss_hold_point_frames") or 2),
"meas_blend": float(yolo.get("meas_blend") or 0.65),
"z_alpha_min": float(yolo.get("z_alpha_min") or 0.10),
"z_alpha_max": float(yolo.get("z_alpha_max") or 0.60),
"z_alpha_gain": float(yolo.get("z_alpha_gain") or 2.0),
"camera_frame": cam_frame,
"object_frame": obj,
"depth_topic": depth_topic,
"camera_source": cam_ns,
"feature_mode": str(yolo.get("feature_mode") or "plane"),
"plane_inlier_m": float(yolo.get("plane_inlier_m") or 0.008),
"plane_band_m": float(yolo.get("plane_band_m") or 0.06),
}
def servo_overrides(data: dict) -> dict:
"""ROS parameter dict for servo_node."""
data, robot = apply_active_arm(data)
paths = data.get("paths") or {}
yolo = data.get("yolo") or {}
servo = data.get("servo") or {}
_ns, cam_frame, _depth = camera_frames(data)
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 ""),
"camera_frame": cam_frame,
"handeye_child_frame": cam_frame,
"arm_label": str(robot.get("label") or robot.get("arm") or ""),
"z_ref_m": float(servo.get("z_ref_m") or yolo.get("z_ref_m") or 0.40),
}
# Pass through remaining servo keys that match node params
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",
"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",
"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",
"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",
"use_static_handeye_tf",
"workspace_xyz_min", "workspace_xyz_max",
]
for k in keys:
if k in servo:
out[k] = servo[k]
return out
def main() -> int:
ap = argparse.ArgumentParser()
ap.add_argument("--json", action="store_true")
ap.add_argument("--print", action="store_true", dest="do_print")
ap.add_argument("--config", default="")
ap.add_argument("--sn", "--arm", dest="sn", default="", help="89|91|left|right|full SN")
args = ap.parse_args()
if args.sn:
os.environ["ROBOT_SN"] = str(args.sn)
path = Path(args.config).expanduser() if args.config else site_path()
data = load_site(path)
flat = flatten(data)
if args.json:
print(json.dumps({"site": data, "env": flat}, ensure_ascii=False, indent=2))
return 0
if args.do_print:
print(f"site config: {path}")
print(
f" robot SN : {flat['SITE_ROBOT_SN']}"
f"{(' (' + flat['SITE_ROBOT_LABEL'] + ')') if flat.get('SITE_ROBOT_LABEL') else ''}"
f" arm={flat.get('SITE_ROBOT_ARM') or '?'}"
)
print(f" catalog : {flat.get('SITE_ROBOT_CATALOG') or '(none)'}")
print(f" robot IP : {flat['SITE_ROBOT_IP']} pc={flat['SITE_PC_IP']} iface={flat.get('SITE_NET_IFACE') or '?'}")
print(f" frames : prefix={flat['SITE_JOINT_PREFIX']} ee={flat['SITE_EE_FRAME']}")
print(f" tool name : {flat.get('SITE_ROBOT_TOOL_NAME') or '(keep current)'}")
catalog_sns = [str(r.get("sn") or "") for r in list_robots(data)]
if catalog_sns and flat["SITE_ROBOT_SN"] not in catalog_sns:
print(
f" WARNING: robot.sn {flat['SITE_ROBOT_SN']!r} is not in robots[]",
file=sys.stderr,
)
print(f" yolo weights : {flat['SITE_YOLO_WEIGHTS']}")
print(f" resolved : {flat['SITE_YOLO_WEIGHTS_RESOLVED']}")
print(f" handeye : {flat['SITE_HANDEYE_FILE']}")
print(f" handeye wp : {flat['SITE_HANDEYE_WAYPOINTS']}")
print(f" handeye dir : {flat['SITE_HANDEYE_WORKDIR']}")
print(f" named poses : {flat.get('SITE_NAMED_POSES_FILE')}")
print(
" board : "
f"{flat['SITE_HANDEYE_BOARD_COLS']}x{flat['SITE_HANDEYE_BOARD_ROWS']} "
f"inner, square={flat['SITE_HANDEYE_SQUARE_M']} m"
)
print(
f" camera : serial={flat['SITE_CAM_SERIAL'] or '(empty)'} "
f"{flat['SITE_CAM_WIDTH']}x{flat['SITE_CAM_HEIGHT']}@{flat['SITE_CAM_FPS']} "
f"ae={flat.get('SITE_CAM_AUTO_EXPOSURE')} "
f"exp={flat.get('SITE_CAM_EXPOSURE_US')}us "
f"gain={flat.get('SITE_CAM_GAIN')} "
f"ae_limit={flat.get('SITE_CAM_AE_LIMIT_US')}us emitter={flat.get('SITE_CAM_EMITTER')}"
)
print(
f" catalog cam : leftD405={flat.get('SITE_D405_LEFT_SERIAL')} "
f"rightD405={flat.get('SITE_D405_RIGHT_SERIAL')} "
f"D435={flat.get('SITE_D435_SERIAL')}"
)
print(
f" gripper : {flat.get('SITE_GRIP_MODEL')} port={flat.get('SITE_GRIP_PORT')}"
)
print(
f" hole_detect : backend={flat.get('SITE_HOLE_BACKEND')} "
f"weights={flat.get('SITE_HOLE_WEIGHTS')}"
)
print(
f" tactile : backend={flat.get('SITE_TACTILE_BACKEND')} "
f"L={flat.get('SITE_TACTILE_LEFT')} R={flat.get('SITE_TACTILE_RIGHT')} "
f"cfg={flat.get('SITE_TACTILE_CONFIG')}"
)
print(f" go_home : pose={flat['SITE_GO_HOME_POSE']} speed={flat['SITE_GO_HOME_SPEED']}")
he_path = Path(flat["SITE_HANDEYE_FILE"]).expanduser()
if not he_path.is_file():
print(f" WARNING: hand-eye file missing: {he_path}", file=sys.stderr)
else:
try:
he_data = yaml.safe_load(he_path.read_text(encoding="utf-8")) or {}
except Exception:
he_data = {}
if isinstance(he_data, dict) and (
he_data.get("uncalibrated") or "T_4x4" not in he_data
):
print(
f" WARNING: {he_path.name} is not calibrated for this arm "
f"(run_handeye.sh teach/calibrate --arm {flat.get('SITE_ROBOT_ARM') or '91'})",
file=sys.stderr,
)
if "063991" in flat["SITE_ROBOT_SN"] and flat.get("SITE_CAM_SERIAL") == flat.get("SITE_D405_LEFT_SERIAL"):
print(
" WARNING: 91 is using the LEFT wrist D405 serial — check robots[].camera.serial",
file=sys.stderr,
)
if "RGI" in str(flat.get("SITE_GRIP_MODEL") or ""):
print(
" NOTE: 本臂是 RGI。CLI: bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open|rotate",
file=sys.stderr,
)
w = Path(flat["SITE_YOLO_WEIGHTS"]).expanduser()
if not w.is_file():
print(f" WARNING: weights file missing: {w}", file=sys.stderr)
return 2
return 0
# bash exports
for k, v in flat.items():
# escape for eval
vv = str(v).replace("'", "'\"'\"'")
print(f"export {k}='{vv}'")
return 0
if __name__ == "__main__":
raise SystemExit(main())
Reference in New Issue View Git Blame Copy Permalink