diff --git a/.gitignore b/.gitignore index dc24876..7ed1b05 100644 --- a/.gitignore +++ b/.gitignore @@ -4,7 +4,10 @@ __pycache__/ *.py[cod] *$py.class -# *.so +*.so +*.so.* +!src/ufactory_lerobot/devices/umi/xvlib/lib*.so +!src/ufactory_lerobot/devices/umi/xvlib/lib*.so.* *.egg *.egg-info/ dist/ diff --git a/src/ufactory_lerobot/cameras/umi_camera/camera_umi.py b/src/ufactory_lerobot/cameras/umi_camera/camera_umi.py index c129f03..e500bb2 100644 --- a/src/ufactory_lerobot/cameras/umi_camera/camera_umi.py +++ b/src/ufactory_lerobot/cameras/umi_camera/camera_umi.py @@ -75,20 +75,16 @@ class UmiCamera(Camera): time.sleep(0.1) def read(self, color_mode = None): - ret, img_data = self.xvlib.xv_get_color_image_rgb_data() - if ret <= 0: + ret, img_data = self.xvlib.xv_get_color_camera_data() + if ret <= 0 or img_data is None: return None requested_color_mode = self.color_mode if color_mode is None else color_mode if requested_color_mode not in (ColorMode.RGB, ColorMode.BGR): raise ValueError( f"Invalid color mode '{requested_color_mode}'. Expected {ColorMode.RGB} or {ColorMode.BGR}." ) - if requested_color_mode == ColorMode.RGB: - frame = img_data.frame(rgb=True) - else: - frame = img_data.frame(rgb=False) - if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE, cv2.ROTATE_180]: - frame = cv2.rotate(frame, self.rotation) + rgb = requested_color_mode == ColorMode.RGB + frame = img_data.frame(rgb=rgb, rotate=self.rotation) return frame def async_read(self, timeout_ms: float = 200): diff --git a/src/ufactory_lerobot/devices/umi/vive_tracker/transformations.py b/src/ufactory_lerobot/devices/umi/vive_tracker/transformations.py index 0783914..eb49755 100644 --- a/src/ufactory_lerobot/devices/umi/vive_tracker/transformations.py +++ b/src/ufactory_lerobot/devices/umi/vive_tracker/transformations.py @@ -28,8 +28,8 @@ class Transformations: @staticmethod def rotation_matrix_to_quaternion(R): """ - 将3x3变换矩阵转换四元数 - 注: 四元素顺序为xyzw + 从 3x3 变换矩阵 或 4x4 齐次变换矩阵提取旋转部分并转换为四元数 + 注: 四元素顺序为 xyzw """ # 提取旋转矩阵部分 rot_matrix = R[:3, :3] @@ -105,13 +105,9 @@ class Transformations: return roll, pitch, yaw @staticmethod - def rxryrz_to_rotation_matrix(axis_angle): - """ - 将轴角向量 (rx, ry, rz) 转换为 3x3 旋转矩阵。 - 输入: np.array([rx, ry, rz]) - - 方向: 旋转轴 - - 模长: 旋转角度 (弧度) - """ + def rxryrz_to_rotation_matrix(rx, ry, rz): + """轴角rxryrz到旋转矩阵的转换""" + axis_angle = [rx, ry, rz] theta = np.linalg.norm(axis_angle) # 如果角度接近0，返回单位矩阵 @@ -140,7 +136,7 @@ class Transformations: @staticmethod def rotation_matrix_to_rxryrz(R): """ - 旋转矩阵到轴角的转换 (rx, ry, rz = aixs * angle) + 旋转矩阵到轴角的转换 (rx, ry, rz = axis * angle) 返回: rx, ry, rz """ R = np.asarray(R) @@ -211,7 +207,7 @@ class Transformations: def xyzrxryrz_to_rotation_matrix(cls, x, y, z, rx, ry, rz): """构造4x4齐次变换矩阵""" T = np.eye(4) - T[:3, :3] = cls.rxryrz_to_rotation_matrix([rx, ry, rz]) + T[:3, :3] = cls.rxryrz_to_rotation_matrix(rx, ry, rz) T[:3, 3] = [x, y, z] return T @@ -249,8 +245,8 @@ class Transformations: # 机械臂目标位置对应的变换矩阵 # 机械臂目标 = 机械臂初始位置 + (当前手姿 - 初始手姿) delta_matrix = np.dot(np.linalg.inv(begin_tracker_robot_matrix), end_tracker_robot_matrix) - robot_martix = np.dot(robot_base_matrix, delta_matrix) + robot_matrix = np.dot(robot_base_matrix, delta_matrix) if is_axis_angle: - return cls.rotation_matrix_to_xyzrxryrz(robot_martix) + return cls.rotation_matrix_to_xyzrxryrz(robot_matrix) else: - return cls.rotation_matrix_to_xyzrpy(robot_martix) + return cls.rotation_matrix_to_xyzrpy(robot_matrix) diff --git a/src/ufactory_lerobot/devices/umi/vive_tracker/vive_tracker.py b/src/ufactory_lerobot/devices/umi/vive_tracker/vive_tracker.py index 0454e0f..0193551 100644 --- a/src/ufactory_lerobot/devices/umi/vive_tracker/vive_tracker.py +++ b/src/ufactory_lerobot/devices/umi/vive_tracker/vive_tracker.py @@ -2,26 +2,19 @@ import sys import ctypes import logging import threading -import pysurvive import numpy as np from .transformations import Transformations -# 配置日志 -logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(name)s - %(levelname)s - %(message)s') logger = logging.getLogger('uf.vive_tracker') class Vector(ctypes.Structure): def __getitem__(self, index): - # 获取字段名列表 field_name = self._fields_[index][0] - # 使用 getattr 获取对应属性的值 return getattr(self, field_name) def __setitem__(self, index, value): - # 获取字段名列表 field_name = self._fields_[index][0] - # 使用 setattr 设置对应属性的值 setattr(self, field_name, value) def __str__(self): @@ -48,17 +41,18 @@ class Vector4D(Vector): ] -class PoseData(ctypes.Structure): +class TrackerPose(ctypes.Structure): _fields_ = [ ("position", Vector3D), - # ("orientation", Vector3D), ("quaternion", Vector4D), ("hostTimestamp", ctypes.c_double), - # ("edgeTimestampUs", ctypes.c_longlong), - # ("confidence", ctypes.c_double) ] +def _to_str(v): + return v.decode("utf-8") if isinstance(v, bytes) else str(v) + + class SingletonMeta(type): _instances = {} @@ -69,118 +63,118 @@ class SingletonMeta(type): class ViveTracker(metaclass=SingletonMeta): - # _instance = None - # _initialized = False def __init__(self, config_path=None, lh_config=None, args=None): - # if self._initialized: - # return - # self._initialized = True - self.config_path = config_path - self.lh_config = lh_config - self.args = args if args else [] + if hasattr(self, '_initialized'): + return + self._initialized = True + self._config_path = config_path + self._lh_config = lh_config + self._args = args if args else [] self.running = False - self.context = None - self.collector_thread = None - self.data_lock = threading.Lock() - self.latest_poses = {} - self.latest_raw_poses = {} - self.init() - - # def __new__(cls, *args, **kwargs): - # if cls._instance is None: - # cls._instance = super().__new__(cls) - # return cls._instance + self._context = None + self._collector_thread = None + self._data_lock = threading.Lock() + self._latest_poses = {} + self._latest_raw_poses = {} + if not self._init(): + raise RuntimeError("Failed to initialize Vive Tracker: pysurvive context creation failed") def __del__(self): - logger.info("正在停止Vive Tracker位姿追踪...") + logger.info("Stopping Vive Tracker pose tracking...") self.running = False # 等待线程结束 - if self.collector_thread: - self.collector_thread.join(timeout=2.0) + if self._collector_thread: + self._collector_thread.join(timeout=2.0) # 清理资源 - self.context = None - logger.info("Vive Tracker已断开连接") + self._context = None + logger.info("Vive Tracker disconnected") - @staticmethod - def to_str(v): - return v.decode("utf-8") if isinstance(v, bytes) else str(v) + # def list_devices(self): + # import pysurvive + # for obj in self._context.Objects(): + # name = _to_str(obj.Name()) + # serial_number = None + # if hasattr(pysurvive, "simple_serial_number"): + # serial_number = _to_str(pysurvive.simple_serial_number(obj.ptr)) + # print("object:", name, "serial:", serial_number) + + def get_tracked_device_names(self): + return [key for key in self._latest_poses.keys() if not key.startswith('WM')] - def list_devices(self): - # import pysurvive - # for obj in self.context.Objects(): - # name = self.to_str(obj.Name()) - # serial_number = None - # if hasattr(pysurvive, "simple_serial_number"): - # serial_number = self.to_str(pysurvive.simple_serial_number(obj.ptr)) - # print("object:", name, "serial:", serial_number) - return [key for key in self.latest_poses.keys() if not key.startswith('WM')] - - def init(self): + def _init(self): + import pysurvive # 延迟导入，避免未安装时影响 umi 包的整体导入 + self._pysurvive = pysurvive # 构建pysurvive参数 survive_args = sys.argv[:1] # 保留程序名 # 添加配置文件参数 - if self.config_path: - survive_args.extend(['--config', self.config_path]) + if self._config_path: + survive_args.extend(['--config', self._config_path]) # 添加灯塔配置参数 - if self.lh_config: - survive_args.extend(['--lh', self.lh_config]) + if self._lh_config: + survive_args.extend(['--lh', self._lh_config]) # 添加其他参数 - survive_args.extend(self.args) + survive_args.extend(self._args) try: - logger.info("正在初始化pysurvive...") - self.context = pysurvive.SimpleContext(survive_args) - if not self.context: - logger.error("错误: 无法初始化pysurvive上下文") + logger.info("Initializing pysurvive...") + self._context = pysurvive.SimpleContext(survive_args) + if not self._context: + logger.error("Error: failed to initialize pysurvive context") return False - logger.info("pysurvive初始化成功") + logger.info("pysurvive initialized successfully") # 标记为运行状态 self.running = True # 创建并启动位姿收集线程 - self.collector_thread = threading.Thread(target=self._pose_collector) - self.collector_thread.daemon = True - self.collector_thread.start() + self._collector_thread = threading.Thread(target=self._pose_collector) + self._collector_thread.daemon = True + self._collector_thread.start() except Exception as e: - logger.error(f"连接Vive Tracker时发生错误: {e}") + logger.error(f"Error connecting to Vive Tracker: {e}") self.running = False return False def _pose_collector(self): - initial_rotation = Transformations.xyzrpy_to_rotation_matrix(0, 0, 0, -30 / 180.0 * np.pi, 0, 0) - - # alignment_rotation = Transformations.xyzrpy_to_rotation_matrix(0, 0, 0, -np.pi / 2, -np.pi / 2, 0) - alignment_rotation = Transformations.xyzrpy_to_rotation_matrix(0, 0, 0, -np.pi / 2, np.pi, np.pi) - - rotate_matrix = np.dot(initial_rotation, alignment_rotation) - # 应用平移变换 - 将采集到的pose数据变换到夹爪中心 - # transform_matrix = Transformations.xyzrpy_to_rotation_matrix(0.172, 0, -0.076, 0, 0, 0) - # transform_matrix = Transformations.xyzrpy_to_rotation_matrix(0, 0, 0, 0, 0, 0) - - # tracker_to_robot_matrix = Transformations.xyzrpy_to_rotation_matrix(0, 0, 0, np.pi, 0, np.pi) - tracker_to_robot_matrix = Transformations.xyzrpy_to_rotation_matrix(0, 0, 0, 0, 0, -np.pi / 2) - - robot_base_matrix = Transformations.xyzrpy_to_rotation_matrix(*[0, 0, 0, np.pi, -np.pi / 2, 0]) - begin_tracker_robot_matrix = None - + if not hasattr(self, '_pysurvive'): + return cnt = 0 - + pysurvive = self._pysurvive + + # —————————————————————————————— 位姿变换矩阵常量 —————————————————————————————— + # initial_rotation: 初始旋转补偿 (roll=-30°) + _INITIAL_ROTATION = Transformations.xyzrpy_to_rotation_matrix(0, 0, 0, -30 / 180.0 * np.pi, 0, 0) + # alignment_rotation: 坐标系对齐 (pitch=-90°, roll=180°, yaw=180°) + # _ALIGNMENT_ROTATION = Transformations.xyzrpy_to_rotation_matrix(0, 0, 0, -np.pi / 2, -np.pi / 2, 0) + _ALIGNMENT_ROTATION = Transformations.xyzrpy_to_rotation_matrix(0, 0, 0, -np.pi / 2, np.pi, np.pi) + # rotate_matrix: 合并后的 tracker 坐标旋转矩阵 + _ROTATE_MATRIX = np.dot(_INITIAL_ROTATION, _ALIGNMENT_ROTATION) + # 应用平移变换 - 将采集到的pose数据变换到夹爪中心 + # _TRANSFORM_MATRIX = Transformations.xyzrpy_to_rotation_matrix(0.172, 0, -0.076, 0, 0, 0) + # _TRANSFORM_MATRIX = Transformations.xyzrpy_to_rotation_matrix(0, 0, 0, 0, 0, 0) + # tracker_to_robot: tracker 到机械臂基座的固定变换 + _TRACKER_TO_ROBOT_MATRIX = Transformations.xyzrpy_to_rotation_matrix(0, 0, 0, 0, 0, -np.pi / 2) + # robot_base: 机械臂基座位姿 + _ROBOT_BASE_MATRIX = Transformations.xyzrpy_to_rotation_matrix(*[0, 0, 0, np.pi, -np.pi / 2, 0]) + # 初始化阶段跳过帧数（让传感器数据稳定） + _SKIP_FRAMES = 100 + # ——————————————————————————————————————————————————————————————————————————— + # 持续获取最新位姿 - while self.running and self.context.Running(): - updated = self.context.NextUpdated() + while self.running and self._context.Running(): + updated = self._context.NextUpdated() if not updated: continue - if cnt < 100: + if cnt < _SKIP_FRAMES: cnt += 1 continue - # 获取设备名称 - device_name = str(updated.Name(), 'utf-8') + # 获取设备名称，使用 replace 容错处理非 UTF-8 字节 + device_name = str(updated.Name(), 'utf-8', errors='replace') serial_number = None if hasattr(pysurvive, "simple_serial_number"): - serial_number = self.to_str(pysurvive.simple_serial_number(updated.ptr)) + serial_number = _to_str(pysurvive.simple_serial_number(updated.ptr)) # 获取位姿数据 pose_obj = updated.Pose() pose_data = pose_obj[0] # 位姿数据 @@ -188,47 +182,38 @@ class ViveTracker(metaclass=SingletonMeta): position = [pose_data.Pos[0], pose_data.Pos[1], pose_data.Pos[2]] quaternion = [pose_data.Rot[1], pose_data.Rot[2], pose_data.Rot[3], pose_data.Rot[0]] origin_mat = Transformations.xyzq_to_rotation_matrix(*position, quaternion) - # tracker_matrix = np.dot(origin_mat, rotate_matrix) - tracker_matrix = np.matmul(origin_mat, rotate_matrix) - # tracker_matrix = np.matmul(np.matmul(origin_mat, rotate_matrix), transform_matrix) + # tracker_matrix = np.dot(origin_mat, _ROTATE_MATRIX) + tracker_matrix = np.matmul(origin_mat, _ROTATE_MATRIX) + # tracker_matrix = np.matmul(np.matmul(origin_mat, _ROTATE_MATRIX), _TRANSFORM_MATRIX) x, y, z, q = Transformations.rotation_matrix_to_xyzq(tracker_matrix) - pose_data = PoseData(position=Vector3D(x, y, z), quaternion=Vector4D(*q), hostTimestamp=timestamp) - pose_raw_data = PoseData(position=Vector3D(*position), quaternion=Vector4D(*quaternion), hostTimestamp=timestamp) - with self.data_lock: - self.latest_poses[device_name] = pose_data - self.latest_raw_poses[device_name] = pose_raw_data + tracker_pose = TrackerPose(position=Vector3D(x, y, z), quaternion=Vector4D(*q), hostTimestamp=timestamp) + tracker_raw_pose = TrackerPose(position=Vector3D(*position), quaternion=Vector4D(*quaternion), hostTimestamp=timestamp) + with self._data_lock: + self._latest_poses[device_name] = tracker_pose + self._latest_raw_poses[device_name] = tracker_raw_pose if serial_number: - self.latest_poses[serial_number] = pose_data - self.latest_raw_poses[serial_number] = pose_raw_data - - # tracker_robot_matrix = np.dot(tracker_matrix, tracker_to_robot_matrix) - # if begin_tracker_robot_matrix is None: - # begin_tracker_robot_matrix = tracker_robot_matrix - # pose = Transformations.tracker_robot_matrix_to_robot_pose(begin_tracker_robot_matrix, tracker_robot_matrix, robot_base_matrix, is_axis_angle=True) - - # pose_data = PoseData(position=Vector3D(*pose[:3]), orientation=Vector3D(*pose[3:]), quaternion=Vector4D(*quaternion), hostTimestamp=timestamp) - # with self.data_lock: - # self.latest_poses[device_name] = pose_data + self._latest_poses[serial_number] = tracker_pose + self._latest_raw_poses[serial_number] = tracker_raw_pose def get_pose(self, device_name=None): if device_name: - with self.data_lock: - if device_name in self.latest_poses: - return self.latest_poses[device_name] + with self._data_lock: + if device_name in self._latest_poses: + return self._latest_poses[device_name] else: return None else: - with self.data_lock: - return self.latest_poses.copy() + with self._data_lock: + return self._latest_poses.copy() def get_raw_pose(self, device_name=None): if device_name: - with self.data_lock: - if device_name in self.latest_raw_poses: - return self.latest_raw_poses[device_name] + with self._data_lock: + if device_name in self._latest_raw_poses: + return self._latest_raw_poses[device_name] else: return None else: - with self.data_lock: - return self.latest_raw_poses.copy() + with self._data_lock: + return self._latest_raw_poses.copy() diff --git a/src/ufactory_lerobot/devices/umi/xvlib/libopencv_core.so.4.2 b/src/ufactory_lerobot/devices/umi/xvlib/libopencv_core.so.4.2 new file mode 100644 index 0000000..45eabca Binary files /dev/null and b/src/ufactory_lerobot/devices/umi/xvlib/libopencv_core.so.4.2 differ diff --git a/src/ufactory_lerobot/devices/umi/xvlib/libopencv_imgproc.so.4.2 b/src/ufactory_lerobot/devices/umi/xvlib/libopencv_imgproc.so.4.2 new file mode 100644 index 0000000..da3d3bd Binary files /dev/null and b/src/ufactory_lerobot/devices/umi/xvlib/libopencv_imgproc.so.4.2 differ diff --git a/src/ufactory_lerobot/devices/umi/xvlib/libxvlib.so b/src/ufactory_lerobot/devices/umi/xvlib/libxvlib.so new file mode 100644 index 0000000..e097308 Binary files /dev/null and b/src/ufactory_lerobot/devices/umi/xvlib/libxvlib.so differ diff --git a/src/ufactory_lerobot/devices/umi/xvlib/xvlib.py b/src/ufactory_lerobot/devices/umi/xvlib/xvlib.py index 12c3291..d9840bc 100644 --- a/src/ufactory_lerobot/devices/umi/xvlib/xvlib.py +++ b/src/ufactory_lerobot/devices/umi/xvlib/xvlib.py @@ -1,23 +1,49 @@ import os -import ctypes -import cv2 import time +import threading +import ctypes +import ctypes.util +import cv2 import logging import numpy as np -# 配置日志 -logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(name)s - %(levelname)s - %(message)s') logger = logging.getLogger('uf.xvlib') +_rotate_codes = { + 90: cv2.ROTATE_90_CLOCKWISE, + -90: cv2.ROTATE_90_COUNTERCLOCKWISE, + 180: cv2.ROTATE_180, +} + +def _apply_rotate(frame, rotate): + """如果 rotate 不为 None, 对 frame 应用旋转""" + if rotate is None or frame is None or (isinstance(rotate, int) and rotate == 0): + return frame + code = _rotate_codes.get(rotate) if isinstance(rotate, int) else rotate + if code is None: + logger.warning(f"Unknown rotate value: {rotate}, skipping") + return frame + return cv2.rotate(frame, code) + +# ============== 数据缓冲区常量 (与 C++ xv_device.h 保持一致) ============== +_MAX_COLOR_BUFFER_SIZE = 1280 * 1280 * 3 # MAX_COLOR_BUFFER_SIZE +_MAX_DEPTH_BUFFER_SIZE = 1280 * 1280 * 3 # MAX_DEPTH_BUFFER_SIZE +_MAX_GRAY_BUFFER_SIZE = 640 * 480 # MAX_GRAY_BUFFER_SIZE +# ===================================================================== + class DeviceStruct(ctypes.Structure): _fields_ = [ - ("uuid", ctypes.c_char * 100) + ("serial_number", ctypes.c_char * 100) ] - + @property - def serial_number(self): - return self.uuid.decode('utf-8') + def serial(self): + """返回解码后的序列号字符串""" + return self.serial_number.decode('utf-8') + + def __repr__(self): + return f"DeviceStruct(serial_number='{self.serial}')" class Vector(ctypes.Structure): @@ -66,6 +92,7 @@ class Vector4D(Vector): class ClampData(ctypes.Structure): + _pack_ = 1 # 匹配 SDK #pragma pack(1) _fields_ = [ ("timestamp", ctypes.c_double), ("data", ctypes.c_double) @@ -77,13 +104,13 @@ class ColorImageData(ctypes.Structure): ("codec", ctypes.c_int), ("width", ctypes.c_int), ("height", ctypes.c_int), - ("data", ctypes.c_uint8 * int(1280*1280*3)), + ("data", ctypes.c_uint8 * _MAX_COLOR_BUFFER_SIZE), ("dataSize", ctypes.c_uint), ("hostTimestamp", ctypes.c_double), ("edgeTimestampUs", ctypes.c_longlong) ] - def frame(self, rgb=False): - np_array = np.frombuffer(bytes(self.data[:self.dataSize]), dtype=np.uint8) + def frame(self, rgb=False, rotate=None): + np_array = np.frombuffer(self.data, dtype=np.uint8, count=self.dataSize) if self.codec == 0: # YUYV 格式, 重塑为 (h, w, 2)，因为每两个字节包含 Y 和 UV 信息 yuv_mat = np_array.reshape((self.height, self.width, 2)) if rgb: @@ -108,7 +135,7 @@ class ColorImageData(ctypes.Structure): frame = cv2.imdecode(np_array, cv2.IMREAD_COLOR) else: frame = np_array - return frame + return _apply_rotate(frame, rotate) class DepthImageData(ctypes.Structure): @@ -117,13 +144,13 @@ class DepthImageData(ctypes.Structure): ("width", ctypes.c_int), ("height", ctypes.c_int), ("confidence", ctypes.c_double), - ("data", ctypes.c_uint8 * int(1280*1280*3)), + ("data", ctypes.c_uint8 * _MAX_DEPTH_BUFFER_SIZE), ("dataSize", ctypes.c_uint), ("hostTimestamp", ctypes.c_double), ("edgeTimestampUs", ctypes.c_longlong) ] - def frame(self): - np_array = np.frombuffer(bytes(self.data[:self.dataSize]), dtype=np.uint8) + def frame(self, rotate=None): + np_array = np.frombuffer(self.data, dtype=np.uint8, count=self.dataSize) if self.type == 0: # Depth_16, 数据大小应为 w * h * 2 # 1. 转换为 uint16 类型 depth_uint16 = np_array.view(dtype=np.uint16).reshape((self.height, self.width)) @@ -155,7 +182,8 @@ class DepthImageData(ctypes.Structure): # 16位转8位显示 ir_img = cv2.normalize(ir_img, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8) else: - raise ValueError("IR 数据长度不匹配") + logger.warning(f"IR data length mismatch: got {len(np_array)}, expected {self.width * self.height} or {self.width * self.height * 2}") + return None # 转伪彩色 ir_color = cv2.applyColorMap(ir_img, cv2.COLORMAP_JET) frame = ir_color @@ -169,36 +197,27 @@ class DepthImageData(ctypes.Structure): frame = None else: frame = np_array - return frame - - -class RgbImageData(ctypes.Structure): - _fields_ = [ - ("width", ctypes.c_int), - ("height", ctypes.c_int), - ("data", ctypes.c_uint8 * (1280*1280*3)), - ("dataSize", ctypes.c_uint), - ("hostTimestamp", ctypes.c_double), - ("edgeTimestampUs", ctypes.c_longlong) - ] - def frame(self, rgb=False): - rgb_frame = np.frombuffer(bytes(self.data[:self.dataSize]), dtype=np.uint8).reshape((self.height, self.width, 3)) - if rgb: - return rgb_frame.copy() - # RGB => BGR - return cv2.cvtColor(rgb_frame, cv2.COLOR_RGB2BGR) # 转换颜色空间 + return _apply_rotate(frame, rotate) class GrayScaleImage(ctypes.Structure): _fields_ = [ ("width", ctypes.c_int), ("height", ctypes.c_int), - ("data", ctypes.c_uint8 * (640*480)) + ("data", ctypes.c_uint8 * _MAX_GRAY_BUFFER_SIZE) ] - def frame(self): - buf_size = 640 * 480 - count = min(self.width * self.height, buf_size) - return np.array(self.data[:count], dtype=np.uint8).reshape((self.height, self.width, 1)) + def frame(self, rotate=None): + """返回 BGR 三通道图像，兼容 cv2.imshow 显示""" + max_size = _MAX_GRAY_BUFFER_SIZE + needed = self.width * self.height + if needed > max_size: + count = max_size + h, w = 480, 640 + else: + count = needed + h, w = self.height, self.width + gray = np.frombuffer(self.data, dtype=np.uint8, count=count).reshape((h, w)) + return _apply_rotate(cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR), rotate) class FisheyeImagesData(ctypes.Structure): @@ -209,19 +228,23 @@ class FisheyeImagesData(ctypes.Structure): ("id", ctypes.c_longlong) ] - def frame(self, inx=-1): - inx_vaild = inx >= 0 and inx < 4 - if inx_vaild: - return self.images[inx].frame() + # 四合一显示时的缩放尺寸 (宽度, 高度) + _DISPLAY_WIDTH = 480 + _DISPLAY_HEIGHT = 360 + + def frame(self, index=-1, rotate=None): + index_valid = index >= 0 and index < 4 + if index_valid: + return self.images[index].frame(rotate=rotate) else: - frame0 = cv2.resize(self.images[0].frame(), (480, 360)) - frame1 = cv2.resize(self.images[1].frame(), (480, 360)) - frame2 = cv2.resize(self.images[2].frame(), (480, 360)) - frame3 = cv2.resize(self.images[3].frame(), (480, 360)) + frame0 = cv2.resize(self.images[0].frame(), (self._DISPLAY_WIDTH, self._DISPLAY_HEIGHT)) + frame1 = cv2.resize(self.images[1].frame(), (self._DISPLAY_WIDTH, self._DISPLAY_HEIGHT)) + frame2 = cv2.resize(self.images[2].frame(), (self._DISPLAY_WIDTH, self._DISPLAY_HEIGHT)) + frame3 = cv2.resize(self.images[3].frame(), (self._DISPLAY_WIDTH, self._DISPLAY_HEIGHT)) up_frame = cv2.hconcat([frame0, frame1]) down_frame = cv2.hconcat([frame2, frame3]) - return cv2.vconcat([up_frame, down_frame]) + return _apply_rotate(cv2.vconcat([up_frame, down_frame]), rotate) class EyetrackingImageData(ctypes.Structure): @@ -231,11 +254,169 @@ class EyetrackingImageData(ctypes.Structure): ("images", GrayScaleImage * 4), ] + # 四合一显示时的缩放尺寸 (宽度, 高度) + _DISPLAY_WIDTH = 480 + _DISPLAY_HEIGHT = 360 + + def frame(self, index=-1, rotate=None): + index_valid = index >= 0 and index < 4 + if index_valid: + return self.images[index].frame(rotate=rotate) + else: + frame0 = cv2.resize(self.images[0].frame(), (self._DISPLAY_WIDTH, self._DISPLAY_HEIGHT)) + frame1 = cv2.resize(self.images[1].frame(), (self._DISPLAY_WIDTH, self._DISPLAY_HEIGHT)) + frame2 = cv2.resize(self.images[2].frame(), (self._DISPLAY_WIDTH, self._DISPLAY_HEIGHT)) + frame3 = cv2.resize(self.images[3].frame(), (self._DISPLAY_WIDTH, self._DISPLAY_HEIGHT)) + + up_frame = cv2.hconcat([frame0, frame1]) + down_frame = cv2.hconcat([frame2, frame3]) + return _apply_rotate(cv2.vconcat([up_frame, down_frame]), rotate) + +# ============== 零拷贝 Ref 结构体 (直接引用 SDK 底层 buffer) ============== + +class ColorImageDataRef(ctypes.Structure): + _fields_ = [ + ("data_ptr", ctypes.c_void_p), + ("data_size", ctypes.c_uint), + ("codec", ctypes.c_int), + ("width", ctypes.c_int), + ("height", ctypes.c_int), + ("hostTimestamp", ctypes.c_double), + ("edgeTimestampUs", ctypes.c_longlong), + ] + def frame(self, rgb=False, rotate=None): + """零拷贝帧解码 — 从 SDK buffer 指针直接构造 numpy 数组""" + if self.data_size == 0 or not self.data_ptr: + return None + buf = (ctypes.c_uint8 * self.data_size).from_address(self.data_ptr) + np_array = np.frombuffer(buf, dtype=np.uint8, count=self.data_size) + if self.codec == 0: # YUYV + yuv_mat = np_array.reshape((self.height, self.width, 2)) + code = cv2.COLOR_YUV2RGB_YUYV if rgb else cv2.COLOR_YUV2BGR_YUYV + frame = cv2.cvtColor(yuv_mat, code) + elif self.codec == 1: # I420 + yuv_mat = np_array.reshape((int(self.height * 1.5), self.width)) + code = cv2.COLOR_YUV2RGB_I420 if rgb else cv2.COLOR_YUV2BGR_I420 + frame = cv2.cvtColor(yuv_mat, code) + elif self.codec in (2, 4): # JPEG / BITSTREAM + frame = cv2.imdecode(np_array, cv2.IMREAD_COLOR) + elif self.codec == 3: # NV12 + yuv_mat = np_array.reshape((int(self.height * 1.5), self.width)) + code = cv2.COLOR_YUV2RGB_NV12 if rgb else cv2.COLOR_YUV2BGR_NV12 + frame = cv2.cvtColor(yuv_mat, code) + else: + frame = np_array + return _apply_rotate(frame, rotate) + + +class DepthImageDataRef(ctypes.Structure): + _fields_ = [ + ("data_ptr", ctypes.c_void_p), + ("data_size", ctypes.c_uint), + ("type", ctypes.c_int), + ("width", ctypes.c_int), + ("height", ctypes.c_int), + ("confidence", ctypes.c_double), + ("hostTimestamp", ctypes.c_double), + ("edgeTimestampUs", ctypes.c_longlong), + ] + def frame(self, rotate=None): + """零拷贝深度帧解码""" + if self.data_size == 0 or not self.data_ptr: + return None + buf = (ctypes.c_uint8 * self.data_size).from_address(self.data_ptr) + np_array = np.frombuffer(buf, dtype=np.uint8, count=self.data_size) + if self.type == 0: # Depth_16 + depth_uint16 = np_array.view(dtype=np.uint16).reshape((self.height, self.width)) + depth_norm = cv2.normalize(depth_uint16, None, 0, 255, cv2.NORM_MINMAX) + frame = cv2.applyColorMap(np.uint8(depth_norm), cv2.COLORMAP_JET) + elif self.type == 1: # Depth_32 + depth_float = np_array.view(dtype=np.float32).reshape((self.height, self.width)) + depth_norm = cv2.normalize(depth_float, None, 0, 255, cv2.NORM_MINMAX, dtype=cv2.CV_8U) + frame = cv2.applyColorMap(depth_norm, cv2.COLORMAP_JET) + elif self.type == 2: # IR + if len(np_array) == self.width * self.height: + ir_img = np_array.reshape((self.height, self.width)) + elif len(np_array) == self.width * self.height * 2: + ir_img = np_array.view(dtype=np.uint16).reshape((self.height, self.width)) + ir_img = cv2.normalize(ir_img, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8) + else: + logger.warning(f"IR data length mismatch: got {len(np_array)}") + return None + frame = cv2.applyColorMap(ir_img, cv2.COLORMAP_JET) + elif self.type == 3: # Cloud + frame = np_array.view(dtype=np.float32).reshape((-1, 3)) + elif self.type in (4, 5, 6): + return None + else: + frame = np_array + return _apply_rotate(frame, rotate) + + +class GrayScaleImageRef(ctypes.Structure): + _fields_ = [ + ("data_ptr", ctypes.c_void_p), + ("width", ctypes.c_int), + ("height", ctypes.c_int), + ] + def frame(self, rotate=None): + if not self.data_ptr or self.width <= 0: + return None + buf = (ctypes.c_uint8 * (self.width * self.height)).from_address(self.data_ptr) + gray = np.frombuffer(buf, dtype=np.uint8, count=self.width * self.height).reshape((self.height, self.width)) + return _apply_rotate(cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR), rotate) + + +class FisheyeImagesDataRef(ctypes.Structure): + _fields_ = [ + ("images", GrayScaleImageRef * 4), + ("image_count", ctypes.c_int), + ("hostTimestamp", ctypes.c_double), + ("edgeTimestampUs", ctypes.c_longlong), + ("id", ctypes.c_longlong), + ] + _DISPLAY_WIDTH = 480 + _DISPLAY_HEIGHT = 360 + + def frame(self, index=-1, rotate=None): + if index >= 0 and index < self.image_count: + return self.images[index].frame(rotate=rotate) + return self._four_in_one(self.image_count, rotate) + + def _four_in_one(self, count, rotate=None): + frames = [] + for i in range(count): + f = self.images[i].frame() + if f is not None: + frames.append(cv2.resize(f, (self._DISPLAY_WIDTH, self._DISPLAY_HEIGHT))) + if not frames: + return None + while len(frames) < 4: + frames.append(frames[-1]) + up = cv2.hconcat([frames[0], frames[1]]) + down = cv2.hconcat([frames[2], frames[3]]) + return _apply_rotate(cv2.vconcat([up, down]), rotate) + +class EyetrackingImageDataRef(ctypes.Structure): + _fields_ = [ + ("images", GrayScaleImageRef * 4), + ("image_count", ctypes.c_int), + ("hostTimestamp", ctypes.c_double), + ("edgeTimestampUs", ctypes.c_longlong), + ] + + def frame(self, index=-1, rotate=None): + if index >= 0 and index < self.image_count: + return self.images[index].frame(rotate=rotate) + return None + +# ===================================================================== + class PoseData(ctypes.Structure): _fields_ = [ ("position", Vector3D), - ("orientation", Vector3D), + # ("orientation", Vector3D), ("quaternion", Vector4D), ("hostTimestamp", ctypes.c_double), ("edgeTimestampUs", ctypes.c_longlong), @@ -244,6 +425,7 @@ class PoseData(ctypes.Structure): class ImuData(ctypes.Structure): + _pack_ = 1 # 匹配 SDK #pragma pack(1) _fields_ = [ ("gyro", Vector3D), ("accel", Vector3D), @@ -256,6 +438,7 @@ class ImuData(ctypes.Structure): class EventData(ctypes.Structure): + _pack_ = 1 # 匹配 SDK #pragma pack(1) _fields_ = [ ("hostTimestamp", ctypes.c_double), ("edgeTimestampUs", ctypes.c_longlong), @@ -266,346 +449,635 @@ class EventData(ctypes.Structure): class XVLib: _xvlib = None + _load_lock = threading.Lock() def __init__(self, serial_number, init_slam=False, init_clamp_stream=False, init_color_camera=False, init_fisheye_cameras=False): self.instance_id = -1 self._clamp_data = ClampData() + self._event_data = EventData() self._color_image_data = ColorImageData() - self._color_image_rgb_data = RgbImageData() self._depth_image_data = DepthImageData() self._fisheye_images_data = FisheyeImagesData() + self._eyetracking_image_data = EyetrackingImageData() self._slam_data = PoseData() self._external_stream_data = PoseData() self._spheretrack_stream_data = PoseData() - self.xv_get_devices() - time.sleep(1) + # 零拷贝 Ref 数据 holder + self._color_image_data_ref = ColorImageDataRef() + self._depth_image_data_ref = DepthImageDataRef() + self._fisheye_images_data_ref = FisheyeImagesDataRef() + self._eyetracking_image_data_ref = EyetrackingImageDataRef() - serial_number = ctypes.c_char_p(serial_number.encode('utf-8')) - self.instance_id = self.xv_init(serial_number, init_slam, init_clamp_stream, init_color_camera, init_fisheye_cameras) + self.__load_library() + + # 等待设备注册完成再初始化（自己有重试循环，不用 xv_get_devices 的 double_query） + sn_bytes = serial_number.encode('utf-8') + for i in range(3): + _, devices = self.xv_get_devices(timeout=5.0 - i, double_query=False) + sn_list = [d.serial for d in devices] + if serial_number in sn_list: + break + time.sleep(1) + else: + raise Exception(f'Device {serial_number} not detected') + + c_serial_number = ctypes.c_char_p(sn_bytes) + self.instance_id = self.xv_init(c_serial_number, init_slam, init_clamp_stream, init_color_camera, init_fisheye_cameras) if self.instance_id > 0: logger.info('Device initialized successfully.') else: - raise Exception('Device initialized failure.') + raise Exception(f'Device initialized failure (ret={self.instance_id}).') def __del__(self): self.xv_uninit() + _DEB_URL = ("https://raw.githubusercontent.com/xArm-Developer/" + "ufactory_resources/main/fastumi/sdk/XVSDK_focal_amd64.deb") + + @classmethod + def _check_xvsdk(cls): + """检查系统是否安装了 XVSDK, 没有则抛异常并提示安装命令.""" + if ctypes.util.find_library("xvsdk") and os.path.exists('/usr/lib/libxvsdk.so'): + return + raise RuntimeError( + "XVSDK not found. Run:\n" + f" curl -sL {cls._DEB_URL} -o /tmp/xvsdk.deb && sudo dpkg -i /tmp/xvsdk.deb\n" + " sudo apt install -y --fix-broken") + + @classmethod + def _find_system_lib(cls, fname): + """在系统标准路径中搜索精确版本 .so.""" + for base in ["/usr/lib", "/usr/lib/x86_64-linux-gnu", "/usr/local/lib"]: + path = os.path.join(base, fname) + if os.path.exists(path): + return path + # find_library 兜底，但必须版本匹配 + lib_name = fname.split(".so")[0] # "libopencv_core" + if lib_name.startswith("lib"): + lib_name = lib_name[3:] # "opencv_core" + sys_lib = ctypes.util.find_library(lib_name) + if sys_lib and fname in sys_lib: + return sys_lib + return None + + @classmethod + def _load_opencv_lib(cls, lib_dir, fname): + """加载 opencv .so: 系统(精确版本) → 本地目录.""" + sys_path = cls._find_system_lib(fname) + if sys_path: + try: + ctypes.CDLL(sys_path, mode=ctypes.RTLD_GLOBAL) + logger.info(f"Loaded {fname} from system ({sys_path})") + return + except OSError: + logger.debug(f"System {fname} load failed, try local") + + local_path = os.path.join(lib_dir, fname) + if os.path.exists(local_path): + ctypes.CDLL(local_path, mode=ctypes.RTLD_GLOBAL) + logger.info(f"Loaded {fname} from local") + return + + logger.warning(f"{fname} not found (system or local)") + @classmethod def __load_library(cls): - if cls._xvlib is None: - # 加载动态库 + if cls._xvlib is not None: + return + with cls._load_lock: + # 双重检查锁定: 防止多个线程同时加载 + if cls._xvlib is not None: + return + cls._check_xvsdk() lib_dir = os.path.dirname(__file__) - if os.path.exists(os.path.join(lib_dir, 'libopencv_core.so.4.2')): - ctypes.CDLL(os.path.join(lib_dir, 'libopencv_core.so.4.2'), mode=ctypes.RTLD_GLOBAL) - if os.path.exists(os.path.join(lib_dir, 'libopencv_imgproc.so.4.2')): - ctypes.CDLL(os.path.join(lib_dir, 'libopencv_imgproc.so.4.2'), mode=ctypes.RTLD_GLOBAL) + + cls._load_opencv_lib(lib_dir, 'libopencv_core.so.4.2') + cls._load_opencv_lib(lib_dir, 'libopencv_imgproc.so.4.2') + lib_path = os.path.abspath(os.path.join(lib_dir, 'libxvlib.so')) + if not os.path.exists(lib_path): + raise FileNotFoundError(f"Shared library not found: {lib_path}") logger.info(f"Loading library from: {lib_path}") cls._xvlib = ctypes.CDLL(lib_path) + cls._configure_signatures() logger.info('Library initialized successfully.') @classmethod - def xv_get_devices(cls, max_devices=16): + def _configure_signatures(cls): + """为关键 C 函数设置 argtypes/restype, 避免 ctypes 类型推断错误""" + lib = cls._xvlib + # xv_get_devices 通过 byref 传数组指针，由 ctypes 自动推断类型 + lib.xv_get_devices.restype = ctypes.c_int + # xv_init / xv_uninit + lib.xv_init.argtypes = [ctypes.c_char_p, ctypes.c_bool, ctypes.c_bool, ctypes.c_bool, ctypes.c_bool] + lib.xv_init.restype = ctypes.c_int + lib.xv_uninit.argtypes = [ctypes.c_int] + lib.xv_uninit.restype = ctypes.c_int + # xv_sleep / xv_wakeup + lib.xv_sleep.argtypes = [ctypes.c_int, ctypes.c_int] + lib.xv_sleep.restype = ctypes.c_int + lib.xv_wakeup.argtypes = [ctypes.c_int] + lib.xv_wakeup.restype = ctypes.c_int + # init/uninit functions (C returns bool, must declare c_bool restype) + _init_funcs = [ + 'xv_slam_init', 'xv_slam_uninit', + 'xv_imu_sensor_init', 'xv_imu_sensor_uninit', + 'xv_event_stream_init', 'xv_event_stream_uninit', + 'xv_orientation_stream_init', 'xv_orientation_stream_uninit', + 'xv_fisheye_cameras_init', 'xv_fisheye_cameras_uninit', + 'xv_color_camera_init', 'xv_color_camera_uninit', + 'xv_tof_camera_init', 'xv_tof_camera_uninit', + 'xv_eyetracking_camera_init', 'xv_eyetracking_camera_uninit', + 'xv_gaze_stream_init', 'xv_gaze_stream_uninit', + 'xv_iris_stream_init', 'xv_iris_stream_uninit', + 'xv_gesture_stream_init', 'xv_gesture_stream_uninit', + 'xv_gps_stream_init', 'xv_gps_stream_uninit', + 'xv_gps_distance_stream_init', 'xv_gps_distance_stream_uninit', + 'xv_terrestrial_magnetism_stream_init', 'xv_terrestrial_magnetism_stream_uninit', + 'xv_external_stream_init', 'xv_external_stream_uninit', + 'xv_mic_stream_init', 'xv_mic_stream_uninit', + 'xv_object_detector_init', 'xv_object_detector_uninit', + 'xv_object_detector_RKNN3588_init', 'xv_object_detector_RKNN3588_uninit', + 'xv_device_status_stream_init', 'xv_device_status_stream_uninit', + 'xv_clamp_stream_init', 'xv_clamp_stream_uninit', + 'xv_spheretrack_stream_init', 'xv_spheretrack_stream_uninit', + ] + for name in _init_funcs: + lib[name].restype = ctypes.c_bool + lib[name].argtypes = [ctypes.c_int] + # sgbm_camera_init has extra config string parameter + lib.xv_sgbm_camera_init.restype = ctypes.c_bool + lib.xv_sgbm_camera_init.argtypes = [ctypes.c_int, ctypes.c_char_p] + lib.xv_sgbm_camera_uninit.restype = ctypes.c_bool + lib.xv_sgbm_camera_uninit.argtypes = [ctypes.c_int] + + # data getters (return int) + for name in ['xv_get_slam_data', 'xv_get_slam_pose', 'xv_get_slam_pose_at', + 'xv_get_imu_sensor_data', 'xv_get_color_camera_data', + 'xv_get_tof_camera_data', 'xv_get_fisheye_cameras_data', + 'xv_get_eyetracking_camera_data', 'xv_get_eyetracking_camera_data_ref', + 'xv_get_external_stream_data', + 'xv_get_clamp_stream_data', 'xv_get_event_stream_data', + 'xv_get_spheretrack_stream_data']: + lib[name].restype = ctypes.c_int + + # imu / clamp / event getters (C 兼容类型) + lib.xv_get_imu_sensor_data.argtypes = [ctypes.c_int, ctypes.c_void_p] + lib.xv_get_imu_sensor_data.restype = ctypes.c_int + lib.xv_get_clamp_stream_data.argtypes = [ctypes.c_int, ctypes.c_void_p] + lib.xv_get_clamp_stream_data.restype = ctypes.c_int + lib.xv_get_event_stream_data.argtypes = [ctypes.c_int, ctypes.c_void_p] + lib.xv_get_event_stream_data.restype = ctypes.c_int + + # slam pose getters (额外参数) + lib.xv_get_slam_pose.argtypes = [ctypes.c_int, ctypes.c_void_p, ctypes.c_double] + lib.xv_get_slam_pose_at.argtypes = [ctypes.c_int, ctypes.c_void_p, ctypes.c_double] + + # color camera setters (argtypes) + lib.xv_set_color_camera_rgb_mode.argtypes = [ctypes.c_int, ctypes.c_int] + lib.xv_set_color_camera_resolution.argtypes = [ctypes.c_int, ctypes.c_int] + lib.xv_set_color_camera_framerate.argtypes = [ctypes.c_int, ctypes.c_float] + lib.xv_set_color_camera_brightness.argtypes = [ctypes.c_int, ctypes.c_int] + + # tof camera setters (argtypes) + lib.xv_set_tof_camera_mode.argtypes = [ctypes.c_int, ctypes.c_int] + lib.xv_set_tof_camera_stream_mode.argtypes = [ctypes.c_int, ctypes.c_int] + lib.xv_set_tof_camera_distance_mode.argtypes = [ctypes.c_int, ctypes.c_int] + lib.xv_set_tof_camera_resolution.argtypes = [ctypes.c_int, ctypes.c_int] + lib.xv_set_tof_camera_framerate.argtypes = [ctypes.c_int, ctypes.c_float] + lib.xv_set_tof_camera_brightness.argtypes = [ctypes.c_int, ctypes.c_int] + + # fisheye cameras setters (argtypes) + lib.xv_set_fisheye_cameras_resolution.argtypes = [ctypes.c_int, ctypes.c_int] + lib.xv_set_fisheye_cameras_framerate.argtypes = [ctypes.c_int, ctypes.c_float] + lib.xv_set_fisheye_cameras_brightness.argtypes = [ctypes.c_int, ctypes.c_int] + + # eyetracking camera setters (argtypes) + lib.xv_set_eyetracking_camera_resolution.argtypes = [ctypes.c_int, ctypes.c_int] + lib.xv_set_eyetracking_camera_framerate.argtypes = [ctypes.c_int, ctypes.c_float] + lib.xv_set_eyetracking_camera_brightness.argtypes = [ctypes.c_int, ctypes.c_int] + + # 零拷贝 Ref getter (return int) + for name in ['xv_get_color_camera_data_ref', 'xv_get_tof_camera_data_ref', + 'xv_get_fisheye_cameras_data_ref', 'xv_get_eyetracking_camera_data_ref']: + lib[name].restype = ctypes.c_int + + @classmethod + def xv_get_devices(cls, timeout=5.0, max_devices=16, double_query=True): + """Scan for connected XV devices. + + Args: + timeout: Max seconds to wait for devices to respond. + max_devices: Max number of device entries to return. + double_query: If True, do a second short scan to catch late-registering + devices (needed when multiple devices are present). Set to False + when the caller has its own retry loop (e.g. __init__). + + Returns: + (device_count, list_of_DeviceStruct) + """ cls.__load_library() devices = (DeviceStruct * max_devices)() device_count = ctypes.c_int(0) cls._xvlib.xv_get_devices( ctypes.byref(devices), ctypes.byref(device_count), - ctypes.c_int(max_devices) + ctypes.c_int(max_devices), + ctypes.c_double(timeout) ) + if double_query: + # 多设备场景下第1次扫描可能遗漏设备，第2次补扫 + if timeout > 3 and device_count.value != 0: + time.sleep(1) + cls._xvlib.xv_get_devices( + ctypes.byref(devices), + ctypes.byref(device_count), + ctypes.c_int(max_devices), + ctypes.c_double(2.0) + ) return device_count.value, list(devices[:device_count.value]) def xv_init(self, serial_number, init_slam, init_clamp_stream, init_color_camera, init_fisheye_cameras): + """Create and initialize a device instance. Returns instance_id (>0) or INSTANCE_INVALID (-9).""" return self._xvlib.xv_init(serial_number, init_slam, init_clamp_stream, init_color_camera, init_fisheye_cameras) def xv_uninit(self): + """Uninitialize device and release all streams. Returns 0 on success.""" if self._xvlib is not None and self.instance_id > 0: return self._xvlib.xv_uninit(self.instance_id) else: return -1 - - def xv_sleep(self, level = 0): + + def xv_sleep(self, level=0): + """Put device into low-power sleep mode.""" return self._xvlib.xv_sleep(self.instance_id, level) def xv_wakeup(self): - return self._xvlib.xv_wakeup(self.instance_id) + """Wake device from sleep mode.""" + return self._xvlib.xv_wakeup(self.instance_id) def xv_slam_init(self): + """Start SLAM tracking stream. Returns True on success.""" return self._xvlib.xv_slam_init(self.instance_id) def xv_slam_uninit(self): + """Stop SLAM tracking stream. Returns True on success.""" return self._xvlib.xv_slam_uninit(self.instance_id) - + def xv_imu_sensor_init(self): + """Start IMU sensor stream. Returns True on success.""" return self._xvlib.xv_imu_sensor_init(self.instance_id) def xv_imu_sensor_uninit(self): + """Stop IMU sensor stream. Returns True on success.""" return self._xvlib.xv_imu_sensor_uninit(self.instance_id) - + def xv_event_stream_init(self): + """Start device event stream. Returns True on success.""" return self._xvlib.xv_event_stream_init(self.instance_id) def xv_event_stream_uninit(self): + """Stop device event stream. Returns True on success.""" return self._xvlib.xv_event_stream_uninit(self.instance_id) - + def xv_orientation_stream_init(self): + """Start 3-DOF orientation stream. Returns True on success.""" return self._xvlib.xv_orientation_stream_init(self.instance_id) def xv_orientation_stream_uninit(self): + """Stop 3-DOF orientation stream. Returns True on success.""" return self._xvlib.xv_orientation_stream_uninit(self.instance_id) - + def xv_fisheye_cameras_init(self): + """Start fisheye cameras stream (used for SLAM). Returns True on success.""" return self._xvlib.xv_fisheye_cameras_init(self.instance_id) def xv_fisheye_cameras_uninit(self): + """Stop fisheye cameras stream. Returns True on success.""" return self._xvlib.xv_fisheye_cameras_uninit(self.instance_id) - + def xv_color_camera_init(self): + """Start color (RGB) camera stream. Returns True on success.""" return self._xvlib.xv_color_camera_init(self.instance_id) - + def xv_color_camera_uninit(self): + """Stop color camera stream. Returns True on success.""" return self._xvlib.xv_color_camera_uninit(self.instance_id) def xv_tof_camera_init(self): + """Start TOF depth camera stream. Returns True on success.""" return self._xvlib.xv_tof_camera_init(self.instance_id) - + def xv_tof_camera_uninit(self): + """Stop TOF depth camera stream. Returns True on success.""" return self._xvlib.xv_tof_camera_uninit(self.instance_id) def xv_sgbm_camera_init(self, config): + """Start SGBM stereo depth camera stream with config string. Returns True on success.""" return self._xvlib.xv_sgbm_camera_init(self.instance_id, ctypes.c_char_p(config.encode('utf-8'))) - + def xv_sgbm_camera_uninit(self): + """Stop SGBM camera stream. Returns True on success.""" return self._xvlib.xv_sgbm_camera_uninit(self.instance_id) - + def xv_eyetracking_camera_init(self): + """Start eyetracking camera stream. Returns True on success.""" return self._xvlib.xv_eyetracking_camera_init(self.instance_id) - + def xv_eyetracking_camera_uninit(self): + """Stop eyetracking camera stream. Returns True on success.""" return self._xvlib.xv_eyetracking_camera_uninit(self.instance_id) def xv_gaze_stream_init(self): + """Start gaze tracking stream. Returns True on success.""" return self._xvlib.xv_gaze_stream_init(self.instance_id) - + def xv_gaze_stream_uninit(self): + """Stop gaze tracking stream. Returns True on success.""" return self._xvlib.xv_gaze_stream_uninit(self.instance_id) - + def xv_iris_stream_init(self): + """Start iris recognition stream. Returns True on success.""" return self._xvlib.xv_iris_stream_init(self.instance_id) - + def xv_iris_stream_uninit(self): + """Stop iris recognition stream. Returns True on success.""" return self._xvlib.xv_iris_stream_uninit(self.instance_id) - + def xv_gesture_stream_init(self): + """Start gesture recognition stream. Returns True on success.""" return self._xvlib.xv_gesture_stream_init(self.instance_id) - + def xv_gesture_stream_uninit(self): + """Stop gesture recognition stream. Returns True on success.""" return self._xvlib.xv_gesture_stream_uninit(self.instance_id) - + def xv_gps_stream_init(self): + """Start GPS data stream. Returns True on success.""" return self._xvlib.xv_gps_stream_init(self.instance_id) - + def xv_gps_stream_uninit(self): + """Stop GPS data stream. Returns True on success.""" return self._xvlib.xv_gps_stream_uninit(self.instance_id) - + def xv_gps_distance_stream_init(self): + """Start GPS distance stream. Returns True on success.""" return self._xvlib.xv_gps_distance_stream_init(self.instance_id) - + def xv_gps_distance_stream_uninit(self): + """Stop GPS distance stream. Returns True on success.""" return self._xvlib.xv_gps_distance_stream_uninit(self.instance_id) - + def xv_terrestrial_magnetism_stream_init(self): + """Start terrestrial magnetism sensor stream. Returns True on success.""" return self._xvlib.xv_terrestrial_magnetism_stream_init(self.instance_id) - + def xv_terrestrial_magnetism_stream_uninit(self): + """Stop terrestrial magnetism sensor stream. Returns True on success.""" return self._xvlib.xv_terrestrial_magnetism_stream_uninit(self.instance_id) def xv_external_stream_init(self): + """Start external sensor stream. Returns True on success.""" return self._xvlib.xv_external_stream_init(self.instance_id) def xv_external_stream_uninit(self): + """Stop external sensor stream. Returns True on success.""" return self._xvlib.xv_external_stream_uninit(self.instance_id) - + def xv_mic_stream_init(self): + """Start microphone stream. Returns True on success.""" return self._xvlib.xv_mic_stream_init(self.instance_id) def xv_mic_stream_uninit(self): + """Stop microphone stream. Returns True on success.""" return self._xvlib.xv_mic_stream_uninit(self.instance_id) - + def xv_object_detector_init(self): + """Start object detector stream. Returns True on success.""" return self._xvlib.xv_object_detector_init(self.instance_id) def xv_object_detector_uninit(self): + """Stop object detector stream. Returns True on success.""" return self._xvlib.xv_object_detector_uninit(self.instance_id) - + def xv_object_detector_RKNN3588_init(self): + """Start object detector (RKNN3588) stream. Returns True on success.""" return self._xvlib.xv_object_detector_RKNN3588_init(self.instance_id) def xv_object_detector_RKNN3588_uninit(self): + """Stop object detector (RKNN3588) stream. Returns True on success.""" return self._xvlib.xv_object_detector_RKNN3588_uninit(self.instance_id) - + def xv_device_status_stream_init(self): + """Start device status stream. Returns True on success.""" return self._xvlib.xv_device_status_stream_init(self.instance_id) def xv_device_status_stream_uninit(self): + """Stop device status stream. Returns True on success.""" return self._xvlib.xv_device_status_stream_uninit(self.instance_id) def xv_clamp_stream_init(self): + """Start clamp sensor stream. Returns True on success.""" return self._xvlib.xv_clamp_stream_init(self.instance_id) - + def xv_clamp_stream_uninit(self): + """Stop clamp sensor stream. Returns True on success.""" return self._xvlib.xv_clamp_stream_uninit(self.instance_id) - + def xv_spheretrack_stream_init(self): + """Start spheretrack stream. Returns True on success.""" return self._xvlib.xv_spheretrack_stream_init(self.instance_id) def xv_spheretrack_stream_uninit(self): + """Stop spheretrack stream. Returns True on success.""" return self._xvlib.xv_spheretrack_stream_uninit(self.instance_id) def xv_get_clamp_stream_data(self): + """Get the most recently received clamp sensor data (updated by callback). Returns (ret_code, ClampData).""" ret = self._xvlib.xv_get_clamp_stream_data(self.instance_id, ctypes.byref(self._clamp_data)) return ret, self._clamp_data - - def xv_get_color_image_data(self): - ret = self._xvlib.xv_get_color_image_data(self.instance_id, ctypes.byref(self._color_image_data)) + + def xv_get_event_stream_data(self): + """Get the most recently received device event (updated by callback). Returns (ret_code, EventData).""" + ret = self._xvlib.xv_get_event_stream_data(self.instance_id, ctypes.byref(self._event_data)) + return ret, self._event_data + + def xv_get_color_camera_data(self, use_ref=False): + """Get the most recently received color camera image (updated by callback). + + Args: + use_ref: If True, use zero-copy Ref API (direct SDK buffer pointer, no memcpy). + If False (default), use legacy API with memcpy into fixed buffer. + + Returns: + (ret_code, ColorImageData | ColorImageDataRef). Call .frame() for an OpenCV BGR mat. + """ + if use_ref: + return self._xv_get_color_camera_data_ref() + ret = self._xvlib.xv_get_color_camera_data(self.instance_id, ctypes.byref(self._color_image_data)) return ret, self._color_image_data - def xv_get_color_image_rgb_data(self): - ret = self._xvlib.xv_get_color_image_rgb_data(self.instance_id, ctypes.byref(self._color_image_rgb_data)) - return ret, self._color_image_rgb_data - - def xv_get_depth_image_data(self): - ret = self._xvlib.xv_get_depth_image_data(self.instance_id, ctypes.byref(self._depth_image_data)) + def xv_get_tof_camera_data(self, use_ref=False): + """Get the most recently received depth/TOF camera image (updated by callback). + + Args: + use_ref: If True, use zero-copy Ref API. Default False (legacy memcpy). + + Returns: + (ret_code, DepthImageData | DepthImageDataRef). Call .frame() for an OpenCV mat. + """ + if use_ref: + return self._xv_get_tof_camera_data_ref() + ret = self._xvlib.xv_get_tof_camera_data(self.instance_id, ctypes.byref(self._depth_image_data)) return ret, self._depth_image_data - def xv_get_fisheye_images_data(self, index=5): - ret = self._xvlib.xv_get_fisheye_images_data(self.instance_id, ctypes.byref(self._fisheye_images_data), ctypes.c_size_t(index)) + def xv_get_fisheye_cameras_data(self, use_ref=False): + """Get the most recently received fisheye camera images, up to 4 grayscale frames (updated by callback). + + Args: + use_ref: If True, use zero-copy Ref API. Default False (legacy memcpy). + + Returns: + (ret_code, FisheyeImagesData | FisheyeImagesDataRef). Call .frame() for a + concatenated BGR mat, or .frame(i) for a single camera. + """ + if use_ref: + return self._xv_get_fisheye_cameras_data_ref() + ret = self._xvlib.xv_get_fisheye_cameras_data(self.instance_id, ctypes.byref(self._fisheye_images_data)) return ret, self._fisheye_images_data - + + def xv_get_eyetracking_camera_data(self, use_ref=False): + """Get the most recently received eyetracking camera images, up to 4 grayscale frames (updated by callback). + + Args: + use_ref: If True, use zero-copy Ref API. Default False (legacy memcpy). + + Returns: + (ret_code, EyetrackingImageData | EyetrackingImageDataRef). + """ + if use_ref: + return self._xv_get_eyetracking_camera_data_ref() + ret = self._xvlib.xv_get_eyetracking_camera_data(self.instance_id, ctypes.byref(self._eyetracking_image_data)) + return ret, self._eyetracking_image_data + def xv_get_slam_data(self): + """Get the most recently received SLAM 6-DOF pose (updated by callback). Returns (ret_code, PoseData).""" ret = self._xvlib.xv_get_slam_data(self.instance_id, ctypes.byref(self._slam_data)) return ret, self._slam_data - + def xv_get_slam_pose(self, prediction): + """Get SLAM pose with extrapolation. + + Args: + prediction: Seconds into the future to predict (0 = current). + + Returns: + (ret_code, PoseData). + """ ret = self._xvlib.xv_get_slam_pose(self.instance_id, ctypes.byref(self._slam_data), ctypes.c_double(prediction)) return ret, self._slam_data - + def xv_get_slam_pose_at(self, timestamp): + """Get SLAM pose at a specific host-clock timestamp. Returns (ret_code, PoseData).""" ret = self._xvlib.xv_get_slam_pose_at(self.instance_id, ctypes.byref(self._slam_data), ctypes.c_double(timestamp)) return ret, self._slam_data - + def xv_get_external_stream_data(self): + """Get the most recently received external sensor pose (updated by callback). Returns (ret_code, PoseData).""" ret = self._xvlib.xv_get_external_stream_data(self.instance_id, ctypes.byref(self._external_stream_data)) return ret, self._external_stream_data - + def xv_get_spheretrack_stream_data(self): + """Get the most recently received spheretrack pose (updated by callback). Returns (ret_code, PoseData).""" ret = self._xvlib.xv_get_spheretrack_stream_data(self.instance_id, ctypes.byref(self._spheretrack_stream_data)) return ret, self._spheretrack_stream_data - + + # ============== Zero-copy Ref getters (internal, called by xv_get_*_data(use_ref=True)) ============== + + def _xv_get_color_camera_data_ref(self): + """Internal: zero-copy color camera data (updated by callback). Returns (ret_code, ColorImageDataRef).""" + ret = self._xvlib.xv_get_color_camera_data_ref(self.instance_id, ctypes.byref(self._color_image_data_ref)) + return ret, self._color_image_data_ref + + def _xv_get_tof_camera_data_ref(self): + """Internal: zero-copy depth camera data (updated by callback). Returns (ret_code, DepthImageDataRef).""" + ret = self._xvlib.xv_get_tof_camera_data_ref(self.instance_id, ctypes.byref(self._depth_image_data_ref)) + return ret, self._depth_image_data_ref + + def _xv_get_fisheye_cameras_data_ref(self): + """Internal: zero-copy fisheye camera data (updated by callback). Returns (ret_code, FisheyeImagesDataRef).""" + ret = self._xvlib.xv_get_fisheye_cameras_data_ref(self.instance_id, ctypes.byref(self._fisheye_images_data_ref)) + return ret, self._fisheye_images_data_ref + + def _xv_get_eyetracking_camera_data_ref(self): + """Internal: zero-copy eyetracking camera data (updated by callback). Returns (ret_code, EyetrackingImageDataRef).""" + ret = self._xvlib.xv_get_eyetracking_camera_data_ref(self.instance_id, ctypes.byref(self._eyetracking_image_data_ref)) + return ret, self._eyetracking_image_data_ref + + # ============================================================================================ + def xv_set_color_camera_rgb_mode(self, mode): - """ - Docstring for xv_set_color_camera_rgb_mode - - :param mode: Description - 0: AF - 1: MF - 2: Unknown - """ + """Set color camera RGB mode. 0=AF, 1=MF, 2=Unknown. Returns ret_code.""" return self._xvlib.xv_set_color_camera_rgb_mode(self.instance_id, ctypes.c_int(mode)) - + def xv_set_color_camera_resolution(self, resolution): - """ - Docstring for xv_set_color_camera_resolution - - :param resolution: - 0: RGB_1920x1080 - 1: RGB_1280x720 - 2: RGB_640x480 - 3: RGB_320x240 (not supported now) - 4: RGB_2560x1920 (not supported now) - 5: RGB_3840x2160 (not supported now) - """ + """Set color camera resolution. 0=1920x1080, 1=1280x720, 2=640x480, 3=320x240, 4=2560x1920, 5=3840x2160. Returns ret_code.""" return self._xvlib.xv_set_color_camera_resolution(self.instance_id, ctypes.c_int(resolution)) - + def xv_set_color_camera_framerate(self, framerate): + """Set color camera framerate (float, Hz). Returns ret_code.""" return self._xvlib.xv_set_color_camera_framerate(self.instance_id, ctypes.c_float(framerate)) - + def xv_set_color_camera_brightness(self, brightness): + """Set color camera brightness. Returns ret_code.""" return self._xvlib.xv_set_color_camera_brightness(self.instance_id, ctypes.c_int(brightness)) - + def xv_set_tof_camera_mode(self, mode): + """Set TOF camera mode. Returns ret_code.""" return self._xvlib.xv_set_tof_camera_mode(self.instance_id, ctypes.c_int(mode)) - + def xv_set_tof_camera_stream_mode(self, mode): - """ - Docstring for xv_set_tof_camera_stream_mode - - :param mode: Description - 0: DepthOnly - 1: CloudOnly - 2: DepthAndCloud - 3: None - 4: CloudOnLeftHandSlam - """ + """Set TOF stream mode. 0=DepthOnly, 1=CloudOnly, 2=DepthAndCloud, 3=None, 4=CloudOnLeftHandSlam. Returns ret_code.""" return self._xvlib.xv_set_tof_camera_stream_mode(self.instance_id, ctypes.c_int(mode)) - + def xv_set_tof_camera_distance_mode(self, mode): - """ - Docstring for xv_set_tof_camera_distance_mode - - :param mode: Description - 0: Short - 1: Middle - 2: Long - """ + """Set TOF distance mode. 0=Short, 1=Middle, 2=Long. Returns ret_code.""" return self._xvlib.xv_set_tof_camera_distance_mode(self.instance_id, ctypes.c_int(mode)) def xv_set_tof_camera_resolution(self, resolution): - """ - Docstring for xv_set_tof_camera_resolution - - :param resolution: Description - -1: Unknown - 0: VGA - 1: QVGA - 2: HQVGA - """ + """Set TOF camera resolution. -1=Unknown, 0=VGA, 1=QVGA, 2=HQVGA. Returns ret_code.""" return self._xvlib.xv_set_tof_camera_resolution(self.instance_id, ctypes.c_int(resolution)) - + def xv_set_tof_camera_framerate(self, framerate): - """ - Docstring for xv_set_tof_camera_framerate - - :param framerate: Description - 0: FPS_5 - 1: FPS_10 - 2: FPS_15 - 3: FPS_20 - 4: FPS_25 - 5: FPS_30 - """ + """Set TOF camera framerate (float, Hz, e.g. 5.0, 10.0, 15.0, 20.0, 25.0, 30.0). Returns ret_code.""" return self._xvlib.xv_set_tof_camera_framerate(self.instance_id, ctypes.c_float(framerate)) - + def xv_set_tof_camera_brightness(self, brightness): + """Set TOF camera brightness. Returns ret_code.""" return self._xvlib.xv_set_tof_camera_brightness(self.instance_id, ctypes.c_int(brightness)) - + def xv_set_fisheye_cameras_resolution(self, resolution): + """Set fisheye cameras resolution. Returns ret_code.""" return self._xvlib.xv_set_fisheye_cameras_resolution(self.instance_id, ctypes.c_int(resolution)) - + def xv_set_fisheye_cameras_framerate(self, framerate): + """Set fisheye cameras framerate (float, Hz). Returns ret_code.""" return self._xvlib.xv_set_fisheye_cameras_framerate(self.instance_id, ctypes.c_float(framerate)) def xv_set_fisheye_cameras_brightness(self, brightness): + """Set fisheye cameras brightness. Returns ret_code.""" return self._xvlib.xv_set_fisheye_cameras_brightness(self.instance_id, ctypes.c_int(brightness)) def xv_set_eyetracking_camera_resolution(self, resolution): + """Set eyetracking camera resolution. Returns ret_code.""" return self._xvlib.xv_set_eyetracking_camera_resolution(self.instance_id, ctypes.c_int(resolution)) - + def xv_set_eyetracking_camera_framerate(self, framerate): + """Set eyetracking camera framerate (float, Hz). Returns ret_code.""" return self._xvlib.xv_set_eyetracking_camera_framerate(self.instance_id, ctypes.c_float(framerate)) - + def xv_set_eyetracking_camera_brightness(self, brightness): + """Set eyetracking camera brightness. Returns ret_code.""" return self._xvlib.xv_set_eyetracking_camera_brightness(self.instance_id, ctypes.c_int(brightness)) diff --git a/src/ufactory_lerobot/scripts/uf_lerobot_eval.py b/src/ufactory_lerobot/scripts/uf_lerobot_eval.py index 170f6f3..e5f6425 100644 --- a/src/ufactory_lerobot/scripts/uf_lerobot_eval.py +++ b/src/ufactory_lerobot/scripts/uf_lerobot_eval.py @@ -73,8 +73,8 @@ def compute_relative_axis_angle(rot_prev, rot_curr): 返回: 相对轴角向量 """ # 1. 转为矩阵 - R_prev = Transformations.rxryrz_to_rotation_matrix(rot_prev) - R_curr = Transformations.rxryrz_to_rotation_matrix(rot_curr) + R_prev = Transformations.rxryrz_to_rotation_matrix(*rot_prev) + R_curr = Transformations.rxryrz_to_rotation_matrix(*rot_curr) # 2. 计算相对旋转矩阵 # R_delta 表示从 prev 坐标系到 curr 坐标系的旋转 @@ -87,8 +87,8 @@ def compute_target_axis_angle(rot_prev, rot_delta): """ 根据起始轴角和相对轴角计算目标轴角 """ - R_prev = Transformations.rxryrz_to_rotation_matrix(rot_prev) - R_delta = Transformations.rxryrz_to_rotation_matrix(rot_delta) + R_prev = Transformations.rxryrz_to_rotation_matrix(*rot_prev) + R_delta = Transformations.rxryrz_to_rotation_matrix(*rot_delta) R_curr = R_prev @ R_delta # R_curr = R_prev.apply(R_delta) return Transformations.rotation_matrix_to_rxryrz(R_curr)