import grpc import numpy as np from PIL import Image from typing import Tuple from .enums import CMVRErrorCode from .models import CameraState from .models import CameraIntrinsics class CameraClient: """相机客户端""" def __init__(self, device_id: str, stub): self.device_id = device_id self.stub = stub self.generated = None # 推迟导入 def _import_generated(self): """推迟导入 generated""" if self.generated is None: try: # 从正确的路径导入生成的模块 from generated.cmvr.api import common_pb2, camera_command_pb2 self.generated = type('GeneratedModules', (), { 'common_pb2': common_pb2, 'camera_command_pb2': camera_command_pb2 }) except ImportError as e: print(f"导入生成的模块失败: {e}") print("请确保已生成 protobuf 代码") raise return self.generated def _create_command_header(self): generated = self._import_generated() """创建命令头""" header = generated.common_pb2.CommandHeader.Request() header.device_id = self.device_id header.timestamp.GetCurrentTime() return header def get_status(self) -> Tuple[CMVRErrorCode, CameraState]: """获取相机状态""" try: generated = self._import_generated() request = generated.camera_command_pb2.GetCameraStateCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.GetStatus(request) if not response.header.success: return CMVRErrorCode.CMVR_RPC_FAILED, CameraState() state = CameraState( is_initialized=response.state.is_initialized, is_opened=response.state.is_opened, is_streaming=response.state.is_streaming, is_recording=response.state.is_recording, width=response.state.width, height=response.state.height, fps=response.state.fps ) return CMVRErrorCode.CMVR_SUCCESS, state except grpc.RpcError as e: print(f"获取相机状态失败: {e}") return CMVRErrorCode.CMVR_RPC_FAILED, CameraState() def start_camera(self) -> CMVRErrorCode: """启动相机""" try: generated = self._import_generated() request = generated.camera_command_pb2.StartCameraCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.StartCamera(request) return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED except grpc.RpcError as e: print(f"启动相机失败: {e}") return CMVRErrorCode.CMVR_RPC_FAILED def stop_camera(self) -> CMVRErrorCode: """停止相机""" try: generated = self._import_generated() request = generated.camera_command_pb2.StopCameraCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.StopCamera(request) return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED except grpc.RpcError as e: print(f"停止相机失败: {e}") return CMVRErrorCode.CMVR_RPC_FAILED def get_rgb_image(self) -> Tuple[CMVRErrorCode, np.ndarray, int, int]: """获取RGB图像""" try: generated = self._import_generated() request = generated.camera_command_pb2.GetRGBImageCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.GetRGBImage(request) if not response.header.success: return CMVRErrorCode.CMVR_RPC_FAILED, np.array([]), 0, 0 width = response.color_frame.width height = response.color_frame.height size = len(response.color_frame.data) # 将字节数据转换为numpy数组 img_data = np.frombuffer(response.color_frame.data, dtype=np.uint8) # 从 Protobuf 消息中读取 intrinsics 数据 protobuf_intrinsics = response.intrinsics # 构造 Python 数据类对象（字段名称完全对应） python_intrinsics = CameraIntrinsics( cx=protobuf_intrinsics.cx, cy=protobuf_intrinsics.cy, fx=protobuf_intrinsics.fx, fy=protobuf_intrinsics.fy, coeffs=list(protobuf_intrinsics.coeffs) # Protobuf repeated 字段通常是列表类型，直接转换 ) # 示例：访问读取后的数据 print(f"主点坐标: ({python_intrinsics.cx}, {python_intrinsics.cy})") print(f"焦距: ({python_intrinsics.fx}, {python_intrinsics.fy})") print(f"畸变系数: {python_intrinsics.coeffs}") # 重塑为图像格式 (height, width, channels) channels = 3 # 假设是RGB图像 if size == width * height * channels: img_array = img_data.reshape((height, width, channels)) else: # 可能是灰度图或其他格式 img_array = img_data.reshape((height, width)) return CMVRErrorCode.CMVR_SUCCESS, img_array, width, height except grpc.RpcError as e: print(f"获取图像失败: {e}") return CMVRErrorCode.CMVR_RPC_FAILED, np.array([]), 0, 0 def get_depth_image(self) -> Tuple[CMVRErrorCode, np.ndarray, int, int, CameraIntrinsics]: """ 获取深度图像及内参（修复返回值类型） """ # 创建默认内参实例（空值）用于错误返回 default_intrinsics = CameraIntrinsics(cx=0, cy=0, fx=0, fy=0, coeffs=[]) # 错误返回模板（类型与声明完全匹配） error_return = ( CMVRErrorCode.CMVR_RPC_FAILED, np.array([], dtype=np.uint16), 0, 0, default_intrinsics ) try: generated = self._import_generated() if not generated: print("无法导入生成的protobuf模块") return error_return # 创建请求 request = generated.camera_command_pb2.GetDepthImageComand.Request() request.header.CopyFrom(self._create_command_header()) # 调用RPC response = self.stub.GetDepthImage(request) if not response.header.success: print(f"深度图像请求失败，错误码: {response.header.error_code}") return ( CMVRErrorCode.CMVR_RPC_FAILED, np.array([], dtype=np.uint16), 0, 0, default_intrinsics ) # 提取图像信息 width = response.depth_frame.width height = response.depth_frame.height depth_data = response.depth_frame.data expected_size = width * height * 2 if len(depth_data) != expected_size: print(f"深度图像数据尺寸不匹配: 预期 {expected_size}, 实际 {len(depth_data)}") return ( CMVRErrorCode.CMVR_INTERNAL_ERROR, np.array([], dtype=np.uint16), 0, 0, default_intrinsics ) # 转换为数组 depth_array = np.frombuffer(depth_data, dtype=np.uint16).reshape((height, width)) # 解析内参（确保始终返回有效实例） intrinsics = CameraIntrinsics( cx=response.intrinsics.cx, cy=response.intrinsics.cy, fx=response.intrinsics.fx, fy=response.intrinsics.fy, coeffs=list(response.intrinsics.coeffs) if hasattr(response.intrinsics, 'coeffs') else [] ) return ( CMVRErrorCode.CMVR_SUCCESS, depth_array, width, height, intrinsics ) except grpc.RpcError as e: print(f"深度图像RPC调用失败: {str(e)}") return error_return except Exception as e: print(f"深度图像处理失败: {str(e)}") return ( CMVRErrorCode.CMVR_INTERNAL_ERROR, np.array([], dtype=np.uint16), 0, 0, default_intrinsics ) def get_rgbd_images(self) -> Tuple[CMVRErrorCode, np.ndarray, np.ndarray, int, int, CameraIntrinsics]: """ 获取同步RGBD图像及内参（修复返回值类型） """ # 创建默认内参实例 default_intrinsics = CameraIntrinsics(cx=0, cy=0, fx=0, fy=0, coeffs=[]) # 错误返回模板 error_return = ( CMVRErrorCode.CMVR_RPC_FAILED, np.array([], dtype=np.uint8), np.array([], dtype=np.uint16), 0, 0, default_intrinsics ) try: generated = self._import_generated() if not generated: print("无法导入生成的protobuf模块") return error_return # 创建请求 request = generated.camera_command_pb2.GetRGBDImagesCommand.Request() request.header.CopyFrom(self._create_command_header()) # 调用RPC response = self.stub.GetRGBDImages(request) if not response.header.success: print(f"RGBD图像请求失败，错误码: {response.header.error_code}") return ( CMVRErrorCode.CMVR_RPC_FAILED, np.array([], dtype=np.uint8), np.array([], dtype=np.uint16), 0, 0, default_intrinsics ) # 提取尺寸信息 rgb_width, rgb_height = response.color_frame.width, response.color_frame.height depth_width, depth_height = response.depth_frame.width, response.depth_frame.height if rgb_width != depth_width or rgb_height != depth_height: print(f"RGBD尺寸不匹配: RGB({rgb_width}x{rgb_height}), Depth({depth_width}x{depth_height})") return ( CMVRErrorCode.CMVR_INTERNAL_ERROR, np.array([], dtype=np.uint8), np.array([], dtype=np.uint16), 0, 0, default_intrinsics ) # 处理RGB图像 rgb_data = response.color_frame.data rgb_expected = rgb_width * rgb_height * 3 if len(rgb_data) != rgb_expected: print(f"RGB数据尺寸不匹配: 预期 {rgb_expected}, 实际 {len(rgb_data)}") return ( CMVRErrorCode.CMVR_INTERNAL_ERROR, np.array([], dtype=np.uint8), np.array([], dtype=np.uint16), 0, 0, default_intrinsics ) rgb_array = np.frombuffer(rgb_data, dtype=np.uint8).reshape((rgb_height, rgb_width, 3)) # 处理深度图像 depth_data = response.depth_frame.data depth_expected = depth_width * depth_height * 2 if len(depth_data) != depth_expected: print(f"深度数据尺寸不匹配: 预期 {depth_expected}, 实际 {len(depth_data)}") return ( CMVRErrorCode.CMVR_INTERNAL_ERROR, np.array([], dtype=np.uint8), np.array([], dtype=np.uint16), 0, 0, default_intrinsics ) depth_array = np.frombuffer(depth_data, dtype=np.uint16).reshape((depth_height, depth_width)) # 解析内参 intrinsics = CameraIntrinsics( cx=response.intrinsics.cx, cy=response.intrinsics.cy, fx=response.intrinsics.fx, fy=response.intrinsics.fy, coeffs=list(response.intrinsics.coeffs) if hasattr(response.intrinsics, 'coeffs') else [] ) return ( CMVRErrorCode.CMVR_SUCCESS, rgb_array, depth_array, rgb_width, rgb_height, intrinsics ) except grpc.RpcError as e: print(f"RGBD RPC调用失败: {str(e)}") return error_return except Exception as e: print(f"RGBD图像处理失败: {str(e)}") return ( CMVRErrorCode.CMVR_INTERNAL_ERROR, np.array([], dtype=np.uint8), np.array([], dtype=np.uint16), 0, 0, default_intrinsics ) def start_record(self, video_path: str) -> CMVRErrorCode: """开始录像""" try: generated = self._import_generated() request = generated.camera_command_pb2.StartCameraRecordingCommand.Request() request.header.CopyFrom(self._create_command_header()) request.video_path = video_path response = self.stub.StartRecording(request) return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED except grpc.RpcError as e: print(f"开始录像失败: {e}") return CMVRErrorCode.CMVR_RPC_FAILED def stop_record(self) -> CMVRErrorCode: """停止录像""" try: generated = self._import_generated() request = generated.camera_command_pb2.StopCameraRecordingCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.StopRecording(request) return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED except grpc.RpcError as e: print(f"停止录像失败: {e}") return CMVRErrorCode.CMVR_RPC_FAILED def create_rgb_stream_request(self): """创建流请求（无参数）""" generated = self._import_generated() request = generated.camera_command_pb2.GetRGBImageStreamCommand.Request() request.header.CopyFrom(self._create_command_header()) # 不需要设置其他参数 return request def get_rgb_stream(self, request_generator): """ 双向流获取传感器数据 参数: request_generator: 请求生成器 返回: 传感器数据流迭代器 """ generated = self._import_generated() return self.stub.GetRGBImageStream(request_generator) def create_depth_stream_request(self): """创建流请求（无参数）""" generated = self._import_generated() request = generated.camera_command_pb2.GetDepthImageStreamCommand.Request() request.header.CopyFrom(self._create_command_header()) # 不需要设置其他参数 return request def get_depth_stream(self, request_generator): """ 双向流获取传感器数据 参数: request_generator: 请求生成器 返回: 传感器数据流迭代器 """ generated = self._import_generated() return self.stub.GetDepthImageStream(request_generator) def create_rgbd_stream_request(self): """创建流请求（无参数）""" generated = self._import_generated() request = generated.camera_command_pb2.GetRGBDImageStreamCommand.Request() request.header.CopyFrom(self._create_command_header()) # 不需要设置其他参数 return request def get_rgbd_stream(self, request_generator): """ 双向流获取传感器数据 参数: request_generator: 请求生成器 返回: 传感器数据流迭代器 """ generated = self._import_generated() return self.stub.GetRGBDImageStream(request_generator)