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