cheng
772 lines
33 KiB
Python
772 lines
33 KiB
Python
#!/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()
|