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