orisys/orisys_arm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial import of ORISYS arm workstation

Browse Source
This commit is contained in:
cheng
2026-10-16 17:27:30 +08:00
commit 2b74c50ba8
745 changed files with 1199101 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
handeye/src/handeye_calibrate.py
+771
View File
@@ -0,0 +1,771 @@
#!/usr/bin/env python3
"""完整手眼标定流水线：移动 → 棋盘稳定判定 → 采集 → 求解 → 清理。
支持 eye_in_hand / eye_to_hand。
相机内参优先级：
1) ROS 的 CameraInfo 话题（若正在发布）
2) 通过 pyrealsense2 读取 RealSense D405/D435 的 EEPROM（camera_source）
3) YAML 中手写的 camera_fx/fy/cx/cy
棋盘尺寸（board_cols / board_rows / square_size_m）可由用户配置。
"""
from __future__ import annotations
import json
import shutil
import time
from datetime import datetime, timezone
from pathlib import Path
from typing import List, Optional, Tuple
import numpy as np
import rclpy
from geometry_msgs.msg import PoseStamped
from rclpy.action import ActionClient
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import CameraInfo, Image, JointState
from std_msgs.msg import String
from arm_application.handeye_lib import (
BoardConfig,
CameraIntrinsics,
ChessboardStabilizer,
T_from_flexiv_tcp,
calibrate_hand_eye,
cam_to_flange_via_tool,
detect_chessboard,
format_matrix_yaml,
format_transform_yaml,
is_pose_filled,
pose_to_Rt,
solve_board_pose,
)
from arm_application.handeye_realsense import (
is_realsense_source,
query_realsense_color_intrinsics,
resolve_realsense_topics,
)
from arm_application.site_io import joint_prefix_default
from arm_msgs.action import FollowJointTrajectory
from arm_msgs.srv import GetArmState
from arm_application.ros_node import TolerantNode
DEFAULT_JOINTS = [
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
]
def _image_to_bgr(msg: Image) -> np.ndarray:
h, w = int(msg.height), int(msg.width)
enc = (msg.encoding or '').lower()
data = np.frombuffer(msg.data, dtype=np.uint8)
if enc in ('bgr8', 'rgb8'):
img = data.reshape(h, w, 3)
return img[:, :, ::-1].copy() if enc == 'rgb8' else img
if enc in ('mono8', '8uc1'):
gray = data.reshape(h, w)
return np.stack([gray, gray, gray], axis=-1)
if enc in ('bgra8', 'rgba8'):
img = data.reshape(h, w, 4)
bgr = img[:, :, :3]
return bgr[:, :, ::-1].copy() if enc == 'rgba8' else bgr
raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}')
def _camera_info_to_intrinsics(msg: CameraInfo) -> CameraIntrinsics:
K = np.array(msg.k, dtype=float).reshape(3, 3)
dist = np.array(msg.d, dtype=float).reshape(-1, 1) if msg.d else np.zeros((5, 1))
return CameraIntrinsics(K=K, dist=dist)
class HandeyeCalibrate(TolerantNode):
def __init__(self) -> None:
super().__init__('handeye_calibrate')
self.declare_parameter('mode', 'eye_in_hand') # eye_in_hand | eye_to_hand
self.declare_parameter('joint_prefix', joint_prefix_default())
self.declare_parameter('joint_names', DEFAULT_JOINTS)
self.declare_parameter('velocity_scale', 0.10)
self.declare_parameter('action_name', 'follow_joint_trajectory')
self.declare_parameter('move_time_sec', 4.0)
self.declare_parameter('start_index', 0)
self.declare_parameter('num_poses', 12)
self.declare_parameter('waypoint_positions', [0.0] * (12 * 7))
self.declare_parameter('min_poses', 5)
# 相机来源：manual | d405 | d435
self.declare_parameter('camera_source', 'manual')
self.declare_parameter('camera_serial', '')
self.declare_parameter('camera_namespace', '')
self.declare_parameter('realsense_width', 1280)
self.declare_parameter('realsense_height', 720)
self.declare_parameter('image_topic', '')
self.declare_parameter('camera_info_topic', '')
self.declare_parameter('camera_fx', 0.0)
self.declare_parameter('camera_fy', 0.0)
self.declare_parameter('camera_cx', 0.0)
self.declare_parameter('camera_cy', 0.0)
self.declare_parameter('camera_dist', [0.0, 0.0, 0.0, 0.0, 0.0])
# 棋盘（可自定义）
# 默认值必须与 site.yaml 的 handeye.board_* 及结果文件头里的实际板子一致。
# 真实使用的板：A5 内角点 11x8，方格 15mm（site.yaml handeye.boards.eye_in_hand）。
# 这里原先写的是 9x6 / 0.025，后来是 8x11；漏配就会静默用错尺寸。
self.declare_parameter('board_cols', 11) # 内角点
self.declare_parameter('board_rows', 8)
self.declare_parameter('square_size_m', 0.015)
self.declare_parameter('stable_window', 8)
self.declare_parameter('stable_pixel_std', 0.45)
self.declare_parameter('stable_timeout_sec', 25.0)
self.declare_parameter('settle_sec', 0.4)
# 到位后的"真的停住了"判据 —— 直接看关节速度，比固定 sleep 可靠。
# 这几个参数原先 site.yaml 和 run_handeye.sh 一直在传，但本节点从未声明，
# ROS 2 会静默忽略未声明的覆盖参数，于是"等静止再拍照"这件事其实没实现，
# 实际只靠一次固定 settle_sec。现在补上。
self.declare_parameter('settle_qd_max', 0.015) # |qd|_max [rad/s] 低于此值算静
self.declare_parameter('settle_stable_sec', 0.6) # 需要连续静多久
self.declare_parameter('settle_timeout_sec', 8.0) # 等静止的总超时
self.declare_parameter('pin_holds', 2) # 静止后再保持几拍
self.declare_parameter('pin_hold_sec', 0.45) # 每拍多长
self.declare_parameter('skip_unstable_poses', True)
from arm_application.site_io import ros2_root
self.declare_parameter('output_dir', str(ros2_root() / 'handeye' / 'tmp'))
self.declare_parameter('result_file', '')
self.declare_parameter('cleanup_images', True)
# 眼在手上：AX=XB 的 gripper 用 Studio 工具 TCP，再折成 cam→法兰写 T_4x4。
# 空 = 旧口径（ee_pose 当 gripper，T_4x4 原样写出）。配了就必须带 TCP。
self.declare_parameter('gripper_tool', '')
self.declare_parameter('gripper_tcp_xyz', [0.0, 0.0, 0.0])
self.declare_parameter('gripper_tcp_qwxyz', [1.0, 0.0, 0.0, 0.0])
prefix = str(self.get_parameter('joint_prefix').value or '')
base = list(
self.get_parameter('joint_names').get_parameter_value().string_array_value
or DEFAULT_JOINTS
)
self.joint_names = [f'{prefix}{n}' for n in base]
self.mode = str(self.get_parameter('mode').value).strip().lower()
self.velocity_scale = float(self.get_parameter('velocity_scale').value)
self.action_name = str(self.get_parameter('action_name').value)
self.move_time_sec = float(self.get_parameter('move_time_sec').value)
self.start_index = max(0, int(self.get_parameter('start_index').value))
self.min_poses = int(self.get_parameter('min_poses').value)
self.cleanup_images = bool(self.get_parameter('cleanup_images').value)
self.gripper_tool = str(self.get_parameter('gripper_tool').value or '').strip()
tcp_xyz = list(
self.get_parameter('gripper_tcp_xyz').get_parameter_value().double_array_value
or [0.0, 0.0, 0.0]
)
tcp_q = list(
self.get_parameter('gripper_tcp_qwxyz').get_parameter_value().double_array_value
or [1.0, 0.0, 0.0, 0.0]
)
self.T_flange_to_tool = T_from_flexiv_tcp(tcp_xyz[:3], tcp_q[:4])
if self.gripper_tool and float(np.linalg.norm(self.T_flange_to_tool[:3, 3])) < 1e-4:
raise RuntimeError(
f'gripper_tool={self.gripper_tool!r} 但 gripper_tcp_xyz 几乎是零：'
'等于没把 Studio TCP 传进来。不许按法兰口径假装在用工具系。'
)
self.camera_source = str(self.get_parameter('camera_source').value).strip().lower()
self.camera_serial = str(self.get_parameter('camera_serial').value or '').strip()
self.camera_namespace = str(
self.get_parameter('camera_namespace').value or ''
).strip()
self.rs_width = int(self.get_parameter('realsense_width').value)
self.rs_height = int(self.get_parameter('realsense_height').value)
self.board = BoardConfig(
cols=int(self.get_parameter('board_cols').value),
rows=int(self.get_parameter('board_rows').value),
square_size=float(self.get_parameter('square_size_m').value),
)
self.stabilizer = ChessboardStabilizer(
window=int(self.get_parameter('stable_window').value),
max_pixel_std=float(self.get_parameter('stable_pixel_std').value),
)
self.stable_timeout = float(self.get_parameter('stable_timeout_sec').value)
self.settle_sec = float(self.get_parameter('settle_sec').value)
self.settle_qd_max = float(self.get_parameter('settle_qd_max').value)
self.settle_stable_sec = float(self.get_parameter('settle_stable_sec').value)
self.settle_timeout = float(self.get_parameter('settle_timeout_sec').value)
self.pin_holds = max(0, int(self.get_parameter('pin_holds').value))
self.pin_hold_sec = max(0.0, float(self.get_parameter('pin_hold_sec').value))
self.skip_unstable = bool(self.get_parameter('skip_unstable_poses').value)
out = Path(str(self.get_parameter('output_dir').value)).expanduser()
self.output_dir = out
self.images_dir = out / 'images'
rf = str(self.get_parameter('result_file').value or '').strip()
self.result_file = Path(rf).expanduser() if rf else (out / 'handeye_result.yaml')
self._client = ActionClient(self, FollowJointTrajectory, self.action_name)
self._get_state = self.create_client(GetArmState, 'get_arm_state')
self._capture_pub = self.create_publisher(String, 'handeye/capture', 10)
self._latest_image: Optional[Image] = None
self._latest_info: Optional[CameraInfo] = None
self._latest_js: Optional[JointState] = None
self._intrinsics: Optional[CameraIntrinsics] = None
img_topic = str(self.get_parameter('image_topic').value or '').strip()
info_topic = str(self.get_parameter('camera_info_topic').value or '').strip()
if is_realsense_source(self.camera_source):
img_topic, info_topic = resolve_realsense_topics(
self.camera_source,
camera_namespace=self.camera_namespace,
image_topic=img_topic,
camera_info_topic=info_topic,
)
self._info_topic = info_topic
self.create_subscription(JointState, '/joint_states', self._on_js, 10)
if not img_topic:
self.get_logger().error(
'image_topic 为空。请设置 camera_source:=d405|d435，或填写 image_topic。'
)
else:
self.create_subscription(
Image, img_topic, self._on_image, qos_profile_sensor_data
)
self.get_logger().info(f'已订阅图像：{img_topic}（BEST_EFFORT）')
if info_topic:
self.create_subscription(
CameraInfo, info_topic, self._on_info, qos_profile_sensor_data
)
self.get_logger().info(f'已订阅 camera_info：{info_topic}（BEST_EFFORT）')
def _on_js(self, msg: JointState) -> None:
self._latest_js = msg
def _on_image(self, msg: Image) -> None:
self._latest_image = msg
def _on_info(self, msg: CameraInfo) -> None:
self._latest_info = msg
def run(self) -> int:
if self.mode not in ('eye_in_hand', 'eye_to_hand'):
self.get_logger().error(
f'未知的 mode={self.mode!r}；请使用 eye_in_hand 或 eye_to_hand'
)
return 1
poses = self._load_poses()
if len(poses) < self.min_poses:
self.get_logger().error(
f'只有 {len(poses)} 个已填写位姿（需要 >= {self.min_poses}）。'
'示教命令：bash ~/ros2/scripts/run_handeye.sh teach && merge。'
)
return 1
if self.start_index >= len(poses):
self.get_logger().error(
f'start_index {self.start_index} >= 已填写位姿数 {len(poses)}'
)
return 1
if not self._resolve_intrinsics():
return 1
if not self._client.wait_for_server(timeout_sec=15.0):
self.get_logger().error(
f'Action [{self.action_name}] 不可用。'
'请启动：bash ~/ros2/scripts/run_real.sh'
)
return 2
self.output_dir.mkdir(parents=True, exist_ok=True)
self.images_dir.mkdir(parents=True, exist_ok=True)
eye_to_hand = self.mode == 'eye_to_hand'
grip = self.gripper_tool or '（ee_pose 原样，无工具折算）'
self.get_logger().info(
f'手眼标定 [{self.mode}] camera_source={self.camera_source}：'
f'{len(poses)} 个位姿，棋盘={self.board.cols}x{self.board.rows} '
f'方格={self.board.square_size}m，v={self.velocity_scale} '
f'gripper_tool={grip}'
)
self.get_logger().warn('清理工作空间；确认急停就绪。Ctrl+C 可中止。')
Rg: List[np.ndarray] = []
tg: List[np.ndarray] = []
Rc: List[np.ndarray] = []
tc: List[np.ndarray] = []
meta = []
try:
for i in range(self.start_index, len(poses)):
q = poses[i]
self._log_progress(i, len(poses), '移动中...')
code = self._move_one(q, self.move_time_sec)
if code != 0:
return code
if not self._verify_joints(q, tol=0.08):
self.get_logger().error(
f'[{i + 1}] 机械臂未到达目标关节位置 '
'（控制器报告成功，但关节没有变化）。'
'请重启 run_real.sh；示教器切 AUTO Remote；清除故障。'
'不要相信这次标定结果。'
)
return 10
end = time.monotonic() + max(0.0, self.settle_sec)
while rclpy.ok() and time.monotonic() < end:
rclpy.spin_once(self, timeout_sec=0.05)
# 固定 settle_sec 之后再看关节速度：真停了才算数，然后 pin 一拍。
self._wait_settled()
self._pin_hold()
ok, std, corners, pattern, bgr = self._wait_stable()
if not ok or corners is None or bgr is None:
msg = f'[{i + 1}] 棋盘不稳定（std={std:.3f}）'
if self.skip_unstable:
self.get_logger().warn(f'{msg} —— 跳过')
continue
self.get_logger().error(f'{msg}。中止。')
return 7
ee = self._read_ee_pose()
if ee is None:
self.get_logger().error('读取 ee_pose 失败')
return 8
assert self._intrinsics is not None
R_t2c, t_t2c = solve_board_pose(
corners, self.board, self._intrinsics, pattern=pattern
)
xyz = [ee.pose.position.x, ee.pose.position.y, ee.pose.position.z]
quat = [
ee.pose.orientation.x,
ee.pose.orientation.y,
ee.pose.orientation.z,
ee.pose.orientation.w,
]
R_g2b, t_g2b = pose_to_Rt(xyz, quat)
stamp = datetime.now(timezone.utc).strftime('%Y%m%d_%H%M%S')
img_path = self.images_dir / f'pose_{i:02d}_{stamp}.png'
self._save_png(bgr, img_path)
msg = String()
msg.data = f'capture:{i}:{len(poses)}'
self._capture_pub.publish(msg)
Rg.append(R_g2b)
tg.append(t_g2b)
Rc.append(R_t2c)
tc.append(t_t2c)
meta.append(
{
'index': i,
'joints': list(q),
'ee_xyz': xyz,
'ee_quat_xyzw': quat,
'image': str(img_path),
'corner_std_px': std,
}
)
self.get_logger().info(
f'[{i + 1}/{len(poses)}] 已采集 '
f'(corner_std={std:.3f}px) -> {img_path.name}'
)
except KeyboardInterrupt:
self.get_logger().info('已中断')
return 0
if len(Rg) < self.min_poses:
self.get_logger().error(
f'只采集到 {len(Rg)} 次；求解至少需要 {self.min_poses} 次'
)
return 9
self.get_logger().info(f'用 {len(Rg)} 个视角求解手眼标定...')
R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=eye_to_hand)
extra = ''
if eye_to_hand:
name = 'T_cam_to_base (eye_to_hand)'
yaml_txt = format_transform_yaml(R, t, name)
if self.gripper_tool:
extra = (
f'gripper_tool: {self.gripper_tool}\n'
+ format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
)
self.get_logger().info(
f'gripper={self.gripper_tool}：AX=XB 的手是 TCP；'
f'T_4x4 是 cam→基座，不折 t={t.reshape(3)*1000} mm'
)
elif self.gripper_tool:
T_cam_tool = np.eye(4)
T_cam_tool[:3, :3] = R
T_cam_tool[:3, 3] = t.reshape(3)
T_cam_flange = cam_to_flange_via_tool(T_cam_tool, self.T_flange_to_tool)
delta = float(np.linalg.norm(T_cam_flange[:3, 3] - T_cam_tool[:3, 3]))
tool_len = float(np.linalg.norm(self.T_flange_to_tool[:3, 3]))
if tool_len > 0.05 and delta < 1e-4:
self.get_logger().error(
'T_cam_to_flange 与 T_cam_to_tool 平移几乎相同，'
'折算没生效。结果作废。'
)
return 11
# 腕部相机在法兰前方（法兰 +Z）。乘反了 Z 会落到 −200 mm 一带。
if (
self.T_flange_to_tool[2, 3] > 0.05
and float(T_cam_flange[2, 3]) < 0.0
):
self.get_logger().error(
f'T_cam_to_flange tz={float(T_cam_flange[2, 3])*1000:.1f} mm '
'在法兰后方，乘序反了。结果作废。'
)
return 12
R, t = T_cam_flange[:3, :3], T_cam_flange[:3, 3].reshape(3, 1)
name = 'T_cam_to_flange (eye_in_hand，由工具 TCP 折算)'
yaml_txt = format_transform_yaml(R, t, name)
extra = (
f'gripper_tool: {self.gripper_tool}\n'
+ format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
+ format_matrix_yaml(T_cam_tool, 'T_cam_to_tool')
)
self.get_logger().info(
f'gripper={self.gripper_tool}：'
f'T_cam_to_tool t={T_cam_tool[:3, 3]*1000} mm → '
f'T_cam_to_flange t={T_cam_flange[:3, 3]*1000} mm'
)
else:
name = 'T_cam_to_gripper (eye_in_hand)'
yaml_txt = format_transform_yaml(R, t, name)
header = (
f'# 手眼标定结果\n'
f'# mode: {self.mode}\n'
f'# camera_source: {self.camera_source}\n'
f'# views: {len(Rg)}\n'
f'# utc: {datetime.now(timezone.utc).isoformat()}\n'
f'# board: {self.board.cols}x{self.board.rows} '
f'square_m={self.board.square_size}\n'
)
if self.gripper_tool and eye_to_hand:
header += (
f'# gripper_tool: {self.gripper_tool}\n'
f'# T_4x4 = cam→基座（AX=XB 的手是 TCP，不折到法兰）\n'
f'# 旧结果备份在 results/backup/\n'
)
elif self.gripper_tool:
header += (
f'# gripper_tool: {self.gripper_tool}\n'
f'# T_4x4 = cam→法兰（AX=XB 在 TCP 上解，再左乘 T_flange_to_tool）\n'
f'# T_cam_to_tool 是原始求解；旧的法兰/tool1 备份在 results/backup/\n'
)
self.result_file.parent.mkdir(parents=True, exist_ok=True)
self.result_file.write_text(header + extra + yaml_txt, encoding='utf-8')
(self.output_dir / 'captures.json').write_text(
json.dumps(meta, indent=2), encoding='utf-8'
)
self.get_logger().info('======== 手眼标定结果 ========')
for line in (header + yaml_txt).strip().splitlines():
self.get_logger().info(line)
self.get_logger().info(f'已保存 -> {self.result_file}')
if self.cleanup_images:
if self.images_dir.exists():
shutil.rmtree(self.images_dir, ignore_errors=True)
self.get_logger().info('标定图像已删除')
else:
self.get_logger().info(f'图像保留在 {self.images_dir}')
return 0
def _load_poses(self) -> List[List[float]]:
flat = list(self.get_parameter('waypoint_positions').value)
n_j = len(self.joint_names)
n_cfg = int(self.get_parameter('num_poses').value)
if n_cfg < 1:
return []
need = n_cfg * n_j
if len(flat) < need:
self.get_logger().warn(
f'waypoint_positions 长度 {len(flat)} < num_poses*{n_j}={need}；'
'用零补齐'
)
flat = flat + [0.0] * (need - len(flat))
poses: List[List[float]] = []
for i in range(n_cfg):
q = [float(v) for v in flat[i * n_j : (i + 1) * n_j]]
if is_pose_filled(q):
poses.append(q)
else:
self.get_logger().warn(f'位姿槽 {i} 为空（全零）—— 已跳过')
return poses
def _resolve_intrinsics(self) -> bool:
# 1) CameraInfo
deadline = time.monotonic() + 3.0
while rclpy.ok() and self._latest_info is None and time.monotonic() < deadline:
rclpy.spin_once(self, timeout_sec=0.1)
if self._latest_info is not None:
self._intrinsics = _camera_info_to_intrinsics(self._latest_info)
K = self._intrinsics.K
self.get_logger().info(
f'内参来自 CameraInfo（{self._info_topic}）：'
f'fx={K[0, 0]:.2f} fy={K[1, 1]:.2f} cx={K[0, 2]:.2f} cy={K[1, 2]:.2f}'
)
return True
# 2) RealSense EEPROM
if is_realsense_source(self.camera_source):
try:
intr = query_realsense_color_intrinsics(
self.camera_source,
serial=self.camera_serial,
width=self.rs_width,
height=self.rs_height,
)
except Exception as exc: # noqa: BLE001
self.get_logger().error(f'RealSense 内参查询失败：{exc}')
else:
self._intrinsics = CameraIntrinsics(K=intr.as_K(), dist=intr.as_dist())
self.get_logger().info(
f'内参来自 RealSense {intr.model.upper()} '
f'serial={intr.serial} {intr.width}x{intr.height}：'
f'fx={intr.fx:.2f} fy={intr.fy:.2f} '
f'cx={intr.cx:.2f} cy={intr.cy:.2f}'
)
if (intr.width, intr.height) != (self.rs_width, self.rs_height):
self.get_logger().warn(
f'请求 {self.rs_width}x{self.rs_height}，'
f'实际得到 {intr.width}x{intr.height}（最接近的 profile）'
)
return True
# 3) Manual YAML
fx = float(self.get_parameter('camera_fx').value)
fy = float(self.get_parameter('camera_fy').value)
cx = float(self.get_parameter('camera_cx').value)
cy = float(self.get_parameter('camera_cy').value)
if fx <= 0 or fy <= 0:
self.get_logger().error(
'没有内参。请设置 camera_source:=d405|d435，'
'或提供 camera_info_topic，或填写 camera_fx/fy/cx/cy。'
)
return False
dist = np.array(
list(self.get_parameter('camera_dist').value), dtype=float
).reshape(-1, 1)
K = np.array([[fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]], dtype=float)
self._intrinsics = CameraIntrinsics(K=K, dist=dist)
self.get_logger().info(
f'内参来自 YAML：fx={fx:.2f} fy={fy:.2f} cx={cx:.2f} cy={cy:.2f}'
)
return True
def _log_progress(self, i: int, total: int, suffix: str = '') -> None:
"""带进度条的日志。
自动标定是无人值守的长流程（十几个点位 × 移动+稳定+拍照），
单看 `[3/12]` 很难有"还要多久"的直觉，加一条可视化进度条。
"""
frac = (i + 1) / max(1, total)
width = 24
filled = int(round(frac * width))
bar = '█' * filled + '░' * (width - filled)
self.get_logger().info(
f'[{i + 1}/{total}] [{bar}] {frac * 100:5.1f}% {suffix}'
)
def _wait_settled(self) -> bool:
"""等机械臂真的停下来，再进入拍照。
判据是关节速度 |qd|_max 连续 ``settle_stable_sec`` 低于 ``settle_qd_max``。
为什么不用固定 sleep：`settle_sec` 是拍脑袋的时长，臂负载/姿态不同，
刹车距离差别很大 —— 等太短会在臂还在动时拍照（棋盘虚 → 角点抖动），
等太长则整轮标定被无谓拉长。直接看速度更准。
超时不算致命：记一条警告后继续，让后面的棋盘稳定性检查去把关。
"""
if self.settle_qd_max <= 0.0:
return True
deadline = time.monotonic() + self.settle_timeout
stable_since: Optional[float] = None
while rclpy.ok() and time.monotonic() < deadline:
rclpy.spin_once(self, timeout_sec=0.05)
js = self._latest_js
if js is None or not list(js.velocity):
continue
try:
vmax = float(np.max(np.abs(np.asarray(js.velocity, dtype=float))))
except Exception: # noqa: BLE001
continue
now = time.monotonic()
if vmax <= self.settle_qd_max:
if stable_since is None:
stable_since = now
elif now - stable_since >= self.settle_stable_sec:
return True
else:
stable_since = None # 又动了，重新计时
self.get_logger().warn(
f'wait_settled 超时 ({self.settle_timeout:.1f}s)，臂可能仍在微动；'
'交给棋盘稳定性检查兜底'
)
return False
def _pin_hold(self) -> None:
"""静止后再保持几拍，抑制到位后的微小回弹/漂移。
语义取自 site.yaml 的注释"零速 pin 后再拍照"：不是重新规划轨迹，
而是纯粹等一段时间让机械臂自己稳下来（控制器在到位后仍会做少量补偿）。
"""
total = self.pin_holds * self.pin_hold_sec
end = time.monotonic() + total
while rclpy.ok() and time.monotonic() < end:
rclpy.spin_once(self, timeout_sec=0.05)
def _wait_stable(
self,
) -> Tuple[bool, float, Optional[np.ndarray], Optional[np.ndarray]]:
self.stabilizer.reset()
deadline = time.monotonic() + self.stable_timeout
last_std = float('inf')
last_corners = None
last_pattern = None
last_bgr = None
last_log = 0.0
while rclpy.ok() and time.monotonic() < deadline:
rclpy.spin_once(self, timeout_sec=0.05)
if self._latest_image is None:
continue
try:
bgr = _image_to_bgr(self._latest_image)
except Exception as exc: # noqa: BLE001
self.get_logger().error(f'图像转换失败：{exc}')
return False, last_std, None, None, None
detected = detect_chessboard(bgr, self.board)
corners = detected[0] if detected is not None else None
pattern = detected[1] if detected is not None else None
ok, std = self.stabilizer.push(corners)
last_std = std
if corners is not None:
last_corners = corners
last_pattern = pattern
last_bgr = bgr
now = time.monotonic()
if now - last_log > 1.0:
seen = '有' if corners is not None else '无'
self.get_logger().info(
f' 棋盘={seen} 角点标准差='
f'{std if std < 1e8 else float("nan"):.3f}px'
)
last_log = now
if ok:
return True, std, last_corners, last_pattern, last_bgr
return False, last_std, last_corners, last_pattern, last_bgr
def _verify_joints(self, target: List[float], tol: float = 0.08) -> bool:
"""确认 ``joint_states`` 真的走到了目标位置。
用来识破"假的成功"：有些执行接口会立刻返回成功，但关节其实没动
（或只动了一半）。这里在 2 秒内轮询关节角，逐轴比对容差 ``tol``（弧度），
只有全部落在容差内才算真到位。
"""
deadline = time.monotonic() + 2.0
while rclpy.ok() and time.monotonic() < deadline:
rclpy.spin_once(self, timeout_sec=0.05)
if self._latest_js is None:
continue
name_to_q = dict(zip(list(self._latest_js.name), list(self._latest_js.position)))
actual = []
for n in self.joint_names:
if n not in name_to_q:
break
actual.append(float(name_to_q[n]))
if len(actual) != len(target):
continue
err = float(np.sqrt(np.mean([(a - b) ** 2 for a, b in zip(actual, target)])))
self.get_logger().info(
f' 关节相对目标的 RMSE：{err:.4f} rad（tol={tol}）'
)
return err <= tol
self.get_logger().error('校验运动时没有 joint_states')
return False
def _move_one(self, positions: List[float], duration: float) -> int:
goal = FollowJointTrajectory.Goal()
goal.joint_names = self.joint_names
goal.num_points = 1
goal.velocity_scale = self.velocity_scale
goal.positions = list(positions)
goal.time_from_start = [float(duration)]
send = self._client.send_goal_async(goal)
if not self._wait_future(send, 10.0, '发送目标'):
return 3
handle = send.result()
if handle is None or not handle.accepted:
self.get_logger().error('目标被拒绝')
return 4
res_fut = handle.get_result_async()
if not self._wait_future(res_fut, max(60.0, duration * 3.0), '结果'):
return 5
result = res_fut.result().result
if not result.success:
self.get_logger().error(f'移动失败：{result.message}')
return 6
return 0
def _wait_future(self, fut, timeout: float, label: str) -> bool:
deadline = time.monotonic() + timeout
while rclpy.ok() and not fut.done():
rclpy.spin_once(self, timeout_sec=0.05)
if time.monotonic() > deadline:
self.get_logger().error(f'等待 {label} 超时')
return False
return fut.done()
def _read_ee_pose(self) -> Optional[PoseStamped]:
if not self._get_state.wait_for_service(timeout_sec=2.0):
return None
fut = self._get_state.call_async(GetArmState.Request())
if not self._wait_future(fut, 2.0, 'get_arm_state'):
return None
res = fut.result()
if res is None or not res.success:
return None
return res.ee_pose
def _save_png(self, bgr: np.ndarray, path: Path) -> None:
import cv2
if not cv2.imwrite(str(path), bgr):
raise RuntimeError(f'cv2.imwrite 失败：{path}')
def main(args=None) -> None:
rclpy.init(args=args)
node = HandeyeCalibrate()
code = 1
try:
code = node.run()
except KeyboardInterrupt:
code = 0
finally:
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
raise SystemExit(code)
if __name__ == '__main__':
main()
handeye/src/handeye_teach.py
+679
View File
@@ -0,0 +1,679 @@
#!/usr/bin/env python3
"""手眼示教：实时预览 + Free Drive + 按 Enter 拍照/记录关节。
两种后端
------------
ros（默认，整套栈正在运行）：
- 关节来自 ``/get_arm_state``
- 拖拽：示教器 Free Drive + 按住使能键
需保持：bash ~/ros2/scripts/run_real.sh
rdk（软件 Free Drive）：
- 先停掉 run_real.sh（RDK 独占）
- 按 'd' 通过 RDK 启用 FloatingJoint 拖拽
- 关节来自 RDK 的 robot.states
source ~/ros2/scripts/env.sh
ros2 run arm_application handeye_teach --ros-args -p backend:=rdk
界面按键（预览窗口）
Enter / Space 拍照 + 记录关节
d 启用 Free Drive（rdk）/ 显示示教器提示（ros）
s 停止 Free Drive（rdk）/ 保持
q / Esc 结束
"""
from __future__ import annotations
import os
import threading
import time
from datetime import datetime, timezone
from pathlib import Path
from typing import List, Optional
import numpy as np
import rclpy
from rcl_interfaces.msg import ParameterDescriptor
from rclpy.node import Node
from arm_application.ros_node import TolerantNode, param_as_str
HELP = """
================================================
手眼示教 TEACH：预览 -> 拖拽 -> 拍照+位姿
================================================
窗口按键：
Enter / Space = 拍照 + 保存关节
d = 开启 Free Drive
s = 关闭 Free Drive / 保持
q / Esc = 结束
提示：让棋盘清晰，并占到画面的约 30-70%
建议采 8-12 个角度有变化的位姿
================================================
"""
def _setup_matplotlib_font():
"""避免中文乱码：优先选用存在的中日韩（CJK）字体。"""
import matplotlib
matplotlib.use('QtAgg')
from matplotlib import font_manager, rcParams
candidates = [
'Noto Sans CJK SC',
'Noto Sans CJK JP',
'Noto Serif CJK SC',
'AR PL UMing CN',
'AR PL UKai CN',
'WenQuanYi Micro Hei',
'Source Han Sans CN',
'DejaVu Sans',
]
available = {f.name for f in font_manager.fontManager.ttflist}
chosen = None
for name in candidates:
if name in available:
chosen = name
break
if chosen is None:
# 按文件路径再试一次
for path in (
'/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc',
'/usr/share/fonts/opentype/noto/NotoSansCJK-Medium.ttc',
'/usr/share/fonts/opentype/noto/NotoSansCJK-DemiLight.ttc',
'/usr/share/fonts/truetype/arphic/uming.ttc',
):
if os.path.isfile(path):
font_manager.fontManager.addfont(path)
prop = font_manager.FontProperties(fname=path)
chosen = prop.get_name()
break
if chosen:
rcParams['font.family'] = chosen
rcParams['axes.unicode_minus'] = False
return chosen
class RdkBridge:
"""通过 rdk_py310 子进程（Python 3.10 ABI）与 Flexiv RDK 通信。"""
def __init__(self, robot_sn: str, logger=None) -> None:
import json
import subprocess
self._json = json
self._logger = logger
self.robot_sn = robot_sn
self._last_q: Optional[List[float]] = None
self._io = threading.Lock()
from arm_application.site_io import ros2_root
py = os.environ.get('RDK_PYTHON', '').strip() or str(
Path.home() / 'rdk_py310' / 'bin' / 'python'
)
bridge = ros2_root() / 'scripts' / 'rdk_drag_bridge.py'
if not bridge.is_file():
raise FileNotFoundError(f'找不到 rdk_drag_bridge.py：{bridge}')
# 只保留 Flexiv 的库 —— 不要继承 conda 的 LD_LIBRARY_PATH（会让 RDK 卡死）。
flexiv_lib = os.path.expanduser('~/flexiv_install/lib')
env = {
'HOME': os.environ.get('HOME', ''),
'PATH': f'{Path(py).parent}:/usr/bin:/bin',
'LD_LIBRARY_PATH': flexiv_lib,
'LANG': 'C.UTF-8',
'SITE_PC_IP': os.environ.get('SITE_PC_IP', ''),
'SITE_NET_IFACE': os.environ.get('SITE_NET_IFACE', ''),
}
# 不要继承 conda 的 PYTHONPATH。
# 重要：stderr 不能用 PIPE —— RDK 日志会填满管道并让桥接进程死锁
# （stdout 永远不返回）。改成写入日志文件。
log_path = Path('/tmp/rdk_drag_bridge.log')
self._err_file = open(log_path, 'w', encoding='utf-8')
self._proc = subprocess.Popen(
[py, str(bridge)],
stdin=subprocess.PIPE,
stdout=subprocess.PIPE,
stderr=self._err_file,
text=True,
bufsize=1,
env=env,
)
if logger:
logger.info(f'RDK 桥接已启动；日志 → {log_path}')
pong = self._ask({'cmd': 'ping'}, timeout=8.0)
if not pong.get('ok'):
raise RuntimeError(
f'RDK 桥接 ping 失败：{pong}。参见 {log_path}'
)
if logger:
logger.info('RDK 桥接 ping 正常，正在连接机器人...')
conn = self._ask(
{
'cmd': 'connect',
'sn': robot_sn,
'iface': os.environ.get('SITE_NET_IFACE', ''),
},
timeout=45.0,
)
if not conn.get('ok'):
raise RuntimeError(
f'RDK 连接失败：{conn.get("error")}。'
f'请先停掉 run_real.sh；切 AUTO Remote 并清除故障。参见 {log_path}'
)
if logger:
logger.info(f'RDK 桥接就绪（py={pong.get("py")}）sn={robot_sn}')
q0 = conn.get('q') or []
if len(q0) >= 7:
self._last_q = [float(v) for v in q0[:7]]
def _ask(self, msg: dict, timeout: float = 10.0) -> dict:
import select
# 同一时刻只允许一条在途命令：相机/GUI 绝不能共用这个管道。
if not self._io.acquire(timeout=max(timeout, 0.2)):
return {'ok': False, 'error': '桥接忙'}
try:
assert self._proc.stdin and self._proc.stdout
self._proc.stdin.write(self._json.dumps(msg) + '\n')
self._proc.stdin.flush()
end = time.monotonic() + timeout
while time.monotonic() < end:
if self._proc.poll() is not None:
tip = '/tmp/rdk_drag_bridge.log'
return {
'ok': False,
'error': f'桥接已退出 rc={self._proc.returncode}；参见 {tip}',
}
ready, _, _ = select.select([self._proc.stdout], [], [], 0.05)
if not ready:
continue
line = self._proc.stdout.readline()
if not line:
continue
line = line.strip()
if not line.startswith('{'):
continue
try:
return self._json.loads(line)
except Exception as exc: # noqa: BLE001
return {'ok': False, 'error': f'应答异常：{exc} {line!r}'}
return {'ok': False, 'error': '桥接超时'}
finally:
self._io.release()
def drag_on(self, primitive: str = 'FloatingJoint', mode: str = 'soft') -> dict:
return self._ask(
{'cmd': 'drag_on', 'primitive': primitive, 'mode': mode},
timeout=20.0,
)
def drag_off(self) -> dict:
return self._ask({'cmd': 'drag_off'}, timeout=6.0)
def joints(self) -> Optional[List[float]]:
res = self._ask({'cmd': 'joints'}, timeout=2.0)
if res.get('ok'):
q = res.get('q') or []
if len(q) >= 7:
self._last_q = [float(v) for v in q[:7]]
return self._last_q
return self._last_q
def move_joints(self, joints: List[float], speed: float = 0.15) -> dict:
"""按关节阻抗把臂缓慢送到已记录的 7 个关节角。speed 单位 rad/s。"""
return self._ask(
{'cmd': 'move_joints', 'q': [float(v) for v in joints[:7]], 'speed': float(speed)},
timeout=180.0,
)
def close(self) -> None:
try:
self._ask({'cmd': 'quit'}, timeout=3.0)
except Exception:
pass
try:
self._proc.terminate()
except Exception:
pass
try:
if getattr(self, '_err_file', None) is not None:
self._err_file.close()
except Exception:
pass
class HandeyeTeach(TolerantNode):
def __init__(self) -> None:
super().__init__('handeye_teach')
from arm_application.site_io import ros2_root
self.declare_parameter('output_dir', str(ros2_root() / 'handeye' / 'tmp'))
self.declare_parameter('width', 1280)
self.declare_parameter('height', 720)
self.declare_parameter('fps', 15)
self.declare_parameter('backend', 'ros') # ros | rdk
self.declare_parameter('robot_sn', os.environ.get('SITE_ROBOT_SN', '').strip())
self.declare_parameter('drag_primitive', 'FloatingJoint') # or FloatingCartesian
self._serial = param_as_str(self, 'camera_serial', '')
self.declare_parameter('auto_exposure', True)
self.declare_parameter('ae_limit_us', 1800.0)
self.declare_parameter('emitter_enabled', False)
self.declare_parameter('exposure_us', 0.0)
self.declare_parameter('gain', 0.0)
# 目标采集点数。0 = 不限（按 q 结束）。
# 设了就只是个"提醒"：采够时提示可以收工，不强制关窗 ——
# 常常采到一半发现某个角度还没覆盖，强制退出反而要重开。
self.declare_parameter('target_points', 0)
# 必须和 run_handeye.sh 的 TAUGHT 是同一个文件。只写 taught_poses.txt、
# 脚本去读 taught_poses_<模式>.txt，merge 就会「文件在、内容空」。
self.declare_parameter('poses_file', '')
self.backend = str(self.get_parameter('backend').value).strip().lower()
self.robot_sn = str(self.get_parameter('robot_sn').value)
self.drag_primitive = str(self.get_parameter('drag_primitive').value).strip()
self.output_dir = Path(str(self.get_parameter('output_dir').value)).expanduser()
self.images_dir = self.output_dir / 'images'
_pf = str(self.get_parameter('poses_file').value).strip()
self.poses_file = (
Path(_pf).expanduser() if _pf else self.output_dir / 'taught_poses.txt'
)
self.yaml_file = self.output_dir / 'taught_waypoint_positions.yaml'
self._last_capture_t = 0.0
self.images_dir.mkdir(parents=True, exist_ok=True)
self._captures: List[dict] = []
self._latest_bgr: Optional[np.ndarray] = None
self._frame_lock = threading.Lock()
self._quit = False
self._busy = False
self._cmd_pending = False
self._drag_on = False
self._robot = None
self._rdk = None
self._get_state = None
self._cam_thread: Optional[threading.Thread] = None
self._pipe = None
self._fps = 15
if self.backend == 'rdk':
self._init_rdk()
else:
self._init_ros_state()
self._start_camera()
font = _setup_matplotlib_font()
self._start_gui()
print(HELP)
if self.backend == 'rdk':
print('[backend=rdk] 按 d = 软拖拽（在 AUTO Remote 下可用，无需 Floating 许可）')
print(' run_real.sh 必须已停止。')
print(' 拖着就能按 Enter 拍照，不必先按 s')
else:
print('[backend=ros] 用示教器 Free Drive + 按住使能键来拖拽。')
print(' 或停掉 run_real.sh 后改用：-p backend:=rdk')
if font:
self.get_logger().info(f'界面字体：{font}')
self.create_timer(1.0 / max(1, self._fps), self._tick)
def _init_ros_state(self) -> None:
from arm_msgs.srv import GetArmState
self._get_state = self.create_client(GetArmState, 'get_arm_state')
if not self._get_state.wait_for_service(timeout_sec=10.0):
raise RuntimeError(
'get_arm_state 不可用。请启动 run_real.sh，'
'或改用：-p backend:=rdk（需先停掉整套栈）'
)
def _init_rdk(self) -> None:
self.get_logger().info(f'正在把 RDK 桥接连到 {self.robot_sn} ...')
self._rdk = RdkBridge(self.robot_sn, logger=self.get_logger())
self._robot = True # 标记：RDK 后端已就绪
self.get_logger().info('RDK 已连接。按 d 启用 Free Drive。')
def _start_camera(self) -> None:
import pyrealsense2 as rs
w = int(self.get_parameter('width').value)
h = int(self.get_parameter('height').value)
fps = int(self.get_parameter('fps').value)
pipe = rs.pipeline()
profile = None
last_err = None
for ww, hh, ff in [
(w, h, fps), (w, h, 15), (w, h, 10),
(640, 480, 30), (640, 480, 15),
]:
try:
cfg = rs.config()
if self._serial:
cfg.enable_device(self._serial)
cfg.enable_stream(rs.stream.color, ww, hh, rs.format.bgr8, ff)
profile = pipe.start(cfg)
self._fps = ff
self.get_logger().info(f'相机 {ww}x{hh}@{ff}Hz')
break
except Exception as exc: # noqa: BLE001
last_err = exc
try:
pipe.stop()
except Exception: # noqa: BLE001
pass
pipe = rs.pipeline()
if profile is None:
raise RuntimeError(
f'无法打开 D405 —— 请先停掉 realsense_color_pub。（{last_err}）'
)
self._pipe = pipe
try:
from arm_application.handeye_realsense import apply_realsense_exposure
apply_realsense_exposure(
profile.get_device(),
auto_exposure=bool(self.get_parameter('auto_exposure').value),
ae_limit_us=float(self.get_parameter('ae_limit_us').value),
emitter_enabled=bool(self.get_parameter('emitter_enabled').value),
exposure_us=(
float(self.get_parameter('exposure_us').value) or None
),
gain=(float(self.get_parameter('gain').value) or None),
log=self.get_logger().info,
)
except Exception as exc: # noqa: BLE001
self.get_logger().warn(f'曝光设置应用失败：{exc}')
self._cam_thread = threading.Thread(target=self._cam_loop, name='d405', daemon=True)
self._cam_thread.start()
def _cam_loop(self) -> None:
while not self._quit:
try:
frames = self._pipe.wait_for_frames(timeout_ms=400)
color = frames.get_color_frame()
except Exception: # noqa: BLE001
time.sleep(0.02)
continue
if not color:
continue
bgr = np.asanyarray(color.get_data()).copy()
with self._frame_lock:
self._latest_bgr = bgr
def _start_gui(self) -> None:
import matplotlib.pyplot as plt
self._plt = plt
self._fig, self._ax = plt.subplots(1, 1, figsize=(11, 7))
self._fig.canvas.manager.set_window_title(
'handeye_teach | Enter=拍照 d=拖拽 s=停止 q=退出'
)
self._im = None
self._status = self._ax.text(
0.02,
0.98,
self._status_text(),
transform=self._ax.transAxes,
color='lime',
fontsize=13,
va='top',
fontweight='bold',
bbox=dict(boxstyle='round', facecolor='black', alpha=0.6),
)
self._ax.axis('off')
self._fig.canvas.mpl_connect('key_press_event', self._on_key)
plt.ion()
plt.show(block=False)
def _status_text(self) -> str:
drag = '拖拽开启' if self._drag_on else '拖拽关闭'
if self.backend == 'rdk':
tip = 'd=软拖拽 s=保持'
else:
tip = '示教器 FreeDrive+使能'
return (
f'已采集:{len(self._captures)} [{drag}] '
f'{tip} | Enter=拍照 q=退出'
)
def _on_key(self, event) -> None:
key = (event.key or '').lower()
if key in ('q', 'escape'):
self._quit = True
self.get_logger().info('收到退出请求')
return
if self._cmd_pending:
return
if key in ('enter', ' ', 'space', 'd', 's'):
self._cmd_pending = True
threading.Thread(target=self._run_key, args=(key,), daemon=True).start()
def _run_key(self, key: str) -> None:
try:
if key in ('enter', ' ', 'space'):
self._capture_once()
elif key == 'd':
self._enable_drag()
elif key == 's':
self._disable_drag()
except Exception as exc: # noqa: BLE001
self.get_logger().error(f'按键 {key}：{exc}')
finally:
self._cmd_pending = False
def _enable_drag(self) -> None:
if self.backend != 'rdk':
self._drag_on = True
self.get_logger().warn(
'ROS 后端：请用示教器的 Free Drive + 按住使能键。'
'想用软件拖拽：先停掉 run_real.sh，然后执行 '
'ros2 run arm_application handeye_teach --ros-args -p backend:=rdk'
)
self._status.set_text(self._status_text())
return
self.get_logger().info('正在启动软拖拽（预览保持实时刷新）...')
# soft = AUTO Remote 模式下的关节阻抗拖拽（不需要 Floating 许可）
res = self._rdk.drag_on(self.drag_primitive, mode='soft')
if res.get('ok'):
self._drag_on = True
self.get_logger().info(
f'拖拽已开启 mode={res.get("mode")} —— 推着机械臂走，Enter=拍照（s=保持，可选）'
)
if res.get('hint'):
self.get_logger().info(str(res.get('hint')))
else:
self._drag_on = False
self.get_logger().error(f'拖拽失败：{res.get("error")}')
# 状态文字由 GUI 线程里的 _tick 刷新
def _disable_drag(self) -> None:
if self.backend == 'rdk' and getattr(self, '_rdk', None) is not None:
res = self._rdk.drag_off()
if not res.get('ok'):
self.get_logger().warn(f'停止拖拽：{res.get("error")}')
self._drag_on = False
self.get_logger().info('Free Drive 已关闭 / 保持')
def _tick(self) -> None:
if self._quit:
return
with self._frame_lock:
bgr = None if self._latest_bgr is None else self._latest_bgr.copy()
if bgr is None:
try:
self._fig.canvas.flush_events()
except Exception: # noqa: BLE001
pass
return
rgb = bgr[:, :, ::-1]
if self._im is None:
self._im = self._ax.imshow(rgb)
else:
self._im.set_data(rgb)
try:
self._status.set_text(self._status_text())
self._fig.canvas.draw_idle()
self._fig.canvas.flush_events()
except Exception: # noqa: BLE001
pass
def _read_joints(self) -> Optional[List[float]]:
if self.backend == 'rdk' and getattr(self, '_rdk', None) is not None:
q = self._rdk.joints()
if q is None:
self.get_logger().error('读取 RDK 关节失败')
return q
from arm_msgs.srv import GetArmState
fut = self._get_state.call_async(GetArmState.Request())
end = time.monotonic() + 2.0
while rclpy.ok() and not fut.done() and time.monotonic() < end:
rclpy.spin_once(self, timeout_sec=0.02)
res = fut.result() if fut.done() else None
if res is None or not res.success:
return None
q = list(res.joint_positions)
return [float(v) for v in q[:7]] if len(q) >= 7 else None
def _capture_once(self) -> None:
if self._busy:
return
self._busy = True
try:
with self._frame_lock:
bgr = None if self._latest_bgr is None else self._latest_bgr.copy()
if bgr is None:
self.get_logger().warn('还没有相机帧')
return
q = self._read_joints()
if q is None:
self.get_logger().error('读取关节失败')
return
now = time.monotonic()
if now - self._last_capture_t < 0.8:
self.get_logger().warn('按太快，忽略（防 Enter 连发）')
return
if self._captures:
last = self._captures[-1]['joints']
delta = max(abs(a - b) for a, b in zip(q, last))
if delta < 0.015:
self.get_logger().warn(
f'关节几乎没动（Δ={delta:.4f} rad），忽略这次拍照。'
'先拖到另一个姿态再按 Enter。'
)
return
self._last_capture_t = now
idx = len(self._captures)
stamp = datetime.now(timezone.utc).strftime('%Y%m%d_%H%M%S')
img_path = self.images_dir / f'teach_{idx:02d}_{stamp}.png'
try:
import cv2
cv2.imwrite(str(img_path), bgr)
except Exception: # noqa: BLE001
from PIL import Image as PILImage
PILImage.fromarray(bgr[:, :, ::-1]).save(str(img_path))
line = ', '.join(f'{v:.6f}' for v in q) + ','
self._captures.append(
{'index': idx, 'joints': q, 'image': str(img_path), 'line': line}
)
with self.poses_file.open('a', encoding='utf-8') as f:
f.write(f'# pose {idx} {stamp}\n')
f.write(line + '\n')
f.write(f'# image {img_path}\n')
print(f'\n# pose {idx} {img_path.name}')
print(line)
n = idx + 1
try:
target = int(self.get_parameter('target_points').value)
except Exception: # noqa: BLE001
target = 0
if target > 0:
if n < target:
self.get_logger().info(
f'[{n}/{target}] 已保存照片+关节 —— 还差 {target - n} 个点'
)
elif n == target:
self.get_logger().info(
f'[{n}/{target}] 已采够 {target} 个点。'
' 建议按 q 结束，然后跑 merge + calibrate；'
' 想多采几个点也可以继续按 Enter。'
)
else:
self.get_logger().info(
f'[{n}/{target}] 已保存照片+关节 —— 已超出目标 '
f'{n - target} 个点（会用全部点求解）'
)
else:
self.get_logger().info(f'[{n}] 已保存照片+关节')
self._write_yaml_snippet()
finally:
self._busy = False
def _write_yaml_snippet(self) -> None:
lines = [
'# 粘贴到 handeye_calibrate.yaml -> waypoint_positions',
f'# num_poses: {len(self._captures)}',
'waypoint_positions: [',
]
for c in self._captures:
lines.append(f' # pose {c["index"]}')
lines.append(f' {c["line"]}')
lines.append(']')
self.yaml_file.write_text('\n'.join(lines) + '\n', encoding='utf-8')
def finish(self) -> None:
self._quit = True
if self._cam_thread is not None:
self._cam_thread.join(timeout=1.5)
if self._drag_on:
self._disable_drag()
if getattr(self, '_rdk', None) is not None:
try:
self._rdk.close()
except Exception:
pass
n = len(self._captures)
print('\n========== 示教完成 ==========')
print(f'位姿: {n}')
print(f'关节: {self.poses_file}')
print(f'yaml: {self.yaml_file}')
print(f'图像: {self.images_dir}')
if n >= 1:
print('\n--- 粘贴到 handeye_calibrate.yaml ---\n')
print(self.yaml_file.read_text(encoding='utf-8'))
print('同时设置 num_poses:', n)
if n < 5:
self.get_logger().warn(f'只有 {n} 个位姿；至少需要 5 个（建议 8-12 个）。')
def main(args=None) -> None:
rclpy.init(args=args)
node = None
try:
node = HandeyeTeach()
while rclpy.ok() and not node._quit:
rclpy.spin_once(node, timeout_sec=0.05)
except SystemExit:
pass
except KeyboardInterrupt:
pass
finally:
if node is not None:
node.finish()
try:
node._pipe.stop()
except Exception: # noqa: BLE001
pass
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__':
main()
handeye/src/handeye_waypoints.py
+525
View File
@@ -0,0 +1,525 @@
#!/usr/bin/env python3
"""手眼标定路点执行器。
逐个移动到每个关节路点，等到位后停留一段时间，
让相机（或操作者）采集一张标定图像。
capture_mode:
auto — 停留 dwell_sec 后进入下一个位姿
keypress — 在终端按 Enter 继续
preview — OpenCV 实时相机 + Enter/Space 确认（示教时推荐）
处于 ``preview`` 模式时，可在预览窗口打开期间用示教器的 **Free Drive**
拖拽微调（ros2_control 保持 AUTO Remote；拖拽经由示教器，而非 RDK）。
"""
from __future__ import annotations
import csv
import array
import sys
import threading
import time
from datetime import datetime, timezone
from pathlib import Path
from typing import List, Optional
import numpy as np
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Image
from std_msgs.msg import String
from arm_application.handeye_realsense import (
is_realsense_source,
resolve_realsense_topics,
)
from arm_application.site_io import joint_prefix_default
from arm_msgs.action import FollowJointTrajectory
from arm_msgs.srv import GetArmState
from arm_application.ros_node import TolerantNode
try:
import cv2
except Exception: # noqa: BLE001
cv2 = None # type: ignore
DEFAULT_JOINTS = [
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
]
DEFAULT_TIMES = [4.0] * 12
DEFAULT_POSITIONS = [
0.00, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
0.35, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
-0.35, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
0.00, -0.90, 0.00, 1.70, 0.00, 0.85, 0.00,
0.00, -0.50, 0.00, 1.40, 0.00, 0.55, 0.00,
0.20, -0.70, 0.15, 1.57, 0.40, 0.70, 0.30,
-0.20, -0.70, -0.15, 1.57, -0.40, 0.70, -0.30,
0.25, -0.80, 0.10, 1.65, 0.25, 0.90, -0.40,
-0.25, -0.80, -0.10, 1.65, -0.25, 0.90, 0.40,
0.15, -0.60, 0.20, 1.50, -0.35, 0.60, 0.50,
-0.15, -0.60, -0.20, 1.50, 0.35, 0.60, -0.50,
0.00, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
]
PREVIEW_HELP_GUI = """
=== 位姿预览 / 拖拽调整（OpenCV 窗口）===
1. 示教器：Free Drive + 按住使能键拖拽微调
2. 预览窗口：确认棋盘在画面内且清晰
3. 键盘：Enter / Space = 下一个位姿；q / Esc = 退出
"""
PREVIEW_HELP_HEADLESS = """
=== 位姿预览 / 拖拽调整（无 OpenCV 窗口）===
1. 另开终端查看画面：
ros2 run rqt_image_view rqt_image_view /handeye/preview_image
2. 示教器：Free Drive + 按住使能键拖拽微调
3. 本终端：Enter = 确认下一个位姿；输入 q 回车 = 退出
"""
def _cv_gui_available() -> bool:
if cv2 is None:
return False
try:
cv2.namedWindow('_handeye_gui_probe', cv2.WINDOW_NORMAL)
cv2.destroyWindow('_handeye_gui_probe')
return True
except Exception: # noqa: BLE001
return False
def _image_to_bgr(msg: Image) -> np.ndarray:
h, w = int(msg.height), int(msg.width)
enc = (msg.encoding or '').lower()
data = np.frombuffer(msg.data, dtype=np.uint8)
if enc in ('bgr8', 'rgb8'):
img = data.reshape(h, w, 3)
return img[:, :, ::-1].copy() if enc == 'rgb8' else img
if enc in ('mono8', '8uc1'):
gray = data.reshape(h, w)
return np.stack([gray, gray, gray], axis=-1)
if enc in ('bgra8', 'rgba8'):
img = data.reshape(h, w, 4)
bgr = img[:, :, :3]
return bgr[:, :, ::-1].copy() if enc == 'rgba8' else bgr
raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}')
class HandeyeWaypoints(TolerantNode):
def __init__(self) -> None:
super().__init__('handeye_waypoints')
self.declare_parameter('joint_prefix', joint_prefix_default())
self.declare_parameter('joint_names', DEFAULT_JOINTS)
self.declare_parameter('velocity_scale', 0.12)
self.declare_parameter('action_name', 'follow_joint_trajectory')
self.declare_parameter('waypoint_times', DEFAULT_TIMES)
self.declare_parameter('waypoint_positions', DEFAULT_POSITIONS)
self.declare_parameter('dwell_sec', 2.5)
# auto | keypress | preview
self.declare_parameter('capture_mode', 'auto')
self.declare_parameter('start_index', 0)
self.declare_parameter('log_csv', '')
self.declare_parameter('capture_topic', 'handeye/capture')
self.declare_parameter('camera_source', 'd405')
self.declare_parameter('camera_namespace', '')
self.declare_parameter('image_topic', '')
self.declare_parameter('preview_window', True)
self.declare_parameter('append_poses_file', '/tmp/handeye_taught_poses.txt')
prefix = str(self.get_parameter('joint_prefix').value or '')
base = list(
self.get_parameter('joint_names').get_parameter_value().string_array_value
or DEFAULT_JOINTS
)
self.joint_names = [f'{prefix}{n}' for n in base]
self.velocity_scale = float(self.get_parameter('velocity_scale').value)
self.action_name = str(self.get_parameter('action_name').value)
self.dwell_sec = float(self.get_parameter('dwell_sec').value)
self.capture_mode = str(self.get_parameter('capture_mode').value).strip().lower()
self.start_index = max(0, int(self.get_parameter('start_index').value))
self.log_csv = str(self.get_parameter('log_csv').value or '').strip()
topic = str(self.get_parameter('capture_topic').value)
self.preview_window = bool(self.get_parameter('preview_window').value)
self.append_poses_file = str(
self.get_parameter('append_poses_file').value or ''
).strip()
cam_src = str(self.get_parameter('camera_source').value).strip().lower()
img_topic = str(self.get_parameter('image_topic').value or '').strip()
ns = str(self.get_parameter('camera_namespace').value or '').strip()
if is_realsense_source(cam_src) and not img_topic:
img_topic, _ = resolve_realsense_topics(
cam_src, camera_namespace=ns, image_topic='', camera_info_topic=''
)
self.image_topic = img_topic
self._client = ActionClient(self, FollowJointTrajectory, self.action_name)
self._get_state = self.create_client(GetArmState, 'get_arm_state')
self._capture_pub = self.create_publisher(String, topic, 10)
self._preview_pub = self.create_publisher(Image, 'handeye/preview_image', 10)
self._latest_bgr: Optional[np.ndarray] = None
self._cv_gui = _cv_gui_available() and self.preview_window
if self.capture_mode == 'preview' and not self._cv_gui:
self.get_logger().warn(
'OpenCV 没有 GUI（无头环境）。请在 /handeye/preview_image 上预览，'
'在本终端按 Enter 确认。'
)
if self.image_topic and self.capture_mode == 'preview':
self.create_subscription(
Image, self.image_topic, self._on_image, qos_profile_sensor_data
)
self.get_logger().info(f'预览图像：{self.image_topic}')
def _on_image(self, msg: Image) -> None:
try:
self._latest_bgr = _image_to_bgr(msg)
except Exception: # noqa: BLE001
pass
def run(self) -> int:
if self.capture_mode == 'preview':
if cv2 is None:
self.get_logger().error('preview 模式需要 OpenCV（cv2）')
return 1
if not self.image_topic:
self.get_logger().error('preview 模式需要 image_topic 或 camera_source')
return 1
times = list(self.get_parameter('waypoint_times').value)
positions = list(self.get_parameter('waypoint_positions').value)
n_j = len(self.joint_names)
n_pts = len(times)
if n_pts < 1:
self.get_logger().error('waypoint_times 为空')
return 1
if len(positions) != n_pts * n_j:
self.get_logger().error(
f'waypoint_positions 长度 {len(positions)} != {n_pts}*{n_j}'
)
return 1
if self.start_index >= n_pts:
self.get_logger().error(
f'start_index {self.start_index} >= 路点总数 {n_pts}'
)
return 1
if not self._client.wait_for_server(timeout_sec=15.0):
self.get_logger().error(
f'Action [{self.action_name}] 不可用。'
'请先启动整套栈：bash scripts/run_real.sh'
)
return 2
csv_file = None
csv_writer: Optional[csv.writer] = None
if self.log_csv:
path = Path(self.log_csv).expanduser()
path.parent.mkdir(parents=True, exist_ok=True)
csv_file = path.open('w', newline='', encoding='utf-8')
csv_writer = csv.writer(csv_file)
csv_writer.writerow(
['index', 'utc_iso', 'move_duration_s']
+ [f'target_{n}' for n in self.joint_names]
+ [f'measured_{n}' for n in self.joint_names]
)
self.get_logger().info(f'正在记录采集数据 → {path}')
if self.capture_mode == 'preview':
print(PREVIEW_HELP_GUI if self._cv_gui else PREVIEW_HELP_HEADLESS)
self.get_logger().info(
f'手眼标定：{n_pts} 个位姿，start={self.start_index}，'
f'v_scale={self.velocity_scale}，dwell={self.dwell_sec}s，'
f'mode={self.capture_mode}'
)
self.get_logger().warn(
'清理工作空间；让棋盘保持在视野内。Ctrl+C 可中止。'
)
win = 'handeye_preview'
try:
for i in range(self.start_index, n_pts):
q = positions[i * n_j : (i + 1) * n_j]
move_t = max(1.0, float(times[i]))
self.get_logger().info(
f'[{i + 1}/{n_pts}] 移动中（约 {move_t:.1f}s）...'
)
code = self._move_one(q, move_t)
if code != 0:
return code
measured = self._read_joints()
self._announce_capture(i, n_pts, q, measured)
if csv_writer is not None and csv_file is not None:
csv_writer.writerow(
[i, datetime.now(timezone.utc).isoformat(), move_t]
+ list(q)
+ list(measured if measured else [float('nan')] * n_j)
)
csv_file.flush()
if not self._wait_capture(i, n_pts):
self.get_logger().info('用户已中止')
return 0
# 在 preview 模式下确认后，保存示教关节
if self.capture_mode == 'preview':
taught = self._read_joints()
if taught:
self._save_taught_pose(i, taught)
finally:
if csv_file is not None:
csv_file.close()
if cv2 is not None:
try:
cv2.destroyWindow(win)
except Exception: # noqa: BLE001
pass
self.get_logger().info('手眼标定路点序列执行完毕')
if self.append_poses_file and self.capture_mode == 'preview':
self.get_logger().info(
f'示教位姿已保存 → {self.append_poses_file}'
)
return 0
def _save_taught_pose(self, index: int, joints: List[float]) -> None:
line = ', '.join(f'{v:.6f}' for v in joints[:7]) + ','
print(f'# pose {index}')
print(line)
if not self.append_poses_file:
return
path = Path(self.append_poses_file).expanduser()
path.parent.mkdir(parents=True, exist_ok=True)
with path.open('a', encoding='utf-8') as f:
f.write(f'# pose {index}\n')
f.write(line + '\n')
self.get_logger().info(f'已保存位姿 {index} → {path}')
def _move_one(self, positions: List[float], duration: float) -> int:
goal = FollowJointTrajectory.Goal()
goal.joint_names = self.joint_names
goal.num_points = 1
goal.velocity_scale = self.velocity_scale
goal.positions = list(positions)
goal.time_from_start = [float(duration)]
send = self._client.send_goal_async(goal)
if not self._wait_future(send, 10.0, '发送目标'):
return 3
handle = send.result()
if handle is None or not handle.accepted:
self.get_logger().error('目标被拒绝')
return 4
res_fut = handle.get_result_async()
if not self._wait_future(res_fut, max(60.0, duration * 3.0), '结果'):
return 5
result = res_fut.result().result
if not result.success:
self.get_logger().error(f'移动失败：{result.message}')
return 6
return 0
def _wait_future(self, fut, timeout: float, label: str) -> bool:
deadline = time.monotonic() + timeout
while rclpy.ok() and not fut.done():
rclpy.spin_once(self, timeout_sec=0.05)
if time.monotonic() > deadline:
self.get_logger().error(f'等待 {label} 超时')
return False
return fut.done()
def _read_joints(self) -> Optional[List[float]]:
if not self._get_state.wait_for_service(timeout_sec=0.5):
return None
fut = self._get_state.call_async(GetArmState.Request())
if not self._wait_future(fut, 2.0, 'get_arm_state'):
return None
res = fut.result()
if res is None or not res.success:
return None
return list(res.joint_positions)
def _announce_capture(
self,
index: int,
n_pts: int,
target: List[float],
measured: Optional[List[float]],
) -> None:
msg = String()
msg.data = f'capture:{index}:{n_pts}'
self._capture_pub.publish(msg)
q_str = ', '.join(f'{v:.4f}' for v in (measured or target))
if self.capture_mode == 'preview':
self.get_logger().info(
f'[{index + 1}/{n_pts}] 已到位 —— 用 Free Drive 调整，然后按 Enter/Space'
)
else:
self.get_logger().info(
f'[{index + 1}/{n_pts}] 已到位 —— 现在采集 q=[{q_str}]'
)
def _wait_capture(self, index: int, n_pts: int) -> bool:
if self.capture_mode == 'preview':
return self._wait_preview(index, n_pts)
if self.capture_mode in ('key', 'keypress', 'manual'):
self.get_logger().info(
'采集完成后按 Enter（q+Enter 退出）...'
)
for _ in range(5):
rclpy.spin_once(self, timeout_sec=0.05)
line = sys.stdin.readline()
if not line or line.strip().lower() in ('q', 'quit', 'exit'):
return False
return True
end = time.monotonic() + max(0.0, self.dwell_sec)
while rclpy.ok() and time.monotonic() < end:
rclpy.spin_once(self, timeout_sec=0.05)
return rclpy.ok()
def _annotate_preview(
self, bgr: np.ndarray, index: int, n_pts: int
) -> np.ndarray:
vis = bgr.copy()
if cv2 is None:
return vis
cv2.putText(
vis,
f'位姿 {index + 1}/{n_pts}',
(12, 28),
cv2.FONT_HERSHEY_SIMPLEX,
0.7,
(0, 255, 0),
2,
)
cv2.putText(
vis,
'Free Drive 拖拽 | Enter=下一个 q=退出',
(12, 56),
cv2.FONT_HERSHEY_SIMPLEX,
0.6,
(0, 255, 255),
2,
)
return vis
def _publish_preview(self, bgr: np.ndarray) -> None:
msg = Image()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = 'camera'
msg.height, msg.width = bgr.shape[0], bgr.shape[1]
msg.encoding = 'bgr8'
msg.is_bigendian = 0
msg.step = msg.width * 3
msg.data = array.array('B', bgr.tobytes()) # 快速路径；用 bytes 会多花约 40 ms
self._preview_pub.publish(msg)
def _wait_preview(self, index: int, n_pts: int) -> bool:
if self._cv_gui:
return self._wait_preview_opencv(index, n_pts)
return self._wait_preview_terminal(index, n_pts)
def _wait_preview_terminal(self, index: int, n_pts: int) -> bool:
confirmed = threading.Event()
quit_req = threading.Event()
def _stdin_reader() -> None:
try:
line = sys.stdin.readline()
except Exception: # noqa: BLE001
quit_req.set()
return
if not line or line.strip().lower() in ('q', 'quit', 'exit'):
quit_req.set()
else:
confirmed.set()
self.get_logger().info(
f'[{index + 1}/{n_pts}] 用 Free Drive 调整，然后按 Enter '
'（q+Enter 退出）'
)
reader = threading.Thread(target=_stdin_reader, daemon=True)
reader.start()
no_frame_warn = False
while rclpy.ok():
rclpy.spin_once(self, timeout_sec=0.05)
if confirmed.is_set():
return True
if quit_req.is_set():
return False
if not reader.is_alive() and not confirmed.is_set() and not quit_req.is_set():
return False
bgr = self._latest_bgr
if bgr is None:
if not no_frame_warn:
self.get_logger().warn(
f'正在等待 {self.image_topic} ...',
throttle_duration_sec=3.0,
)
no_frame_warn = True
continue
vis = self._annotate_preview(bgr, index, n_pts)
self._publish_preview(vis)
return False
def _wait_preview_opencv(self, index: int, n_pts: int) -> bool:
assert cv2 is not None
win = 'handeye_preview'
cv2.namedWindow(win, cv2.WINDOW_NORMAL)
no_frame_warn = False
while rclpy.ok():
rclpy.spin_once(self, timeout_sec=0.02)
bgr = self._latest_bgr
if bgr is None:
if not no_frame_warn:
self.get_logger().warn(
f'正在等待 {self.image_topic} ...',
throttle_duration_sec=3.0,
)
no_frame_warn = True
key = cv2.waitKey(30) & 0xFF
else:
vis = self._annotate_preview(bgr, index, n_pts)
self._publish_preview(vis)
cv2.imshow(win, vis)
key = cv2.waitKey(30) & 0xFF
if key in (13, 10, 32):
return True
if key in (27, ord('q'), ord('Q')):
return False
return False
def main(args=None) -> None:
rclpy.init(args=args)
node = HandeyeWaypoints()
code = 1
try:
code = node.run()
except KeyboardInterrupt:
code = 0
finally:
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
raise SystemExit(code)
if __name__ == '__main__':
main()
handeye/src/merge_handeye_poses.py
Executable
+163
View File
@@ -0,0 +1,163 @@
#!/usr/bin/env python3
"""Merge taught joint lines into handeye waypoints YAML.
Input (from handeye_teach / handeye_waypoints append file):
# pose 0
0.123456, -0.700000, ...
Target can be either:
- handeye/poses/<臂号>_<模式>.yaml (/** ros__parameters)
- handeye/calibrate.yaml (handeye_calibrate: ros__parameters)
Usage:
python3 scripts/merge_handeye_poses.py \\
~/ros2/handeye/tmp/89/taught_poses.txt \\
~/ros2/handeye/poses/89_eye_in_hand.yaml
# or via wrapper:
bash ~/ros2/scripts/run_handeye.sh merge
"""
from __future__ import annotations
import re
import sys
from pathlib import Path
def parse_poses(path: Path) -> list[list[float]]:
lines = path.read_text(encoding="utf-8").splitlines()
poses: list[list[float]] = []
buf: list[float] = []
num_re = re.compile(r"[-+]?(?:\d+\.\d*|\.\d+|\d+)(?:[eE][-+]?\d+)?")
for raw in lines:
line = raw.split("#", 1)[0].strip()
if not line:
if buf:
if len(buf) != 7:
raise ValueError(f"Expected 7 joints, got {len(buf)}: {buf}")
poses.append(buf)
buf = []
continue
vals = [float(x) for x in num_re.findall(line)]
if not vals:
continue
if len(vals) == 7:
poses.append(vals)
elif len(vals) < 7:
buf.extend(vals)
if len(buf) > 7:
raise ValueError(f"Too many joint values in line: {line!r}")
else:
poses.append(vals[:7])
if buf:
if len(buf) != 7:
raise ValueError(f"Expected 7 joints at EOF, got {len(buf)}")
poses.append(buf)
return poses
def unique_poses(poses: list[list[float]], tol: float = 0.015) -> list[list[float]]:
"""关节几乎不动的连拍只留一条。阈值 0.015 rad ≈ 0.86°。"""
kept: list[list[float]] = []
for pose in poses:
if any(max(abs(a - b) for a, b in zip(pose, old)) < tol for old in kept):
continue
kept.append(pose)
return kept
def format_positions_block(poses: list[list[float]], indent: str = " ") -> str:
rows = [f"{indent}waypoint_positions: ["]
for i, q in enumerate(poses):
rows.append(f"{indent} # pose {i}")
s = ", ".join(f"{v:.6f}" for v in q) + ","
rows.append(f"{indent} {s}")
rows.append(f"{indent}]")
return "\n".join(rows)
def merge(yaml_path: Path, poses: list[list[float]]) -> None:
text = yaml_path.read_text(encoding="utf-8").replace("\r\n", "\n").replace("\r", "\n")
# 探测 waypoint_positions 的缩进（通常 2 空格或 4 空格）
m_ind = re.search(r"^([ \t]*)waypoint_positions:\s*\[", text, re.MULTILINE)
indent = m_ind.group(1) if m_ind else " "
block = format_positions_block(poses, indent=indent)
pattern = re.compile(
rf"{re.escape(indent)}waypoint_positions:\s*\[[\s\S]*?{re.escape(indent)}\]",
re.MULTILINE,
)
# 也要匹配"单行空列表"的写法：waypoint_positions: []
one_line = re.compile(
rf"{re.escape(indent)}waypoint_positions:\s*\[\s*\]",
re.MULTILINE,
)
if pattern.search(text):
new_text = pattern.sub(block, text, count=1)
elif one_line.search(text):
new_text = one_line.sub(block, text, count=1)
else:
raise RuntimeError("waypoint_positions block not found in YAML")
# 顺手把 num_poses 同步成实际点数（如果原本就有这个键）
n = len(poses)
if re.search(r"^\s*num_poses:\s*\d+", new_text, re.MULTILINE):
new_text = re.sub(
r"^(\s*num_poses:\s*)\d+",
rf"\g<1>{n}",
new_text,
count=1,
flags=re.MULTILINE,
)
else:
# 原本没有 num_poses，就插在 waypoint_positions 前面
new_text = new_text.replace(
block,
f"{indent}num_poses: {n}\n{block}",
1,
)
yaml_path.write_text(new_text, encoding="utf-8")
def main(argv: list[str]) -> int:
if len(argv) != 3:
print(
"用法：merge_handeye_poses.py <taught_poses.txt> <waypoints.yaml>",
file=sys.stderr,
)
return 1
src = Path(argv[1]).expanduser()
dst = Path(argv[2]).expanduser()
if not src.is_file():
print(f"Not found: {src}", file=sys.stderr)
return 2
if not dst.is_file():
print(f"Not found: {dst}", file=sys.stderr)
return 3
poses = parse_poses(src)
uniq = unique_poses(poses)
if len(uniq) < 5:
print(
f"Need >=5 unique poses, got {len(poses)} lines / {len(uniq)} unique",
file=sys.stderr,
)
return 4
if len(uniq) < len(poses):
print(
f"警告: {len(poses)} 条里只有 {len(uniq)} 个姿态不同，已丢掉连按重复",
file=sys.stderr,
)
poses = uniq
merge(dst, poses)
print(f"Merged {len(poses)} poses -> {dst}")
return 0
if __name__ == "__main__":
raise SystemExit(main(sys.argv))