#!/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()