Files

37 lines
2.3 KiB
Python

#!/usr/bin/env python3
"""Capture calibration-board images from the running ROS camera; no device commands."""
import json,time,os
from storage import stamp as session_timestamp
from datetime import datetime,timezone
from pathlib import Path
import numpy as np,cv2,rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Image,CameraInfo
ROOT=Path(__file__).resolve().parents[1]
def main():
rclpy.init();n=Node('capture_board_readonly');frames=[];infos=[];last=[0.]
def cb(msg):
if len(frames)>=5 or time.monotonic()-last[0]<.35:return
if msg.encoding not in ('rgb8','bgr8'):return
image=np.frombuffer(msg.data,dtype=np.uint8).reshape(msg.height,msg.step)[:,:msg.width*3].reshape(msg.height,msg.width,3).copy()
if msg.encoding=='rgb8':image=cv2.cvtColor(image,cv2.COLOR_RGB2BGR)
frames.append((image,{'sec':msg.header.stamp.sec,'nanosec':msg.header.stamp.nanosec,'frame_id':msg.header.frame_id}));last[0]=time.monotonic()
n.create_subscription(Image,'/camera/camera/color/image_raw',cb,qos_profile_sensor_data)
n.create_subscription(CameraInfo,'/camera/camera/color/camera_info',lambda m:infos.append(m),qos_profile_sensor_data)
deadline=time.monotonic()+20
while (len(frames)<5 or not infos) and time.monotonic()<deadline:rclpy.spin_once(n,timeout_sec=.1)
if not frames or not infos:
missing=[]
if not frames:missing.append('/camera/camera/color/image_raw')
if not infos:missing.append('/camera/camera/color/camera_info')
n.destroy_node();rclpy.shutdown()
raise SystemExit('20秒内未收到相机消息:'+', '.join(missing)+
'\n当前ROS_DOMAIN_ID='+os.environ.get('ROS_DOMAIN_ID','0')+
'。capture只订阅已运行的相机,请先启动同一ROS域内的D405驱动;也需核对话题名。')
out=ROOT/'data'/('handeye_'+session_timestamp());out.mkdir(parents=True)
for i,(im,frame_stamp) in enumerate(frames):cv2.imwrite(str(out/('rgb.png' if i==0 else f'rgb_{i:02d}.png')),im)
info=infos[-1];meta={'source':'live_ROS_D405','K':np.array(info.k).reshape(3,3).tolist(),'D':list(info.d),'distortion_model':info.distortion_model,'width':info.width,'height':info.height,'frames':[s for im,s in frames],'board_fixed_user_confirmed':True,'motion_sent':False}
(out/'camera.json').write_text(json.dumps(meta,indent=2));print(out,flush=True);n.destroy_node();rclpy.shutdown()
if __name__=='__main__':main()