From 49dd1c4248e58f343a26b6974570348759bd32eb Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Mon, 25 Aug 2025 10:34:44 +0800 Subject: [PATCH] update --- cmvr/camera_client.py | 38 ++++- cmvr/client.py | 35 ++++- cmvr/dexhand_client.py | 298 ++++++++++++++++++++++++++++++++--- cmvr/enums.py | 3 +- cmvr/micphone_client.py | 28 +++- cmvr/models.py | 42 +++-- cmvr/speaker_client.py | 40 ++++- example_usage.py | 340 +++++++++++++++++++++++++++++++++++++++- 8 files changed, 763 insertions(+), 61 deletions(-) diff --git a/cmvr/camera_client.py b/cmvr/camera_client.py index cf5e1b4..a05def3 100644 --- a/cmvr/camera_client.py +++ b/cmvr/camera_client.py @@ -3,7 +3,7 @@ import numpy as np from PIL import Image from typing import Tuple -from . import generated + from .enums import CMVRErrorCode from .models import CameraState @@ -13,8 +13,26 @@ 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 @@ -24,7 +42,8 @@ class CameraClient: def get_status(self) -> Tuple[CMVRErrorCode, CameraState]: """获取相机状态""" try: - request = generated.camera_service_pb2.GetCameraStateCommand.Request() + generated = self._import_generated() + request = generated.camera_command_pb2.GetCameraStateCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.GetStatus(request) @@ -51,7 +70,8 @@ class CameraClient: def start_camera(self) -> CMVRErrorCode: """启动相机""" try: - request = generated.camera_service_pb2.StartCameraCommand.Request() + generated = self._import_generated() + request = generated.camera_command_pb2.StartCameraCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.StartCamera(request) @@ -65,7 +85,8 @@ class CameraClient: def stop_camera(self) -> CMVRErrorCode: """停止相机""" try: - request = generated.camera_service_pb2.StopCameraCommand.Request() + generated = self._import_generated() + request = generated.camera_command_pb2.StopCameraCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.StopCamera(request) @@ -79,7 +100,8 @@ class CameraClient: def get_rgb_image(self) -> Tuple[CMVRErrorCode, np.ndarray, int, int]: """获取RGB图像""" try: - request = generated.camera_service_pb2.GetRGBImageCommand.Request() + generated = self._import_generated() + request = generated.camera_command_pb2.GetRGBImageCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.GetRGBImage(request) @@ -111,7 +133,8 @@ class CameraClient: def start_record(self, video_path: str) -> CMVRErrorCode: """开始录像""" try: - request = generated.camera_service_pb2.StartCameraRecordingCommand.Request() + generated = self._import_generated() + request = generated.camera_command_pb2.StartCameraRecordingCommand.Request() request.header.CopyFrom(self._create_command_header()) request.video_path = video_path @@ -126,7 +149,8 @@ class CameraClient: def stop_record(self) -> CMVRErrorCode: """停止录像""" try: - request = generated.camera_service_pb2.StopCameraRecordingCommand.Request() + generated = self._import_generated() + request = generated.camera_command_pb2.StopCameraRecordingCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.StopRecording(request) diff --git a/cmvr/client.py b/cmvr/client.py index d4b7c5f..237e391 100644 --- a/cmvr/client.py +++ b/cmvr/client.py @@ -3,7 +3,8 @@ from typing import Dict from .enums import CMVRErrorCode from .biohead_client import BioHeadClient - +from .camera_client import CameraClient +from .dexhand_client import DexHandClient class CMVRGrpcClient: """CMVR gRPC 主客户端""" @@ -20,12 +21,23 @@ class CMVRGrpcClient: self.biohead_stub = None self.biohead_map: Dict[str, BioHeadClient] = {} + self.camera_stub = None + self.camera_map: Dict[str, CameraClient] = {} + + self.dexhand_stub = None + self.dexhand_map: Dict[str, DexHandClient] = {} + # 检查连接状态 try: grpc.channel_ready_future(self.channel).result(timeout=5) self.connected = True self._import_generated() # 在连接成功后才导入 generated self.biohead_stub = self.generated.biohead_service_pb2_grpc.BioHeadServiceStub(self.channel) + + self.camera_stub = self.generated.camera_service_pb2_grpc.CameraServiceStub(self.channel) + + self.dexhand_stub = self.generated.dexhand_service_pb2_grpc.DexHandServiceStub(self.channel) + except grpc.FutureTimeoutError: print(f"连接服务器超时: {server_address}") self.connected = False @@ -36,12 +48,21 @@ class CMVRGrpcClient: try: # 从正确的路径导入生成的模块 from generated.cmvr.api import biohead_service_pb2_grpc + from generated.cmvr.api import camera_service_pb2_grpc from generated.cmvr.api import common_pb2, biohead_command_pb2 + from generated.cmvr.api import camera_command_pb2 + from generated.cmvr.api import dexhand_command_pb2 + from generated.cmvr.api import dexhand_service_pb2_grpc self.generated = type('GeneratedModules', (), { 'biohead_service_pb2_grpc': biohead_service_pb2_grpc, 'common_pb2': common_pb2, - 'biohead_command_pb2': biohead_command_pb2 + 'biohead_command_pb2': biohead_command_pb2, + 'camera_service_pb2_grpc': camera_service_pb2_grpc, + 'camera_command_pb2': camera_command_pb2, + 'dexhand_service_pb2_grpc': dexhand_service_pb2_grpc, + 'dexhand_command_pb2': dexhand_command_pb2 }) + except ImportError as e: print(f"导入生成的模块失败: {e}") print("请确保已生成 protobuf 代码") @@ -57,6 +78,16 @@ class CMVRGrpcClient: if device_id not in self.biohead_map: self.biohead_map[device_id] = BioHeadClient(device_id, self.biohead_stub) return self.biohead_map[device_id] + def get_camera(self, device_id: str) -> CameraClient: + """获取摄像头客户端""" + if device_id not in self.camera_map: + self.camera_map[device_id] = CameraClient(device_id, self.camera_stub) + return self.camera_map[device_id] + + def get_dexhand(self, device_id: str) -> DexHandClient: + if device_id not in self.dexhand_map: + self.dexhand_map[device_id] = DexHandClient(device_id, self.dexhand_stub) + return self.dexhand_map[device_id] def close(self): """关闭所有连接""" diff --git a/cmvr/dexhand_client.py b/cmvr/dexhand_client.py index 72793b7..3a12ba1 100644 --- a/cmvr/dexhand_client.py +++ b/cmvr/dexhand_client.py @@ -1,9 +1,10 @@ import grpc +import numpy as np from typing import Tuple +from typing import Dict -from . import generated -from .enums import CMVRErrorCode -from .models import DexHandState, RH56DFTPDexHand +from .enums import CMVRErrorCode, FingerType +from .models import DexHandState, RH56DFTPDexHand, HandTactileSensors class DexHandClient: """灵巧手客户端""" @@ -11,9 +12,27 @@ class DexHandClient: 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, dexhand_command_pb2 + self.generated = type('GeneratedModules', (), { + 'common_pb2': common_pb2, + 'dexhand_command_pb2': dexhand_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() @@ -22,7 +41,8 @@ class DexHandClient: def get_status(self) -> Tuple[CMVRErrorCode, DexHandState]: """获取灵巧手状态""" try: - request = generated.dexhand_service_pb2.GetDexHandStateCommand.Request() + generated = self._import_generated() + request = generated.dexhand_command_pb2.GetDexHandStateCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.GetStatus(request) @@ -54,36 +74,272 @@ class DexHandClient: print(f"获取灵巧手状态失败: {e}") return CMVRErrorCode.CMVR_RPC_FAILED, DexHandState() - def set_position(self, dof_id: int, value: float) -> CMVRErrorCode: - """设置位置""" + def set_angle(self, angle_map: Dict[int, float]) -> CMVRErrorCode: + """ + 设置DexHand的角度 + + 参数: + angle_map: 字典，键为自由度ID (0-6)，值为角度百分比 (0-1) + 对应proto中的FreedomValue (id和value) + + 返回: + CMVRErrorCode: 操作结果状态码 + """ try: - request = generated.dexhand_service_pb2.SetDexHandPositionsCommand.Request() + generated = self._import_generated() + # 创建角度设置请求对象 + request = generated.dexhand_command_pb2.SetDexHandAnglesCommand.Request() request.header.CopyFrom(self._create_command_header()) - # 添加自由度值 - freedom_value = request.values.add() - freedom_value.id = dof_id - freedom_value.value = value + # 将字典转换为protobuf的FreedomValue列表 + for dof_id, angle_value in angle_map.items(): + # 验证输入有效性 + if not (0 <= dof_id <= 6): + print(f"警告: 无效的自由度ID {dof_id}，必须在0-6范围内") + continue + if not (0 <= angle_value <= 1): + print(f"警告: 角度值 {angle_value} 超出范围，必须在0-1之间") + # 可选：将值限制在有效范围内 + angle_value = max(0, min(1, angle_value)) - response = self.stub.SetDexHandPos(request) + # 添加自由度角度设置 + freedom_value = request.values.add() + freedom_value.id = dof_id + freedom_value.value = angle_value + # 调用RPC接口 + response = self.stub.SetDexHandAngle(request) + + # 返回操作结果 return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED except grpc.RpcError as e: - print(f"设置位置失败: {e}") + print(f"设置DexHand角度失败: {e}") return CMVRErrorCode.CMVR_RPC_FAILED - def set_preset_act(self, preset_act_id: int) -> CMVRErrorCode: - """设置预设动作""" + def get_sensor_data(self) -> Tuple[CMVRErrorCode, HandTactileSensors]: + """ + 获取灵巧手所有触觉传感器数据 + 将Protobuf的SensorData转换为HandTactileSensors对象 + + 返回: + Tuple[CMVRErrorCode, HandTactileSensors]: 错误码和传感器数据对象 + """ try: - request = generated.dexhand_service_pb2.SetDexHandPresetActCommand.Request() + generated = self._import_generated() + # 创建传感器数据请求 + request = generated.dexhand_command_pb2.GetSensorDataCommand.Request() request.header.CopyFrom(self._create_command_header()) - request.presetactid = preset_act_id - response = self.stub.SetDexHandPresetAct(request) + # 调用RPC接口获取数据 + response = self.stub.GetSensorData(request) - return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED + if not response.header.success: + return CMVRErrorCode.CMVR_RPC_FAILED, HandTactileSensors() + + # 初始化传感器数据容器 + tactile_sensors = HandTactileSensors() + + # 映射表：FingerType + PartType -> HandTactileSensors属性 + sensor_mapping = { + # 小拇指 + (FingerType.PINKY, generated.dexhand_command_pb2.SensorData.PartType.TIP): + 'pinky_tip', + (FingerType.PINKY, generated.dexhand_command_pb2.SensorData.PartType.FINGER): + 'pinky_finger', + (FingerType.PINKY, generated.dexhand_command_pb2.SensorData.PartType.PAD): + 'pinky_pad', + + # 无名指 + (FingerType.RING, generated.dexhand_command_pb2.SensorData.PartType.TIP): + 'ring_tip', + (FingerType.RING, generated.dexhand_command_pb2.SensorData.PartType.FINGER): + 'ring_finger', + (FingerType.RING, generated.dexhand_command_pb2.SensorData.PartType.PAD): + 'ring_pad', + + # 中指 + (FingerType.MIDDLE, generated.dexhand_command_pb2.SensorData.PartType.TIP): + 'middle_tip', + (FingerType.MIDDLE, generated.dexhand_command_pb2.SensorData.PartType.FINGER): + 'middle_finger', + (FingerType.MIDDLE, generated.dexhand_command_pb2.SensorData.PartType.PAD): + 'middle_pad', + + # 食指 + (FingerType.INDEX, generated.dexhand_command_pb2.SensorData.PartType.TIP): + 'index_tip', + (FingerType.INDEX, generated.dexhand_command_pb2.SensorData.PartType.FINGER): + 'index_finger', + (FingerType.INDEX, generated.dexhand_command_pb2.SensorData.PartType.PAD): + 'index_pad', + + # 大拇指 + (FingerType.THUMB, generated.dexhand_command_pb2.SensorData.PartType.TIP): + 'thumb_tip', + (FingerType.THUMB, generated.dexhand_command_pb2.SensorData.PartType.FINGER): + 'thumb_finger', + (FingerType.THUMB, generated.dexhand_command_pb2.SensorData.PartType.THUMB_MIDDLE): + 'thumb_middle', + (FingerType.THUMB, generated.dexhand_command_pb2.SensorData.PartType.PAD): + 'thumb_pad', + + # 掌心 + (FingerType.PALM, generated.dexhand_command_pb2.SensorData.PartType.PALM_PAD): + 'palm' + } + + # 遍历所有传感器数据并填充到对应的结构中 + for sensor in response.sensor: + # 获取映射的属性名 + key = (FingerType(sensor.finger_type), sensor.part_type) + attr_name = sensor_mapping.get(key) + + if not attr_name: + # 跳过未定义的传感器类型 + print(f"警告: 未定义的传感器类型 - 手指: {sensor.finger_type}, 部位: {sensor.part_type}") + continue + + # 获取对应的传感器对象 + sensor_obj = getattr(tactile_sensors, attr_name) + + # 更新传感器基本信息 + sensor_obj.name = sensor.sensor_name + sensor_obj.rows = sensor.rows + sensor_obj.cols = sensor.cols + + # 计算字节大小 (每个int32占2字节) + sensor_obj.byteSize = sensor.rows * sensor.cols * 2 + + # 转换数据为numpy数组 + if sensor.rows > 0 and sensor.cols > 0 and len(sensor.data) > 0: + # 初始化数据数组 + data_array = np.zeros((sensor.rows, sensor.cols), dtype=np.uint16) + + # 填充数据 + for row_idx, row_data in enumerate(sensor.data): + if row_idx < sensor.rows: # 防止数组越界 + # 截取有效长度并转换 + values = row_data.values[:sensor.cols] + data_array[row_idx, :len(values)] = values + + sensor_obj.data = data_array + + return CMVRErrorCode.CMVR_SUCCESS, tactile_sensors except grpc.RpcError as e: - print(f"设置预设动作失败: {e}") - return CMVRErrorCode.CMVR_RPC_FAILED \ No newline at end of file + print(f"获取灵巧手传感器数据失败: {e}") + return CMVRErrorCode.CMVR_RPC_FAILED, HandTactileSensors() + + def sensor_data_stream(self): + try: + generated = self._import_generated() + # 创建传感器数据请求 + request = generated.dexhand_command_pb2.GetSensorDataCommand.Request() + request.header.CopyFrom(self._create_command_header()) + + # 调用RPC接口获取数据 + response = self.stub.GetSensorData(request) + + if not response.header.success: + return CMVRErrorCode.CMVR_RPC_FAILED, HandTactileSensors() + except grpc.RpcError as e: + print(f"获取灵巧手传感器数据失败: {e}") + + def create_stream_request(self): + """创建流请求（无参数）""" + generated = self._import_generated() + request = generated.dexhand_command_pb2.GetSensorDataStreamCommand.Request() + request.header.CopyFrom(self._create_command_header()) + # 不需要设置其他参数 + return request + + def get_sensor_data_stream(self, request_generator): + """ + 双向流获取传感器数据 + 参数: + request_generator: 请求生成器 + 返回: + 传感器数据流迭代器 + """ + generated = self._import_generated() + return self.stub.GetSensorDataStream(request_generator) + + def parse_sensor_stream_data(self, feedback) -> Tuple[CMVRErrorCode, HandTactileSensors]: + """ + 解析流反馈中的传感器数据 + 复用之前的传感器数据转换逻辑 + """ + try: + generated = self._import_generated() + SensorData = generated.dexhand_command_pb2.SensorData + pb_finger_type = SensorData.FingerType + pb_part_type = SensorData.PartType + + # 初始化传感器容器 + tactile_sensors = HandTactileSensors() + + # 传感器映射表（与get_sensor_data保持一致） + sensor_mapping = { + # 小拇指 + (pb_finger_type.PINKY, pb_part_type.TIP): 'pinky_tip', + (pb_finger_type.PINKY, pb_part_type.FINGER): 'pinky_finger', + (pb_finger_type.PINKY, pb_part_type.PAD): 'pinky_pad', + + # 无名指 + (pb_finger_type.RING, pb_part_type.TIP): 'ring_tip', + (pb_finger_type.RING, pb_part_type.FINGER): 'ring_finger', + (pb_finger_type.RING, pb_part_type.PAD): 'ring_pad', + + # 中指 + (pb_finger_type.MIDDLE_FINGER, pb_part_type.TIP): 'middle_tip', + (pb_finger_type.MIDDLE_FINGER, pb_part_type.FINGER): 'middle_finger', + (pb_finger_type.MIDDLE_FINGER, pb_part_type.PAD): 'middle_pad', + + # 食指 + (pb_finger_type.INDEX, pb_part_type.TIP): 'index_tip', + (pb_finger_type.INDEX, pb_part_type.FINGER): 'index_finger', + (pb_finger_type.INDEX, pb_part_type.PAD): 'index_pad', + + # 大拇指 + (pb_finger_type.THUMB, pb_part_type.TIP): 'thumb_tip', + (pb_finger_type.THUMB, pb_part_type.FINGER): 'thumb_finger', + (pb_finger_type.THUMB, pb_part_type.THUMB_MIDDLE): 'thumb_middle', + (pb_finger_type.THUMB, pb_part_type.PAD): 'thumb_pad', + + # 掌心 + (pb_finger_type.PALM, pb_part_type.PALM_PAD): 'palm' + } + + # 解析反馈中的传感器数据 + for sensor in feedback.sensor: # 假设反馈中的传感器数据字段是sensor_data + key = (sensor.finger_type, sensor.part_type) + attr_name = sensor_mapping.get(key) + + if not attr_name: + finger_name = pb_finger_type.Name(sensor.finger_type) + part_name = pb_part_type.Name(sensor.part_type) + print(f"警告: 未定义的传感器类型 - 手指: {finger_name}, 部位: {part_name}") + continue + + # 填充传感器数据 + sensor_obj = getattr(tactile_sensors, attr_name) + sensor_obj.name = sensor.sensor_name + sensor_obj.rows = sensor.rows + sensor_obj.cols = sensor.cols + sensor_obj.byteSize = sensor.rows * sensor.cols * 2 + + # 转换数据为numpy数组 + if sensor.rows > 0 and sensor.cols > 0 and len(sensor.data) > 0: + data_array = np.zeros((sensor.rows, sensor.cols), dtype=np.uint16) + for row_idx, row_data in enumerate(sensor.data): + if row_idx < sensor.rows: + values = row_data.values[:sensor.cols] + data_array[row_idx, :len(values)] = values + sensor_obj.data = data_array + + return CMVRErrorCode.CMVR_SUCCESS, tactile_sensors + + except Exception as e: + print(f"解析流数据失败: {e}") + return CMVRErrorCode.CMVR_INTERNAL_ERROR, HandTactileSensors() diff --git a/cmvr/enums.py b/cmvr/enums.py index 6aee693..9401979 100644 --- a/cmvr/enums.py +++ b/cmvr/enums.py @@ -24,4 +24,5 @@ class FingerType(Enum): RING = 1 # 无名指 MIDDLE = 2 # 中指 INDEX = 3 # 食指 - THUMB = 4 # 大拇指 \ No newline at end of file + THUMB = 4 # 大拇指 + PALM = 5 # 掌心 \ No newline at end of file diff --git a/cmvr/micphone_client.py b/cmvr/micphone_client.py index 2f2dab2..998ad5d 100644 --- a/cmvr/micphone_client.py +++ b/cmvr/micphone_client.py @@ -1,7 +1,6 @@ import grpc from typing import Tuple -from . import generated from .enums import CMVRErrorCode from .models import MicState @@ -11,9 +10,27 @@ class MicphoneClient: 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, microphone_command_pb2 + self.generated = type('GeneratedModules', (), { + 'common_pb2': common_pb2, + 'microphone_command_pb2': microphone_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() @@ -22,7 +39,8 @@ class MicphoneClient: def get_status(self) -> Tuple[CMVRErrorCode, MicState]: """获取麦克风状态""" try: - request = generated.microphone_service_pb2.GetMicStateCommand.Request() + generated = self._import_generated() + request = generated.microphone_command_pb2.GetMicStateCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.GetStatus(request) @@ -47,7 +65,8 @@ class MicphoneClient: def start_record(self, audio_path: str) -> CMVRErrorCode: """开始录音""" try: - request = generated.microphone_service_pb2.StartMicRecordingCommand.Request() + generated = self._import_generated() + request = generated.microphone_command_pb2.StartMicRecordingCommand.Request() request.header.CopyFrom(self._create_command_header()) request.file_path = audio_path @@ -62,7 +81,8 @@ class MicphoneClient: def stop_record(self) -> CMVRErrorCode: """停止录音""" try: - request = generated.microphone_service_pb2.StopMicRecordingCommand.Request() + generated = self._import_generated() + request = generated.microphone_command_pb2.StopMicRecordingCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.StopRecord(request) diff --git a/cmvr/models.py b/cmvr/models.py index 5b7d1f4..c6f9c74 100644 --- a/cmvr/models.py +++ b/cmvr/models.py @@ -34,6 +34,7 @@ class CameraState: @dataclass class RH56DFTPDexHand: """灵巧手自由度状态""" + dof_id: int = 0 angle: int = 0 speed: int = 0 force: int = 0 @@ -108,7 +109,7 @@ class FingerTactileData: cols: int = 0 byteSize: int = 0 name: str = "" - + def __post_init__(self): if self.data is None and self.rows > 0 and self.cols > 0: self.data = np.zeros((self.rows, self.cols), dtype=np.uint16) @@ -121,7 +122,7 @@ class PalmTactileData: cols: int = 14 byteSize: int = 224 name: str = "掌心" - + def __post_init__(self): if self.data is None: self.data = np.zeros((self.rows, self.cols), dtype=np.uint16) @@ -132,61 +133,61 @@ class HandTactileSensors: pinky_tip: FingerTactileData = None pinky_finger: FingerTactileData = None pinky_pad: FingerTactileData = None - + ring_tip: FingerTactileData = None ring_finger: FingerTactileData = None ring_pad: FingerTactileData = None - + middle_tip: FingerTactileData = None middle_finger: FingerTactileData = None middle_pad: FingerTactileData = None - + index_tip: FingerTactileData = None index_finger: FingerTactileData = None index_pad: FingerTactileData = None - + thumb_tip: FingerTactileData = None thumb_finger: FingerTactileData = None thumb_middle: FingerTactileData = None thumb_pad: FingerTactileData = None - + palm: PalmTactileData = None - + def __post_init__(self): """初始化所有传感器数据""" # 小拇指 self.pinky_tip = FingerTactileData(rows=3, cols=3, byteSize=18, name="小拇指指端") self.pinky_finger = FingerTactileData(rows=12, cols=8, byteSize=192, name="小拇指指尖") self.pinky_pad = FingerTactileData(rows=10, cols=8, byteSize=160, name="小拇指指腹") - + # 无名指 self.ring_tip = FingerTactileData(rows=3, cols=3, byteSize=18, name="无名指指端") self.ring_finger = FingerTactileData(rows=12, cols=8, byteSize=192, name="无名指指尖") self.ring_pad = FingerTactileData(rows=10, cols=8, byteSize=160, name="无名指指腹") - + # 中指 self.middle_tip = FingerTactileData(rows=3, cols=3, byteSize=18, name="中指指端") self.middle_finger = FingerTactileData(rows=12, cols=8, byteSize=192, name="中指指尖") self.middle_pad = FingerTactileData(rows=10, cols=8, byteSize=160, name="中指指腹") - + # 食指 self.index_tip = FingerTactileData(rows=3, cols=3, byteSize=18, name="食指指端") self.index_finger = FingerTactileData(rows=12, cols=8, byteSize=192, name="食指指尖") self.index_pad = FingerTactileData(rows=10, cols=8, byteSize=160, name="食指指腹") - + # 大拇指 self.thumb_tip = FingerTactileData(rows=3, cols=3, byteSize=18, name="大拇指指端") self.thumb_finger = FingerTactileData(rows=12, cols=8, byteSize=192, name="大拇指尖") self.thumb_middle = FingerTactileData(rows=3, cols=3, byteSize=18, name="大拇指指中") self.thumb_pad = FingerTactileData(rows=12, cols=8, byteSize=192, name="大拇指指腹") - + # 掌心 self.palm = PalmTactileData() - + def get_finger_name(self, finger_type): """获取手指名称""" from .enums import FingerType - + names = { FingerType.PINKY: "小拇指", FingerType.RING: "无名指", @@ -194,4 +195,13 @@ class HandTactileSensors: FingerType.INDEX: "食指", FingerType.THUMB: "大拇指" } - return names.get(finger_type, "未知") \ No newline at end of file + return names.get(finger_type, "未知") + def print_summary(self): + """打印传感器数据摘要信息""" + print(f" 小拇指指端: {self.pinky_tip.rows}x{self.pinky_tip.cols} (数据大小: {self.pinky_tip.byteSize} bytes)") + print(f" 无名指指端: {self.ring_tip.rows}x{self.ring_tip.cols} (数据大小: {self.ring_tip.byteSize} bytes)") + print(f" 中指指端: {self.middle_tip.rows}x{self.middle_tip.cols} (数据大小: {self.middle_tip.byteSize} bytes)") + print(f" 食指指端: {self.index_tip.rows}x{self.index_tip.cols} (数据大小: {self.index_tip.byteSize} bytes)") + print(f" 大拇指指端: {self.thumb_tip.rows}x{self.thumb_tip.cols} (数据大小: {self.thumb_tip.byteSize} bytes)") + print(f" 大拇指指中: {self.thumb_middle.rows}x{self.thumb_middle.cols} (数据大小: {self.thumb_middle.byteSize} bytes)") + print(f" 掌心: {self.palm.rows}x{self.palm.cols} (数据大小: {self.palm.byteSize} bytes)") \ No newline at end of file diff --git a/cmvr/speaker_client.py b/cmvr/speaker_client.py index b7aa18a..2c4e8fe 100644 --- a/cmvr/speaker_client.py +++ b/cmvr/speaker_client.py @@ -1,7 +1,6 @@ import grpc from typing import Tuple -from . import generated from .enums import CMVRErrorCode from .models import SpeakerState @@ -11,9 +10,27 @@ class SpeakerClient: 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, speaker_command_pb2 + self.generated = type('GeneratedModules', (), { + 'common_pb2': common_pb2, + 'speaker_command_pb2': speaker_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() @@ -22,7 +39,8 @@ class SpeakerClient: def get_status(self) -> Tuple[CMVRErrorCode, SpeakerState]: """获取扬声器状态""" try: - request = generated.speaker_service_pb2.GetSpeakerStateCommand.Request() + generated = self._import_generated() + request = generated.speaker_command_pb2.GetSpeakerStateCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.GetStatus(request) @@ -47,7 +65,8 @@ class SpeakerClient: def play_audio(self, audio_path: str) -> CMVRErrorCode: """播放音频""" try: - request = generated.speaker_service_pb2.PlayAudioCommand.Request() + generated = self._import_generated() + request = generated.speaker_command_pb2.PlayAudioCommand.Request() request.header.CopyFrom(self._create_command_header()) request.audio_path = audio_path @@ -62,7 +81,8 @@ class SpeakerClient: def pause_audio(self) -> CMVRErrorCode: """暂停播放""" try: - request = generated.speaker_service_pb2.PauseSpeakerCommand.Request() + generated = self._import_generated() + request = generated.speaker_command_pb2.PauseSpeakerCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.PausePlayback(request) @@ -76,7 +96,8 @@ class SpeakerClient: def resume_audio(self) -> CMVRErrorCode: """继续播放""" try: - request = generated.speaker_service_pb2.ResumeSpeakerCommand.Request() + generated = self._import_generated() + request = generated.speaker_command_pb2.ResumeSpeakerCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.ResumePlayback(request) @@ -90,7 +111,8 @@ class SpeakerClient: def stop_audio(self) -> CMVRErrorCode: """停止播放""" try: - request = generated.speaker_service_pb2.StopSpeakerCommand.Request() + generated = self._import_generated() + request = generated.speaker_command_pb2.StopSpeakerCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.StopPlayback(request) @@ -104,7 +126,8 @@ class SpeakerClient: def set_volume(self, volume: float) -> CMVRErrorCode: """设置音量""" try: - request = generated.speaker_service_pb2.SetSpeakerVolumeCommand.Request() + generated = self._import_generated() + request = generated.speaker_command_pb2.SetSpeakerVolumeCommand.Request() request.header.CopyFrom(self._create_command_header()) request.volume = volume @@ -119,7 +142,8 @@ class SpeakerClient: def get_volume(self) -> Tuple[CMVRErrorCode, float]: """获取音量""" try: - request = generated.speaker_service_pb2.GetSpeakerVolumeCommand.Request() + generated = self._import_generated() + request = generated.speaker_command_pb2.GetSpeakerVolumeCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.GetVolume(request) diff --git a/example_usage.py b/example_usage.py index f6f0f67..ea2b0a4 100644 --- a/example_usage.py +++ b/example_usage.py @@ -3,7 +3,10 @@ import sys import os import time -import random # 导入 random 模块 +import random +import grpc # 新增grpc导入 +from typing import Generator # 新增Generator类型导入 +import numpy as np # 确保导入numpy（传感器数据处理需要） # 添加项目根目录到 Python 路径 sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) @@ -131,5 +134,338 @@ def test_biohead(): client.close() +def test_camera(): + """测试相机功能""" + print("=== 测试相机功能 ===") + + # 初始化客户端 + client = CMVRGrpcClient("192.168.1.6:50052") + if not client.is_connected(): + print("连接服务器失败") + return + + # 获取相机客户端 + camera = client.get_camera("cam4") # 假设设备ID为"cam4" + + # 获取相机初始状态 + error_code, state = camera.get_status() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print(f"相机初始状态: 已初始化={state.is_initialized}, 已打开={state.is_opened}, " + f"已流传输={state.is_streaming}, 已录制={state.is_recording}, " + f"分辨率={state.width}x{state.height}, FPS={state.fps}") + else: + print(f"获取相机状态失败，错误码: {error_code}") + client.close() + return + + # 启动相机 + start_result = camera.start_camera() + if start_result == CMVRErrorCode.CMVR_SUCCESS: + print("相机启动成功") + else: + print(f"相机启动失败，错误码: {start_result}") + client.close() + return + + # 等待相机启动 + time.sleep(1) + + # 再次获取相机状态确认 + error_code, state = camera.get_status() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print(f"启动后状态: 已打开={state.is_opened}, 已流传输={state.is_streaming}") + else: + print(f"获取相机状态失败，错误码: {error_code}") + + # 测试获取RGB图像并保存 + print("开始获取图像并保存 (3次)...") + for i in range(3): + error_code, img_array, width, height = camera.get_rgb_image() + if error_code == CMVRErrorCode.CMVR_SUCCESS and img_array.size > 0: + # 生成保存路径（当前目录） + img_filename = f"camera_test_image_{i+1}_{int(time.time())}.jpg" + # 转换颜色格式（如果需要） + if len(img_array.shape) == 3: + cv2_img = cv2.cvtColor(img_array, cv2.COLOR_RGB2BGR) + else: + cv2_img = img_array + # 保存图像 + cv2.imwrite(img_filename, cv2_img) + print(f"第{i+1}次获取图像成功，已保存至: {img_filename}，分辨率: {width}x{height}") + else: + print(f"第{i+1}次获取图像失败，错误码: {error_code}") + time.sleep(1) + + # 测试录像功能 + video_path = f"test_recording_{int(time.time())}.mp4" + print(f"开始录像，保存路径: {os.path.abspath(video_path)}") # 显示绝对路径 + record_start_result = camera.start_record(video_path) + if record_start_result == CMVRErrorCode.CMVR_SUCCESS: + print("录像开始成功，录制5秒...") + time.sleep(5) + + # 停止录像 + record_stop_result = camera.stop_record() + if record_stop_result == CMVRErrorCode.CMVR_SUCCESS: + print("录像停止成功") + else: + print(f"录像停止失败，错误码: {record_stop_result}") + else: + print(f"录像开始失败，错误码: {record_start_result}") + + # 停止相机 + stop_result = camera.stop_camera() + if stop_result == CMVRErrorCode.CMVR_SUCCESS: + print("相机停止成功") + else: + print(f"相机停止失败，错误码: {stop_result}") + + # 最终状态确认 + error_code, state = camera.get_status() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print(f"最终状态: 已打开={state.is_opened}, 已流传输={state.is_streaming}") + else: + print(f"获取相机状态失败，错误码: {error_code}") + + client.close() + +def test_dexhand(): + """测试DexHand功能（获取状态、设置角度和传感器数据）""" + print("=== 测试DexHand功能 ===") + + # 初始化GRPC客户端（使用实际服务器地址） + client = CMVRGrpcClient("192.168.1.6:50052") + if not client.is_connected(): + print("连接服务器失败") + return + + # 获取DexHand客户端（假设设备ID为"hand1"，根据实际设备ID修改） + dexhand = client.get_dexhand("hand1") + if not dexhand: + print("获取DexHand客户端失败") + client.close() + return + + # 1. 获取初始状态 + print("\n--- 获取初始状态 ---") + error_code, state = dexhand.get_status() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print(f"初始化状态: 已初始化={state.is_initialized}") + print("各自由度状态:") + for hand in state.hands: + print(f" 自由度ID: {hand.dof_id}, 角度: {hand.angle}, 速度: {hand.speed}, " + f"受力: {hand.force}, 位置: {hand.position}, 温度: {hand.temperature}°C") + if hand.error != 0: + print(f" 警告: 自由度ID {hand.dof_id} 存在故障: {hand.error_message}") + else: + print(f"获取初始状态失败，错误码: {error_code}") + client.close() + return + + # 1.1 获取初始传感器数据 + print("\n--- 获取初始传感器数据 ---") + error_code, sensors = dexhand.get_sensor_data() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print("初始传感器数据摘要:") + sensors.print_summary() # 调用传感器数据摘要打印方法 + # 打印掌心和食指指端的传感器数据示例 + print(f" 掌心传感器数据形状: {sensors.palm.data.shape}") + print(f" 食指指端传感器数据形状: {sensors.index_tip.data.shape}") + else: + print(f"获取初始传感器数据失败，错误码: {error_code}") + + # 2. 设置测试角度（小拇指、食指、大拇指弯曲） + print("\n--- 设置测试角度 ---") + # 角度字典: 键为自由度ID (0-6)，值为0-1百分比 + test_angles = { + 0: 0.9, # 小拇指 + 1: 0.9, # 无名指 + 2: 0.9, # 中指 + 3: 0.9, # 食指 + 4: 0.9, # 大拇指弯曲 + 5: 0.9 # 大拇指旋转 + } + + # 打印设置信息（映射自由度ID到手指名称） + finger_map = { + 0: "小拇指", + 1: "无名指", + 2: "中指", + 3: "食指", + 4: "大拇指弯曲", + 5: "大拇指旋转", + 6: "预留自由度" + } + print("准备设置角度:") + for dof_id, value in test_angles.items(): + finger_name = finger_map.get(dof_id, f"自由度{dof_id}") + print(f" {finger_name} (ID:{dof_id}): {value*100}%") + + # 执行角度设置 + set_result = dexhand.set_angle(test_angles) + if set_result == CMVRErrorCode.CMVR_SUCCESS: + print("角度设置命令发送成功，等待执行器动作...") + time.sleep(2) # 等待执行器完成动作 + else: + print(f"角度设置失败，错误码: {set_result}") + client.close() + return + + # 2.1 获取角度变化后的传感器数据 + print("\n--- 获取角度变化后传感器数据 ---") + error_code, sensors_after = dexhand.get_sensor_data() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print("角度变化后传感器数据摘要:") + # 对比食指指腹在动作前后的平均压力变化 + if hasattr(sensors, 'index_pad') and hasattr(sensors_after, 'index_pad'): + avg_before = sensors.index_pad.data.mean() if sensors.index_pad.data.size > 0 else 0 + avg_after = sensors_after.index_pad.data.mean() if sensors_after.index_pad.data.size > 0 else 0 + print(f" 食指指腹平均压力变化: {avg_before:.2f} → {avg_after:.2f}") + + # 打印大拇指指端数据示例（前3个值） + if sensors_after.thumb_tip.data.size > 0: + print(f" 大拇指指端部分数据: {sensors_after.thumb_tip.data.flatten()[:3]}...") + else: + print(f"获取角度变化后传感器数据失败，错误码: {error_code}") + + # 3. 获取设置后的状态 + print("\n--- 获取设置后状态 ---") + error_code, state = dexhand.get_status() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print("设置后各自由度状态:") + for hand in state.hands: + # 只打印我们设置过的自由度 + if hand.dof_id in test_angles: + print(f" 自由度ID: {hand.dof_id}, 新角度: {hand.angle}, 当前位置: {hand.position}") + else: + print(f"获取设置后状态失败，错误码: {error_code}") + + # 4. 恢复默认角度（复位操作） + print("\n--- 恢复默认角度 ---") + default_angles = { + 0: 0.1, + 1: 0.1, + 2: 0.1, + 3: 0.1, + 4: 0.1, + 5: 0.8 + } + + set_result = dexhand.set_angle(default_angles) + if set_result == CMVRErrorCode.CMVR_SUCCESS: + print("默认角度恢复成功，等待复位完成...") + time.sleep(2) + else: + print(f"默认角度恢复失败，错误码: {set_result}") + + # 4.1 获取复位后的传感器数据 + print("\n--- 获取复位后传感器数据 ---") + error_code, sensors_reset = dexhand.get_sensor_data() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print("复位后传感器数据摘要:") + # 对比掌心压力在复位前后的变化 + if hasattr(sensors_after, 'palm') and hasattr(sensors_reset, 'palm'): + avg_after = sensors_after.palm.data.mean() if sensors_after.palm.data.size > 0 else 0 + avg_reset = sensors_reset.palm.data.mean() if sensors_reset.palm.data.size > 0 else 0 + print(f" 掌心平均压力变化: {avg_after:.2f} → {avg_reset:.2f}") + else: + print(f"获取复位后传感器数据失败，错误码: {error_code}") + + # 5. 最终状态确认 + print("\n--- 最终状态确认 ---") + error_code, state = dexhand.get_status() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print(f"最终初始化状态: {state.is_initialized}") + print("关键自由度最终位置:") + for dof_id in test_angles.keys(): + for hand in state.hands: + if hand.dof_id == dof_id: + print(f" 自由度ID {dof_id}: 角度={hand.angle}, 位置={hand.position}") + else: + print(f"获取最终状态失败，错误码: {error_code}") + + # 关闭连接 + client.close() + print("\n=== DexHand测试完成 ===") + +def test_sensor_data_stream(): + """测试双向流传感器数据接口""" + print("=== 测试双向流传感器数据接口 ===") + + # 初始化GRPC客户端 + client = CMVRGrpcClient("192.168.1.222:50052") + if not client.is_connected(): + print("连接服务器失败") + return + + # 获取DexHand客户端 + dexhand = client.get_dexhand("hand1") + if not dexhand: + print("获取DexHand客户端失败") + client.close() + return + + try: + # 1. 启动传感器数据流 + print("\n--- 启动传感器数据流 ---") + # 创建请求生成器（双向流不需要参数，发送空请求） + def request_generator() -> Generator: + # 发送初始请求启动流 + yield dexhand.create_stream_request() + # 保持流连接，每5秒发送一次心跳（可选） + try: + while True: + time.sleep(5) + # 发送空请求保持连接 + yield dexhand.create_stream_request() + except GeneratorExit: + print("请求生成器已关闭") + return + + # 2. 接收并处理流数据 + print("开始接收传感器流数据（5秒后自动停止）...") + start_time = time.time() + stream_duration = 15 # 接收15秒数据 + + # 调用双向流接口 + for feedback in dexhand.get_sensor_data_stream(request_generator()): + # 检查是否超时 + if time.time() - start_time > stream_duration: + print("\n达到预设接收时间，停止接收") + break + + # # 检查反馈状态 + # if not feedback.header.success: + # print(f"数据流接收失败，错误码: {feedback.header.error_code}") + # continue + + # 处理传感器数据（转换为HandTactileSensors对象） + error_code, sensors = dexhand.parse_sensor_stream_data(feedback) + if error_code != CMVRErrorCode.CMVR_SUCCESS: + print("传感器数据解析失败") + continue + + # 打印传感器数据摘要（每秒打印一次） + if int(time.time()) % 1 == 0: # 控制打印频率 + print(f"\n--- 传感器数据 (时间: {time.time() - start_time:.2f}s) ---") + sensors.print_summary() + + # 打印食指指端的实时数据示例 + if sensors.index_tip.data is not None and sensors.index_tip.data.size > 0: + print(f"食指指端平均压力: {sensors.index_tip.data.mean():.2f}") + if sensors.palm.data is not None and sensors.palm.data.size > 0: + print(f"掌心平均压力: {sensors.palm.data.mean():.2f}") + + except grpc.RpcError as e: + print(f"双向流通信错误: {e}") + except Exception as e: + print(f"处理流数据时发生错误: {e}") + finally: + # 关闭连接 + client.close() + print("\n=== 双向流传感器数据测试完成 ===") if __name__ == "__main__": - test_biohead() + # test_biohead() + # test_camera() + # test_dexhand() + test_sensor_data_stream()