472 lines
19 KiB
Python
472 lines
19 KiB
Python
#!/usr/bin/env python3
|
||
|
||
import sys
|
||
import os
|
||
import time
|
||
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__)))
|
||
|
||
# 直接导入生成的模块
|
||
from generated.cmvr.api import biohead_command_pb2
|
||
from cmvr import CMVRGrpcClient, CMVRErrorCode, FacialExpressionState
|
||
|
||
def test_biohead():
|
||
"""测试仿生头功能"""
|
||
print("=== 测试仿生头功能 ===")
|
||
|
||
# 初始化客户端
|
||
client = CMVRGrpcClient("192.168.1.6:50051")
|
||
if not client.is_connected():
|
||
print("连接服务器失败")
|
||
return
|
||
|
||
# 获取仿生头
|
||
biohead = client.get_biohead("bio_head")
|
||
|
||
# 创建表情
|
||
expression = FacialExpressionState()
|
||
|
||
# 设置眉毛，使用随机数并打印
|
||
expression.left_eyebrow_outside_y = random.uniform(0, 1)
|
||
expression.left_eyebrow_inside_y = random.uniform(0, 1)
|
||
expression.right_eyebrow_outside_y = random.uniform(0, 1)
|
||
expression.right_eyebrow_inside_y = random.uniform(0, 1)
|
||
print(f"眉毛设置: 左外 {expression.left_eyebrow_outside_y}, 左内 {expression.left_eyebrow_inside_y}, 右外 {expression.right_eyebrow_outside_y}, 右内 {expression.right_eyebrow_inside_y}")
|
||
|
||
# 设置眼睑，使用随机数并打印
|
||
expression.left_eye_upper_lid_y = random.uniform(0, 1)
|
||
expression.left_eye_lower_lid_y = random.uniform(0, 1)
|
||
expression.right_eye_upper_lid_y = random.uniform(0, 1)
|
||
expression.right_eye_lower_lid_y = random.uniform(0, 1)
|
||
print(f"眼睑设置: 左上 {expression.left_eye_upper_lid_y}, 左下 {expression.left_eye_lower_lid_y}, 右上 {expression.right_eye_upper_lid_y}, 右下 {expression.right_eye_lower_lid_y}")
|
||
|
||
# 设置眼球，使用随机数并打印
|
||
expression.left_eye_ball_x = random.uniform(0, 1)
|
||
expression.left_eye_ball_y = random.uniform(0, 1)
|
||
expression.right_eye_ball_x = random.uniform(0, 1)
|
||
expression.right_eye_ball_y = random.uniform(0, 1)
|
||
print(f"眼球设置: 左X {expression.left_eye_ball_x}, 左Y {expression.left_eye_ball_y}, 右X {expression.right_eye_ball_x}, 右Y {expression.right_eye_ball_y}")
|
||
|
||
# 设置嘴巴，使用随机数并打印
|
||
expression.upper_lip_y = random.uniform(0, 1)
|
||
expression.lower_lip_y = random.uniform(0, 1)
|
||
print(f"嘴巴设置: 上唇 {expression.upper_lip_y}, 下唇 {expression.lower_lip_y}")
|
||
|
||
# 设置下巴，使用随机数并打印
|
||
expression.jaw_x = random.uniform(0, 1)
|
||
expression.jaw_y = random.uniform(0, 1)
|
||
print(f"下巴设置: X {expression.jaw_x}, Y {expression.jaw_y}")
|
||
|
||
# 设置表情
|
||
result = biohead.set_expression(expression)
|
||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
||
print("设置表情成功")
|
||
else:
|
||
print(f"设置表情失败，错误码: {result}")
|
||
|
||
# 测试流式控制
|
||
print("开始流式控制表情 (5次眨眼)...")
|
||
result = biohead.start_stream()
|
||
if result != CMVRErrorCode.CMVR_SUCCESS:
|
||
print(f"开始流式会话失败，错误码: {result}")
|
||
else:
|
||
for i in range(5):
|
||
# 创建 protobuf 消息用于流式传输
|
||
proto_expr = biohead_command_pb2.FacialExpression()
|
||
|
||
# 睁眼，使用随机数并打印
|
||
proto_expr.eyelid.left_upper_y = random.uniform(0, 1)
|
||
proto_expr.eyelid.left_lower_y = random.uniform(0, 1)
|
||
proto_expr.eyelid.right_upper_y = random.uniform(0, 1)
|
||
proto_expr.eyelid.right_lower_y = random.uniform(0, 1)
|
||
print(f"第{i+1}次流式睁眼设置: 左上 {proto_expr.eyelid.left_upper_y}, 左下 {proto_expr.eyelid.left_lower_y}, 右上 {proto_expr.eyelid.right_upper_y}, 右下 {proto_expr.eyelid.right_lower_y}")
|
||
|
||
result = biohead.stream_expression(proto_expr)
|
||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
||
print(f"第{i+1}次流式睁眼成功")
|
||
else:
|
||
print(f"第{i+1}次流式睁眼失败，错误码: {result}")
|
||
|
||
time.sleep(0.5)
|
||
|
||
# 闭眼，使用随机数并打印
|
||
proto_expr.eyelid.left_upper_y = random.uniform(0, 1)
|
||
proto_expr.eyelid.left_lower_y = random.uniform(0, 1)
|
||
proto_expr.eyelid.right_upper_y = random.uniform(0, 1)
|
||
proto_expr.eyelid.right_lower_y = random.uniform(0, 1)
|
||
print(f"第{i+1}次流式闭眼设置: 左上 {proto_expr.eyelid.left_upper_y}, 左下 {proto_expr.eyelid.left_lower_y}, 右上 {proto_expr.eyelid.right_upper_y}, 右下 {proto_expr.eyelid.right_lower_y}")
|
||
|
||
result = biohead.stream_expression(proto_expr)
|
||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
||
print(f"第{i+1}次流式闭眼成功")
|
||
else:
|
||
print(f"第{i+1}次流式闭眼失败，错误码: {result}")
|
||
|
||
time.sleep(0.2)
|
||
|
||
# 结束流式会话
|
||
result = biohead.end_stream()
|
||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
||
print("结束流式会话成功")
|
||
else:
|
||
print(f"结束流式会话失败，错误码: {result}")
|
||
|
||
print("流式控制完成")
|
||
|
||
# 获取系统状态
|
||
result = biohead.get_system_status()
|
||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
||
print("获取系统状态成功")
|
||
else:
|
||
print(f"获取系统状态失败，错误码: {result}")
|
||
|
||
# 测试紧急停止
|
||
result = biohead.emergency_stop()
|
||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
||
print("紧急停止成功")
|
||
else:
|
||
print(f"紧急停止失败，错误码: {result}")
|
||
|
||
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_camera()
|
||
# test_dexhand()
|
||
test_sensor_data_stream()
|