443 lines
16 KiB
Python
443 lines
16 KiB
Python
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) |