Compare commits
No commits in common. "python" and "main" have entirely different histories.
@ -15,14 +15,14 @@ Already a pro? Just edit this README.md and make it your own. Want to make it ea
|
|||||||
|
|
||||||
```
|
```
|
||||||
cd existing_repo
|
cd existing_repo
|
||||||
git remote add origin http://10.148.20.36:18088/smart_bench/cmvr-es-cli.git
|
git remote add origin http://192.168.1.100:18088/smart_bench/cmvr-es-cli.git
|
||||||
git branch -M main
|
git branch -M main
|
||||||
git push -uf origin main
|
git push -uf origin main
|
||||||
```
|
```
|
||||||
|
|
||||||
## Integrate with your tools
|
## Integrate with your tools
|
||||||
|
|
||||||
- [ ] [Set up project integrations](http://10.148.20.36:18088/smart_bench/cmvr-es-cli/-/settings/integrations)
|
- [ ] [Set up project integrations](http://192.168.1.100:18088/smart_bench/cmvr-es-cli/-/settings/integrations)
|
||||||
|
|
||||||
## Collaborate with your team
|
## Collaborate with your team
|
||||||
|
|
||||||
|
|||||||
557
biohead.py
557
biohead.py
@ -1,557 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
# -*- coding: utf-8 -*-
|
|
||||||
"""
|
|
||||||
biohead.py — 基于gRPC的仿生头控制脚本
|
|
||||||
功能：
|
|
||||||
1. 控制单个表情参数
|
|
||||||
2. 控制多个表情参数
|
|
||||||
3. 退出
|
|
||||||
4. 自动增减模式：从0到1再到0循环变化
|
|
||||||
5. 设置自动模式参数数量
|
|
||||||
6. 眨眼模式：执行预设的眨眼动作
|
|
||||||
7. 打哈欠模式：模拟打哈欠动作
|
|
||||||
8. 说话模式：模拟嘴巴开合动作
|
|
||||||
"""
|
|
||||||
import sys
|
|
||||||
import time
|
|
||||||
import random
|
|
||||||
import os
|
|
||||||
import grpc
|
|
||||||
from typing import Generator
|
|
||||||
|
|
||||||
# 添加项目根目录到Python路径
|
|
||||||
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
|
|
||||||
|
|
||||||
# 导入gRPC相关模块
|
|
||||||
from generated.cmvr.api import biohead_command_pb2
|
|
||||||
from cmvr import CMVRGrpcClient, CMVRErrorCode, FacialExpressionState
|
|
||||||
|
|
||||||
def angle_to_normalized(angle):
|
|
||||||
"""将角度值(0-180)转换为归一化值(0-1)"""
|
|
||||||
return max(0.0, min(1.0, angle / 180.0))
|
|
||||||
|
|
||||||
def normalized_to_angle(normalized):
|
|
||||||
"""将归一化值(0-1)转换为角度值(0-180)"""
|
|
||||||
return int(normalized * 180)
|
|
||||||
|
|
||||||
def init_biohead_client(server_addr="192.168.1.222:50051"):
|
|
||||||
"""初始化仿生头gRPC客户端"""
|
|
||||||
client = CMVRGrpcClient(server_addr)
|
|
||||||
if not client.is_connected():
|
|
||||||
print("连接服务器失败")
|
|
||||||
return None
|
|
||||||
|
|
||||||
biohead = client.get_biohead("bio_head")
|
|
||||||
if not biohead:
|
|
||||||
print("获取仿生头客户端失败")
|
|
||||||
client.close()
|
|
||||||
return None
|
|
||||||
|
|
||||||
return client, biohead
|
|
||||||
|
|
||||||
def set_single_expression(biohead):
|
|
||||||
"""设置单个表情参数"""
|
|
||||||
print("\n--- 设置单个表情参数 ---")
|
|
||||||
# 表情参数映射表
|
|
||||||
param_map = {
|
|
||||||
1: "left_eyebrow_outside_y",
|
|
||||||
2: "left_eyebrow_inside_y",
|
|
||||||
3: "right_eyebrow_outside_y",
|
|
||||||
4: "right_eyebrow_inside_y",
|
|
||||||
5: "left_eye_upper_lid_y",
|
|
||||||
6: "left_eye_lower_lid_y",
|
|
||||||
7: "right_eye_upper_lid_y",
|
|
||||||
8: "right_eye_lower_lid_y",
|
|
||||||
9: "left_eye_ball_x",
|
|
||||||
10: "left_eye_ball_y",
|
|
||||||
11: "right_eye_ball_x",
|
|
||||||
12: "right_eye_ball_y",
|
|
||||||
13: "upper_lip_y",
|
|
||||||
14: "lower_lip_y",
|
|
||||||
15: "jaw_x",
|
|
||||||
16: "jaw_y"
|
|
||||||
}
|
|
||||||
|
|
||||||
print("可用表情参数:")
|
|
||||||
for idx, name in param_map.items():
|
|
||||||
print(f"{idx}: {name}")
|
|
||||||
|
|
||||||
try:
|
|
||||||
param_id = int(input("选择参数ID: "))
|
|
||||||
if param_id not in param_map:
|
|
||||||
print("无效的参数ID")
|
|
||||||
return
|
|
||||||
|
|
||||||
angle = int(input("输入角度(0-180): "))
|
|
||||||
if angle < 0 or angle > 180:
|
|
||||||
print("角度必须在0-180之间")
|
|
||||||
return
|
|
||||||
|
|
||||||
# 转换为归一化值
|
|
||||||
value = angle_to_normalized(angle)
|
|
||||||
|
|
||||||
# 创建表情对象
|
|
||||||
expression = FacialExpressionState()
|
|
||||||
# 设置对应的参数
|
|
||||||
setattr(expression, param_map[param_id], value)
|
|
||||||
|
|
||||||
# 发送表情设置
|
|
||||||
result = biohead.set_expression(expression)
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"设置 {param_map[param_id]} 成功，角度: {angle}°，归一化值: {value:.2f}")
|
|
||||||
else:
|
|
||||||
print(f"设置失败，错误码: {result}")
|
|
||||||
|
|
||||||
except ValueError:
|
|
||||||
print("输入无效，请输入数字")
|
|
||||||
|
|
||||||
def set_multiple_expressions(biohead):
|
|
||||||
"""设置多个表情参数"""
|
|
||||||
print("\n--- 设置多个表情参数 ---")
|
|
||||||
# 表情参数映射表
|
|
||||||
param_map = {
|
|
||||||
1: "left_eyebrow_outside_y",
|
|
||||||
2: "left_eyebrow_inside_y",
|
|
||||||
3: "right_eyebrow_outside_y",
|
|
||||||
4: "right_eyebrow_inside_y",
|
|
||||||
5: "left_eye_upper_lid_y",
|
|
||||||
6: "left_eye_lower_lid_y",
|
|
||||||
7: "right_eye_upper_lid_y",
|
|
||||||
8: "right_eye_lower_lid_y",
|
|
||||||
9: "left_eye_ball_x",
|
|
||||||
10: "left_eye_ball_y",
|
|
||||||
11: "right_eye_ball_x",
|
|
||||||
12: "right_eye_ball_y",
|
|
||||||
13: "upper_lip_y",
|
|
||||||
14: "lower_lip_y",
|
|
||||||
15: "jaw_x",
|
|
||||||
16: "jaw_y"
|
|
||||||
}
|
|
||||||
|
|
||||||
print("可用表情参数:")
|
|
||||||
for idx, name in param_map.items():
|
|
||||||
print(f"{idx}: {name}")
|
|
||||||
|
|
||||||
try:
|
|
||||||
count = int(input("输入要设置的参数数量: "))
|
|
||||||
if count <= 0:
|
|
||||||
print("数量必须大于0")
|
|
||||||
return
|
|
||||||
|
|
||||||
# 创建表情对象
|
|
||||||
expression = FacialExpressionState()
|
|
||||||
|
|
||||||
for i in range(count):
|
|
||||||
print(f"\n第{i+1}个参数:")
|
|
||||||
param_id = int(input("选择参数ID: "))
|
|
||||||
if param_id not in param_map:
|
|
||||||
print("无效的参数ID，跳过该参数")
|
|
||||||
continue
|
|
||||||
|
|
||||||
angle = int(input("输入角度(0-180): "))
|
|
||||||
if angle < 0 or angle > 180:
|
|
||||||
print("角度必须在0-180之间，跳过该参数")
|
|
||||||
continue
|
|
||||||
|
|
||||||
# 转换为归一化值
|
|
||||||
value = angle_to_normalized(angle)
|
|
||||||
# 设置对应的参数
|
|
||||||
setattr(expression, param_map[param_id], value)
|
|
||||||
print(f"已添加: {param_map[param_id]} = {value:.2f} (对应角度: {angle}°)")
|
|
||||||
|
|
||||||
# 发送表情设置
|
|
||||||
result = biohead.set_expression(expression)
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("设置多个表情参数成功")
|
|
||||||
else:
|
|
||||||
print(f"设置失败，错误码: {result}")
|
|
||||||
|
|
||||||
except ValueError:
|
|
||||||
print("输入无效，请输入数字")
|
|
||||||
|
|
||||||
def auto_mode(biohead, params):
|
|
||||||
"""自动增减模式：0→1→0，循环变化"""
|
|
||||||
print("\n--- 自动增减模式 ---")
|
|
||||||
try:
|
|
||||||
# 表情参数映射表
|
|
||||||
param_map = {
|
|
||||||
1: "left_eyebrow_outside_y",
|
|
||||||
2: "left_eyebrow_inside_y",
|
|
||||||
3: "right_eyebrow_outside_y",
|
|
||||||
4: "right_eyebrow_inside_y",
|
|
||||||
5: "left_eye_upper_lid_y",
|
|
||||||
6: "left_eye_lower_lid_y",
|
|
||||||
7: "right_eye_upper_lid_y",
|
|
||||||
8: "right_eye_lower_lid_y",
|
|
||||||
9: "left_eye_ball_x",
|
|
||||||
10: "left_eye_ball_y",
|
|
||||||
11: "right_eye_ball_x",
|
|
||||||
12: "right_eye_ball_y",
|
|
||||||
13: "upper_lip_y",
|
|
||||||
14: "lower_lip_y",
|
|
||||||
15: "jaw_x",
|
|
||||||
16: "jaw_y"
|
|
||||||
}
|
|
||||||
|
|
||||||
# 过滤有效的参数ID
|
|
||||||
valid_params = [p for p in params if p in param_map]
|
|
||||||
if not valid_params:
|
|
||||||
print("没有有效的参数，返回菜单")
|
|
||||||
return
|
|
||||||
|
|
||||||
print(f"将自动控制以下参数: {[param_map[p] for p in valid_params]}")
|
|
||||||
|
|
||||||
frequency = 50.0 # 50Hz
|
|
||||||
interval = 1.0 / frequency
|
|
||||||
value = 0.0
|
|
||||||
direction = 0.02 # 步长
|
|
||||||
|
|
||||||
# 启动流式控制
|
|
||||||
result = biohead.start_stream()
|
|
||||||
if result != CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"开始流式会话失败，错误码: {result}")
|
|
||||||
return
|
|
||||||
|
|
||||||
print("启动自动模式，按Ctrl+C停止")
|
|
||||||
while True:
|
|
||||||
# 创建protobuf消息
|
|
||||||
proto_expr = biohead_command_pb2.FacialExpression()
|
|
||||||
|
|
||||||
# 设置所有参数
|
|
||||||
for param_id in valid_params:
|
|
||||||
param_name = param_map[param_id]
|
|
||||||
|
|
||||||
# 根据参数类型设置到不同的字段
|
|
||||||
if param_name in ["left_eyebrow_outside_y", "left_eyebrow_inside_y",
|
|
||||||
"right_eyebrow_outside_y", "right_eyebrow_inside_y"]:
|
|
||||||
setattr(proto_expr.eyebrow, param_name, value)
|
|
||||||
elif param_name in ["left_eye_upper_lid_y", "left_eye_lower_lid_y",
|
|
||||||
"right_eye_upper_lid_y", "right_eye_lower_lid_y"]:
|
|
||||||
setattr(proto_expr.eyelid, param_name, value)
|
|
||||||
elif param_name in ["left_eye_ball_x", "left_eye_ball_y",
|
|
||||||
"right_eye_ball_x", "right_eye_ball_y"]:
|
|
||||||
setattr(proto_expr.eye_ball, param_name, value)
|
|
||||||
elif param_name in ["upper_lip_y", "lower_lip_y"]:
|
|
||||||
setattr(proto_expr.mouth, param_name, value)
|
|
||||||
elif param_name in ["jaw_x", "jaw_y"]:
|
|
||||||
setattr(proto_expr.jaw, param_name, value)
|
|
||||||
|
|
||||||
# 发送流式表情
|
|
||||||
result = biohead.stream_expression(proto_expr)
|
|
||||||
if result != CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"流式控制失败，错误码: {result}")
|
|
||||||
break
|
|
||||||
|
|
||||||
# 更新值
|
|
||||||
value += direction
|
|
||||||
if value >= 1.0:
|
|
||||||
value = 1.0
|
|
||||||
direction = -direction
|
|
||||||
elif value <= 0.0:
|
|
||||||
value = 0.0
|
|
||||||
direction = -direction
|
|
||||||
|
|
||||||
time.sleep(interval)
|
|
||||||
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print("\n自动模式已停止")
|
|
||||||
finally:
|
|
||||||
# 结束流式会话
|
|
||||||
biohead.end_stream()
|
|
||||||
|
|
||||||
def set_auto_params():
|
|
||||||
"""设置自动模式的参数列表"""
|
|
||||||
print("\n--- 设置自动模式参数 ---")
|
|
||||||
# 表情参数映射表
|
|
||||||
param_map = {
|
|
||||||
1: "left_eyebrow_outside_y",
|
|
||||||
2: "left_eyebrow_inside_y",
|
|
||||||
3: "right_eyebrow_outside_y",
|
|
||||||
4: "right_eyebrow_inside_y",
|
|
||||||
5: "left_eye_upper_lid_y",
|
|
||||||
6: "left_eye_lower_lid_y",
|
|
||||||
7: "right_eye_upper_lid_y",
|
|
||||||
8: "right_eye_lower_lid_y",
|
|
||||||
9: "left_eye_ball_x",
|
|
||||||
10: "left_eye_ball_y",
|
|
||||||
11: "right_eye_ball_x",
|
|
||||||
12: "right_eye_ball_y",
|
|
||||||
13: "upper_lip_y",
|
|
||||||
14: "lower_lip_y",
|
|
||||||
15: "jaw_x",
|
|
||||||
16: "jaw_y"
|
|
||||||
}
|
|
||||||
|
|
||||||
print("可用表情参数:")
|
|
||||||
for idx, name in param_map.items():
|
|
||||||
print(f"{idx}: {name}")
|
|
||||||
|
|
||||||
try:
|
|
||||||
count = int(input("输入要自动控制的参数数量: "))
|
|
||||||
if count <= 0:
|
|
||||||
print("数量必须大于0")
|
|
||||||
return None
|
|
||||||
|
|
||||||
params = []
|
|
||||||
for i in range(count):
|
|
||||||
param_id = int(input(f"第{i+1}个参数ID: "))
|
|
||||||
if param_id not in param_map:
|
|
||||||
print("无效的参数ID，跳过")
|
|
||||||
continue
|
|
||||||
params.append(param_id)
|
|
||||||
|
|
||||||
if not params:
|
|
||||||
print("未设置有效的参数")
|
|
||||||
return None
|
|
||||||
|
|
||||||
print(f"已设置自动模式参数: {[param_map[p] for p in params]}")
|
|
||||||
return params
|
|
||||||
|
|
||||||
except ValueError:
|
|
||||||
print("输入无效，请输入数字")
|
|
||||||
return None
|
|
||||||
|
|
||||||
def blink_mode(biohead):
|
|
||||||
"""眨眼模式：执行预设的眨眼动作"""
|
|
||||||
print("\n--- 眨眼模式 ---")
|
|
||||||
try:
|
|
||||||
blink_count = int(input("输入眨眼次数(默认10次): ") or "10")
|
|
||||||
interval = float(input("输入间隔时间(秒，默认0.1): ") or "0.1")
|
|
||||||
|
|
||||||
# 启动流式控制
|
|
||||||
result = biohead.start_stream()
|
|
||||||
if result != CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"开始流式会话失败，错误码: {result}")
|
|
||||||
return
|
|
||||||
|
|
||||||
print(f"启动眨眼模式，共{blink_count}次，间隔{interval}秒")
|
|
||||||
|
|
||||||
for i in range(blink_count):
|
|
||||||
print(f"第{i+1}次眨眼...")
|
|
||||||
|
|
||||||
# 闭眼动作
|
|
||||||
proto_expr = biohead_command_pb2.FacialExpression()
|
|
||||||
proto_expr.eyelid.left_upper_lid_y = angle_to_normalized(90)
|
|
||||||
proto_expr.eyelid.left_lower_lid_y = angle_to_normalized(90)
|
|
||||||
proto_expr.eyelid.right_upper_lid_y = angle_to_normalized(90)
|
|
||||||
proto_expr.eyelid.right_lower_lid_y = angle_to_normalized(90)
|
|
||||||
|
|
||||||
result = biohead.stream_expression(proto_expr)
|
|
||||||
if result != CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"闭眼动作失败，错误码: {result}")
|
|
||||||
break
|
|
||||||
|
|
||||||
time.sleep(interval)
|
|
||||||
|
|
||||||
# 睁眼动作
|
|
||||||
proto_expr = biohead_command_pb2.FacialExpression()
|
|
||||||
proto_expr.eyelid.left_upper_lid_y = angle_to_normalized(50)
|
|
||||||
proto_expr.eyelid.left_lower_lid_y = angle_to_normalized(130)
|
|
||||||
proto_expr.eyelid.right_upper_lid_y = angle_to_normalized(130)
|
|
||||||
proto_expr.eyelid.right_lower_lid_y = angle_to_normalized(60)
|
|
||||||
|
|
||||||
result = biohead.stream_expression(proto_expr)
|
|
||||||
if result != CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"睁眼动作失败，错误码: {result}")
|
|
||||||
break
|
|
||||||
|
|
||||||
time.sleep(interval)
|
|
||||||
|
|
||||||
print("眨眼模式结束")
|
|
||||||
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print("\n眨眼模式已停止")
|
|
||||||
except ValueError:
|
|
||||||
print("输入无效，请输入数字")
|
|
||||||
finally:
|
|
||||||
# 结束流式会话
|
|
||||||
biohead.end_stream()
|
|
||||||
|
|
||||||
def yawn_mode(biohead):
|
|
||||||
"""打哈欠模式：模拟打哈欠动作"""
|
|
||||||
print("\n--- 打哈欠模式 ---")
|
|
||||||
try:
|
|
||||||
yawn_count = int(input("输入打哈欠次数(默认5次): ") or "5")
|
|
||||||
interval = float(input("输入间隔时间(秒，默认1): ") or "1")
|
|
||||||
|
|
||||||
# 启动流式控制
|
|
||||||
result = biohead.start_stream()
|
|
||||||
if result != CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"开始流式会话失败，错误码: {result}")
|
|
||||||
return
|
|
||||||
|
|
||||||
print(f"启动打哈欠模式，共{yawn_count}次，间隔{interval}秒")
|
|
||||||
|
|
||||||
for i in range(yawn_count):
|
|
||||||
print(f"第{i+1}次打哈欠...")
|
|
||||||
|
|
||||||
# 打哈欠动作
|
|
||||||
proto_expr = biohead_command_pb2.FacialExpression()
|
|
||||||
# 眉毛
|
|
||||||
proto_expr.eyebrow.left_eyebrow_outside_y = angle_to_normalized(70)
|
|
||||||
proto_expr.eyebrow.right_eyebrow_outside_y = angle_to_normalized(110)
|
|
||||||
# 眼睑
|
|
||||||
proto_expr.eyelid.left_upper_lid_y = angle_to_normalized(60)
|
|
||||||
proto_expr.eyelid.left_lower_lid_y = angle_to_normalized(130)
|
|
||||||
proto_expr.eyelid.right_upper_lid_y = angle_to_normalized(120)
|
|
||||||
proto_expr.eyelid.right_lower_lid_y = angle_to_normalized(80)
|
|
||||||
# 嘴巴和下巴
|
|
||||||
proto_expr.mouth.upper_lip_y = angle_to_normalized(120)
|
|
||||||
proto_expr.mouth.lower_lip_y = angle_to_normalized(45)
|
|
||||||
proto_expr.jaw.x = angle_to_normalized(60)
|
|
||||||
proto_expr.jaw.y = angle_to_normalized(120)
|
|
||||||
|
|
||||||
result = biohead.stream_expression(proto_expr)
|
|
||||||
if result != CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"打哈欠动作失败，错误码: {result}")
|
|
||||||
break
|
|
||||||
|
|
||||||
time.sleep(interval)
|
|
||||||
|
|
||||||
# 恢复动作
|
|
||||||
proto_expr = biohead_command_pb2.FacialExpression()
|
|
||||||
# 恢复到正常状态
|
|
||||||
proto_expr.eyebrow.left_eyebrow_outside_y = angle_to_normalized(90)
|
|
||||||
proto_expr.eyebrow.right_eyebrow_outside_y = angle_to_normalized(90)
|
|
||||||
proto_expr.eyelid.left_upper_lid_y = angle_to_normalized(90)
|
|
||||||
proto_expr.eyelid.left_lower_lid_y = angle_to_normalized(90)
|
|
||||||
proto_expr.eyelid.right_upper_lid_y = angle_to_normalized(90)
|
|
||||||
proto_expr.eyelid.right_lower_lid_y = angle_to_normalized(90)
|
|
||||||
proto_expr.mouth.upper_lip_y = angle_to_normalized(90)
|
|
||||||
proto_expr.mouth.lower_lip_y = angle_to_normalized(90)
|
|
||||||
proto_expr.jaw.x = angle_to_normalized(90)
|
|
||||||
proto_expr.jaw.y = angle_to_normalized(90)
|
|
||||||
|
|
||||||
result = biohead.stream_expression(proto_expr)
|
|
||||||
if result != CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"恢复动作失败，错误码: {result}")
|
|
||||||
break
|
|
||||||
|
|
||||||
time.sleep(interval)
|
|
||||||
|
|
||||||
print("打哈欠模式结束")
|
|
||||||
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print("\n打哈欠模式已停止")
|
|
||||||
except ValueError:
|
|
||||||
print("输入无效，请输入数字")
|
|
||||||
finally:
|
|
||||||
# 结束流式会话
|
|
||||||
biohead.end_stream()
|
|
||||||
|
|
||||||
def talk_mode(biohead):
|
|
||||||
"""说话模式：模拟嘴巴开合动作"""
|
|
||||||
print("\n--- 说话模式 ---")
|
|
||||||
try:
|
|
||||||
talk_count = int(input("输入说话循环次数(默认50次): ") or "50")
|
|
||||||
|
|
||||||
# 启动流式控制
|
|
||||||
result = biohead.start_stream()
|
|
||||||
if result != CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"开始流式会话失败，错误码: {result}")
|
|
||||||
return
|
|
||||||
|
|
||||||
print(f"启动说话模式，共{talk_count}次")
|
|
||||||
|
|
||||||
for i in range(talk_count):
|
|
||||||
print(f"第{i+1}次说话...")
|
|
||||||
|
|
||||||
# 开口动作
|
|
||||||
proto_expr = biohead_command_pb2.FacialExpression()
|
|
||||||
proto_expr.mouth.upper_lip_y = angle_to_normalized(120)
|
|
||||||
proto_expr.mouth.lower_lip_y = angle_to_normalized(45)
|
|
||||||
proto_expr.jaw.x = angle_to_normalized(80)
|
|
||||||
proto_expr.jaw.y = angle_to_normalized(110)
|
|
||||||
|
|
||||||
result = biohead.stream_expression(proto_expr)
|
|
||||||
if result != CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"开口动作失败，错误码: {result}")
|
|
||||||
break
|
|
||||||
|
|
||||||
# 随机间隔
|
|
||||||
interval = random.uniform(0, 0.3)
|
|
||||||
time.sleep(interval)
|
|
||||||
|
|
||||||
# 闭口动作
|
|
||||||
proto_expr = biohead_command_pb2.FacialExpression()
|
|
||||||
proto_expr.mouth.upper_lip_y = angle_to_normalized(120)
|
|
||||||
proto_expr.mouth.lower_lip_y = angle_to_normalized(45)
|
|
||||||
proto_expr.jaw.x = angle_to_normalized(90)
|
|
||||||
proto_expr.jaw.y = angle_to_normalized(90)
|
|
||||||
|
|
||||||
result = biohead.stream_expression(proto_expr)
|
|
||||||
if result != CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"闭口动作失败，错误码: {result}")
|
|
||||||
break
|
|
||||||
|
|
||||||
# 随机间隔
|
|
||||||
interval = random.uniform(0, 0.3)
|
|
||||||
time.sleep(interval)
|
|
||||||
|
|
||||||
print("说话模式结束")
|
|
||||||
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print("\n说话模式已停止")
|
|
||||||
except ValueError:
|
|
||||||
print("输入无效，请输入数字")
|
|
||||||
finally:
|
|
||||||
# 结束流式会话
|
|
||||||
biohead.end_stream()
|
|
||||||
|
|
||||||
def main():
|
|
||||||
# 初始化gRPC客户端
|
|
||||||
server_addr = "192.168.1.222:50051"
|
|
||||||
if len(sys.argv) > 1:
|
|
||||||
server_addr = sys.argv[1]
|
|
||||||
|
|
||||||
print(f"连接到仿生头服务器: {server_addr}")
|
|
||||||
client, biohead = init_biohead_client(server_addr)
|
|
||||||
if not client or not biohead:
|
|
||||||
sys.exit(1)
|
|
||||||
|
|
||||||
# 默认自动模式参数
|
|
||||||
auto_params = [5, 6, 7, 8] # 默认控制眼睑参数
|
|
||||||
|
|
||||||
try:
|
|
||||||
while True:
|
|
||||||
print("\n=== 仿生头控制菜单 ===")
|
|
||||||
print("1. 控制单个表情参数")
|
|
||||||
print("2. 控制多个表情参数")
|
|
||||||
print("3. 退出")
|
|
||||||
print("4. 自动增减模式(50Hz)")
|
|
||||||
print("5. 设置自动模式参数")
|
|
||||||
print("6. 眨眼模式")
|
|
||||||
print("7. 打哈欠模式")
|
|
||||||
print("8. 说话模式")
|
|
||||||
opt = input("选择操作: ").strip()
|
|
||||||
|
|
||||||
if opt == '1':
|
|
||||||
set_single_expression(biohead)
|
|
||||||
elif opt == '2':
|
|
||||||
set_multiple_expressions(biohead)
|
|
||||||
elif opt == '3':
|
|
||||||
print("退出程序")
|
|
||||||
break
|
|
||||||
elif opt == '4':
|
|
||||||
auto_mode(biohead, auto_params)
|
|
||||||
elif opt == '5':
|
|
||||||
new_params = set_auto_params()
|
|
||||||
if new_params:
|
|
||||||
auto_params = new_params
|
|
||||||
elif opt == '6':
|
|
||||||
blink_mode(biohead)
|
|
||||||
elif opt == '7':
|
|
||||||
yawn_mode(biohead)
|
|
||||||
elif opt == '8':
|
|
||||||
talk_mode(biohead)
|
|
||||||
else:
|
|
||||||
print("无效选项，请重试")
|
|
||||||
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print("\n用户中断，退出程序")
|
|
||||||
finally:
|
|
||||||
client.close()
|
|
||||||
print("已关闭连接")
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
344
biohead_test.py
344
biohead_test.py
@ -1,344 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
# -*- coding: utf-8 -*-
|
|
||||||
"""
|
|
||||||
interactive_servo_test.py — 基于 Python 的 ESP32 舵机控制交互脚本
|
|
||||||
依赖：pip install pyserial
|
|
||||||
用法：
|
|
||||||
python3 interactive_servo_test.py /dev/ttyUSB0 [baudrate]
|
|
||||||
功能：
|
|
||||||
1. 控制单个舵机
|
|
||||||
2. 控制多个舵机
|
|
||||||
3. 退出
|
|
||||||
4. 自动增减模式：从 0 到 180° 每步 +6° → -6° 循环，50Hz 频率发送
|
|
||||||
5. 设置自动模式舵机数量（通道 1~N）
|
|
||||||
6. 眨眼模式：让五个舵机执行预设角度循环，模拟眨眼，8号舵机保持120度
|
|
||||||
7. 打哈欠模式：模拟打哈欠动作
|
|
||||||
8. 说话模式：模拟嘴巴开合动作
|
|
||||||
"""
|
|
||||||
import sys
|
|
||||||
import time
|
|
||||||
import serial
|
|
||||||
import random
|
|
||||||
|
|
||||||
FRAME_HEADER = 0xAA # 帧头
|
|
||||||
|
|
||||||
def wakeup_esp32(port, baud=115200):
|
|
||||||
"""通过 RTS/DTR 线复位并唤醒 ESP32"""
|
|
||||||
try:
|
|
||||||
ser = serial.Serial(port, baud, timeout=1)
|
|
||||||
ser.setDTR(False)
|
|
||||||
ser.setRTS(False)
|
|
||||||
time.sleep(0.01)
|
|
||||||
ser.setDTR(True)
|
|
||||||
ser.setRTS(True)
|
|
||||||
ser.write(b"\r\n")
|
|
||||||
time.sleep(1)
|
|
||||||
except Exception as e:
|
|
||||||
print(f"ESP32 唤醒失败: {e}")
|
|
||||||
finally:
|
|
||||||
try:
|
|
||||||
ser.close()
|
|
||||||
except:
|
|
||||||
pass
|
|
||||||
|
|
||||||
|
|
||||||
def open_serial(port, baud):
|
|
||||||
"""打开并配置串口"""
|
|
||||||
try:
|
|
||||||
ser = serial.Serial(port, baud, timeout=1)
|
|
||||||
print(f"已打开串口: {port} @ {baud}bps")
|
|
||||||
return ser
|
|
||||||
except Exception as e:
|
|
||||||
print(f"打开串口失败: {e}")
|
|
||||||
sys.exit(1)
|
|
||||||
|
|
||||||
|
|
||||||
def calc_checksum(data: bytearray) -> int:
|
|
||||||
"""计算 XOR 校验和"""
|
|
||||||
cs = 0
|
|
||||||
for b in data:
|
|
||||||
cs ^= b
|
|
||||||
return cs
|
|
||||||
|
|
||||||
|
|
||||||
def pack_commands(cmds):
|
|
||||||
"""
|
|
||||||
cmds: list of (addr, channel, angle, duration_ms)
|
|
||||||
返回：完整帧字节数组
|
|
||||||
格式： [FRAME_HEADER][count] [addr, ch, ang, durL, durH]... [checksum]
|
|
||||||
"""
|
|
||||||
pkt = bytearray([FRAME_HEADER, len(cmds)])
|
|
||||||
for addr, ch, ang, dur in cmds:
|
|
||||||
pkt.extend([addr, ch, ang, dur & 0xFF, (dur >> 8) & 0xFF])
|
|
||||||
pkt.append(calc_checksum(pkt))
|
|
||||||
return pkt
|
|
||||||
|
|
||||||
|
|
||||||
def print_packet(pkt):
|
|
||||||
"""打印十六进制数据包"""
|
|
||||||
print("发送数据包:", ' '.join(f"0x{b:02X}" for b in pkt))
|
|
||||||
|
|
||||||
|
|
||||||
def input_hex(prompt):
|
|
||||||
"""输入 0x 开头或十进制整型"""
|
|
||||||
val = input(prompt).strip()
|
|
||||||
try:
|
|
||||||
return int(val, 0)
|
|
||||||
except ValueError:
|
|
||||||
print(f"无效数字: {val}")
|
|
||||||
return input_hex(prompt)
|
|
||||||
|
|
||||||
|
|
||||||
def input_cmd_single():
|
|
||||||
"""交互输入单条命令"""
|
|
||||||
addr = input_hex("输入 addr (hex or dec): ")
|
|
||||||
ch = input_hex("输入 channel (hex or dec): ")
|
|
||||||
ang = input_hex("输入 angle (0-180): ")
|
|
||||||
dur = input_hex("输入 duration(ms): ")
|
|
||||||
return [(addr, ch, ang, dur)]
|
|
||||||
|
|
||||||
|
|
||||||
def input_cmd_multiple():
|
|
||||||
"""交互输入多条命令"""
|
|
||||||
cnt = input_hex("输入命令数量: ")
|
|
||||||
cmds = []
|
|
||||||
for i in range(cnt):
|
|
||||||
print(f"第 {i+1} 条:")
|
|
||||||
cmds.extend(input_cmd_single())
|
|
||||||
return cmds
|
|
||||||
|
|
||||||
|
|
||||||
def auto_mode(ser, channels):
|
|
||||||
"""自动增减模式：0→180→0，步长6，50Hz，多舵机"""
|
|
||||||
addr = input_hex("自动模式: 输入 addr (hex或dec): ")
|
|
||||||
interval = 1.0 / 50.0
|
|
||||||
dur_ms = int(interval * 1000)
|
|
||||||
angle = 0
|
|
||||||
direction = 1
|
|
||||||
step = 6
|
|
||||||
|
|
||||||
print(f"启动自动模式 @50Hz, addr=0x{addr:02X}, channels={channels}, step={step}°")
|
|
||||||
try:
|
|
||||||
while True:
|
|
||||||
cmds = [(addr, ch, angle, dur_ms) for ch in channels]
|
|
||||||
pkt = pack_commands(cmds)
|
|
||||||
print_packet(pkt)
|
|
||||||
ser.write(pkt)
|
|
||||||
# 更新角度
|
|
||||||
angle += direction * step
|
|
||||||
if angle >= 180:
|
|
||||||
angle = 180
|
|
||||||
direction = -1
|
|
||||||
elif angle <= 0:
|
|
||||||
angle = 0
|
|
||||||
direction = 1
|
|
||||||
time.sleep(interval)
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print("\n自动模式已停止，返回菜单")
|
|
||||||
|
|
||||||
|
|
||||||
def set_channels():
|
|
||||||
"""自定义舵机数量，返回通道列表 1~N"""
|
|
||||||
cnt = input_hex("输入自动模式舵机数量 N: ")
|
|
||||||
if cnt < 1:
|
|
||||||
print("数量必须 >= 1")
|
|
||||||
return None
|
|
||||||
channels = list(range(1, cnt+1))
|
|
||||||
print(f"已设置通道列表: {channels}")
|
|
||||||
return channels
|
|
||||||
|
|
||||||
|
|
||||||
def blink_mode(ser):
|
|
||||||
"""眨眼模式：根据输入的角度进行指定舵机的眨眼操作"""
|
|
||||||
addr = input_hex("眨眼模式: 输入 addr (hex或dec): ")
|
|
||||||
blink_count = 10 # 循环10次
|
|
||||||
interval = 0.1 # 每秒眨眼一次
|
|
||||||
|
|
||||||
# 定义五个舵机的通道
|
|
||||||
servo_channels = [4, 5, 6, 7, 8] # 控制五个舵机
|
|
||||||
blink_angles = [90, 90, 90, 90, 90] # 眨眼时的角度
|
|
||||||
open_angles = [50, 130, 130, 60, 90] # 睁开时的角度（4-7号舵机回到90度，8号保持120度）
|
|
||||||
|
|
||||||
print(f"启动眨眼模式，10次眨眼，每次间隔 0.1 秒")
|
|
||||||
|
|
||||||
try:
|
|
||||||
for i in range(blink_count):
|
|
||||||
print(f"第 {i+1} 次眨眼...")
|
|
||||||
|
|
||||||
# 眨眼动作：同步发送所有舵机到眨眼角度
|
|
||||||
cmds = [(addr, ch, angle, 0) for ch, angle in zip(servo_channels, blink_angles)]
|
|
||||||
pkt = pack_commands(cmds)
|
|
||||||
print_packet(pkt)
|
|
||||||
ser.write(pkt)
|
|
||||||
time.sleep(interval)
|
|
||||||
|
|
||||||
# 睁开动作：同步发送4-7号舵机回到90度，8号保持120度
|
|
||||||
print("睁开动作：舵机4-7回到90度")
|
|
||||||
cmds = [(addr, ch, angle, 0) for ch, angle in zip(servo_channels, open_angles)]
|
|
||||||
pkt = pack_commands(cmds)
|
|
||||||
print_packet(pkt)
|
|
||||||
ser.write(pkt)
|
|
||||||
time.sleep(interval)
|
|
||||||
|
|
||||||
print("眨眼模式结束，返回菜单")
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print("\n眨眼模式已停止，返回菜单")
|
|
||||||
|
|
||||||
|
|
||||||
def yawn_mode(ser):
|
|
||||||
"""打哈欠模式：控制多个舵机模拟打哈欠动作"""
|
|
||||||
addr = input_hex("打哈欠模式: 输入 addr (hex或dec): ")
|
|
||||||
yawn_count = 5 # 循环5次
|
|
||||||
interval = 1 # 每次打哈欠动作间隔1秒
|
|
||||||
|
|
||||||
# 定义控制的舵机通道
|
|
||||||
# 打哈欠时使用舵机1, 3, 4, 5, 6, 7, 8（通道1~8）
|
|
||||||
# 闭嘴时使用舵机8、9（65号舵机）
|
|
||||||
yawning_channels = [1, 3, 4, 5, 6, 7, 8] # 控制通道1~8
|
|
||||||
yawning_angles = [70, 110, 60, 130, 120, 80, 90] # 打哈欠时的角度（根据需要调整）
|
|
||||||
closing_angles = [90, 90, 90, 90, 90, 90, 90] # 闭嘴时的角度（闭嘴动作）
|
|
||||||
|
|
||||||
# 65号舵机（8、9号通道）设置不同的角度，模拟控制头部动作
|
|
||||||
yawning_65_angles = [120, 45, 60, 120] # 65号舵机（8、9号通道）在打哈欠时的角度
|
|
||||||
closing_65_angles = [120, 45, 90, 90] # 闭嘴时65号舵机（8、9号通道）的角度
|
|
||||||
|
|
||||||
print(f"启动打哈欠模式，循环 {yawn_count} 次，每次间隔 {interval} 秒")
|
|
||||||
|
|
||||||
try:
|
|
||||||
for i in range(yawn_count):
|
|
||||||
print(f"第 {i+1} 次打哈欠...")
|
|
||||||
|
|
||||||
# 打哈欠动作：同步发送所有舵机到打哈欠角度
|
|
||||||
cmds = [(addr, ch, angle, 3000) for ch, angle in zip(yawning_channels, yawning_angles)]
|
|
||||||
cmds_65 = [(65, ch, angle, 3000) for ch, angle in zip([0,1,8, 9], yawning_65_angles)] # 控制65号舵机（8、9号通道）
|
|
||||||
cmds.extend(cmds_65) # 合并65号舵机的指令
|
|
||||||
|
|
||||||
pkt = pack_commands(cmds)
|
|
||||||
print_packet(pkt)
|
|
||||||
ser.write(pkt) # 一次性发送所有指令
|
|
||||||
time.sleep(interval)
|
|
||||||
|
|
||||||
# 闭嘴动作：同步发送舵机回到闭嘴角度
|
|
||||||
print("闭嘴动作：所有舵机回到闭嘴角度")
|
|
||||||
cmds = [(addr, ch, angle, 3000) for ch, angle in zip(yawning_channels, closing_angles)]
|
|
||||||
cmds_65 = [(65, ch, angle, 3000) for ch, angle in zip([0,1,8, 9], closing_65_angles)] # 65号舵机（8、9号通道）
|
|
||||||
cmds.extend(cmds_65) # 合并65号舵机的指令
|
|
||||||
|
|
||||||
pkt = pack_commands(cmds)
|
|
||||||
print_packet(pkt)
|
|
||||||
ser.write(pkt) # 一次性发送所有指令
|
|
||||||
time.sleep(interval)
|
|
||||||
|
|
||||||
print("打哈欠模式结束，返回菜单")
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print("\n打哈欠模式已停止，返回菜单")
|
|
||||||
|
|
||||||
|
|
||||||
def talk_mode(ser):
|
|
||||||
"""说话模式：模拟嘴巴开合动作"""
|
|
||||||
addr = input_hex("说话模式: 输入 addr (hex或dec): ")
|
|
||||||
talk_count = 50 # 循环5次
|
|
||||||
|
|
||||||
# 定义控制的舵机通道
|
|
||||||
mouth_channels = [0, 1, 8, 9] # 控制3个舵机（口型）
|
|
||||||
mouth_open_angles = [120, 45, 80, 110] # 嘴巴打开时的角度
|
|
||||||
mouth_close_angles = [120, 45, 90, 90] # 嘴巴关闭时的角度（正常闭合）
|
|
||||||
|
|
||||||
print(f"启动说话模式，循环 {talk_count} 次，每次间隔随机生成在0~1秒之间")
|
|
||||||
|
|
||||||
try:
|
|
||||||
for i in range(talk_count):
|
|
||||||
print(f"第 {i + 1} 次说话...")
|
|
||||||
|
|
||||||
# 开口动作：同步发送所有舵机到嘴巴打开的角度
|
|
||||||
cmds = [(addr, ch, angle, 0) for ch, angle in zip(mouth_channels, mouth_open_angles)]
|
|
||||||
pkt = pack_commands(cmds)
|
|
||||||
print_packet(pkt)
|
|
||||||
ser.write(pkt) # 一次性发送所有指令
|
|
||||||
|
|
||||||
# 生成一个 0 到 1 之间的随机间隔时间
|
|
||||||
interval = random.uniform(0, 0.3)
|
|
||||||
print(f"随机间隔时间：{interval:.2f}秒")
|
|
||||||
time.sleep(interval)
|
|
||||||
|
|
||||||
# 闭口动作：同步发送所有舵机回到嘴巴关闭的角度
|
|
||||||
print("闭口动作：舵机回到闭口角度")
|
|
||||||
cmds = [(addr, ch, angle, 0) for ch, angle in zip(mouth_channels, mouth_close_angles)]
|
|
||||||
pkt = pack_commands(cmds)
|
|
||||||
print_packet(pkt)
|
|
||||||
ser.write(pkt) # 一次性发送所有指令
|
|
||||||
|
|
||||||
# 生成一个 0 到 1 之间的随机间隔时间
|
|
||||||
interval = random.uniform(0, 0.3)
|
|
||||||
print(f"随机间隔时间：{interval:.2f}秒")
|
|
||||||
time.sleep(interval)
|
|
||||||
|
|
||||||
print("说话模式结束，返回菜单")
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print("\n说话模式已停止，返回菜单")
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
if len(sys.argv) < 2:
|
|
||||||
print(f"用法: {sys.argv[0]} /dev/ttyUSB0 [baudrate]")
|
|
||||||
sys.exit(1)
|
|
||||||
port = sys.argv[1]
|
|
||||||
baud = int(sys.argv[2]) if len(sys.argv) > 2 else 115200
|
|
||||||
|
|
||||||
print("唤醒 ESP32...")
|
|
||||||
wakeup_esp32(port, baud)
|
|
||||||
ser = open_serial(port, baud)
|
|
||||||
channels = list(range(1, 10)) # 默认通道 1-9
|
|
||||||
|
|
||||||
try:
|
|
||||||
while True:
|
|
||||||
print("\n=== 操作菜单 ===")
|
|
||||||
print("1. 控制单个舵机")
|
|
||||||
print("2. 控制多个舵机")
|
|
||||||
print("3. 退出")
|
|
||||||
print("4. 自动增减模式(50Hz)")
|
|
||||||
print("5. 设置自动模式舵机数量")
|
|
||||||
print("6. 眨眼模式")
|
|
||||||
print("7. 打哈欠模式")
|
|
||||||
print("8. 说话模式") # 新增选项
|
|
||||||
opt = input("选择: ").strip()
|
|
||||||
if opt == '1':
|
|
||||||
cmds = input_cmd_single()
|
|
||||||
elif opt == '2':
|
|
||||||
cmds = input_cmd_multiple()
|
|
||||||
elif opt == '4':
|
|
||||||
auto_mode(ser, channels)
|
|
||||||
continue
|
|
||||||
elif opt == '5':
|
|
||||||
new_ch = set_channels()
|
|
||||||
if new_ch:
|
|
||||||
channels = new_ch
|
|
||||||
continue
|
|
||||||
elif opt == '6':
|
|
||||||
blink_mode(ser)
|
|
||||||
continue
|
|
||||||
elif opt == '7': # 调用打哈欠模式
|
|
||||||
yawn_mode(ser)
|
|
||||||
continue
|
|
||||||
elif opt == '8': # 调用说话模式
|
|
||||||
talk_mode(ser)
|
|
||||||
continue
|
|
||||||
elif opt == '3':
|
|
||||||
break
|
|
||||||
else:
|
|
||||||
print("无效选项，重试")
|
|
||||||
continue
|
|
||||||
|
|
||||||
pkt = pack_commands(cmds)
|
|
||||||
print_packet(pkt)
|
|
||||||
ser.write(pkt)
|
|
||||||
print("✅ 指令已发送")
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print("\n用户中断，退出")
|
|
||||||
finally:
|
|
||||||
ser.close()
|
|
||||||
print("串口已关闭")
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
@ -1,15 +0,0 @@
|
|||||||
from .client import CMVRGrpcClient
|
|
||||||
from .enums import CMVRErrorCode, DeviceState, FingerType, RobotCartesian, RobotJointIndexDirection
|
|
||||||
from .models import (
|
|
||||||
MicState, SpeakerState, CameraState,
|
|
||||||
FreedomState, DexHandState, FacialExpressionState,
|
|
||||||
FingerTactileData, PalmTactileData, HandTactileSensors,JointCmd,Pose3D,JointState
|
|
||||||
)
|
|
||||||
|
|
||||||
__all__ = [
|
|
||||||
'CMVRGrpcClient',
|
|
||||||
'CMVRErrorCode', 'DeviceState', 'FingerType',"RobotCartesian", "RobotJointIndexDirection",
|
|
||||||
'MicState', 'SpeakerState', 'CameraState',
|
|
||||||
'FreedomState', 'DexHandState', 'FacialExpressionState',
|
|
||||||
'FingerTactileData', 'PalmTactileData', 'HandTactileSensors',"JointCmd","Pose3D","JointState"
|
|
||||||
]
|
|
||||||
@ -1,223 +0,0 @@
|
|||||||
import grpc
|
|
||||||
import time
|
|
||||||
from typing import Tuple
|
|
||||||
from datetime import datetime
|
|
||||||
|
|
||||||
from .enums import CMVRErrorCode
|
|
||||||
from .models import FacialExpressionState
|
|
||||||
|
|
||||||
class BioHeadClient:
|
|
||||||
"""仿生头客户端"""
|
|
||||||
|
|
||||||
def __init__(self, device_id: str, stub):
|
|
||||||
self.device_id = device_id
|
|
||||||
self.stub = stub
|
|
||||||
self.generated = None # 推迟导入
|
|
||||||
self.request_queue = [] # 请求队列
|
|
||||||
self.response_iterator = None # 响应迭代器
|
|
||||||
self.stream_active = False # 流是否活跃
|
|
||||||
|
|
||||||
def _import_generated(self):
|
|
||||||
"""推迟导入 generated"""
|
|
||||||
if self.generated is None:
|
|
||||||
try:
|
|
||||||
# 从正确的路径导入生成的模块
|
|
||||||
from generated.cmvr.api import common_pb2, biohead_command_pb2
|
|
||||||
self.generated = type('GeneratedModules', (), {
|
|
||||||
'common_pb2': common_pb2,
|
|
||||||
'biohead_command_pb2': biohead_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
|
|
||||||
# 使用正确的方式设置时间戳
|
|
||||||
from google.protobuf.timestamp_pb2 import Timestamp
|
|
||||||
timestamp = Timestamp()
|
|
||||||
timestamp.GetCurrentTime()
|
|
||||||
header.timestamp.CopyFrom(timestamp)
|
|
||||||
return header
|
|
||||||
|
|
||||||
def set_expression(self, expression: FacialExpressionState) -> CMVRErrorCode:
|
|
||||||
"""设置表情"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
# 创建请求
|
|
||||||
request = generated.biohead_command_pb2.SetFacialExpression.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
|
|
||||||
# 填充表情数据
|
|
||||||
expr = request.expression
|
|
||||||
expr.eyebrow.left_outside_y = expression.left_eyebrow_outside_y
|
|
||||||
expr.eyebrow.left_inside_y = expression.left_eyebrow_inside_y
|
|
||||||
expr.eyebrow.right_outside_y = expression.right_eyebrow_outside_y
|
|
||||||
expr.eyebrow.right_inside_y = expression.right_eyebrow_inside_y
|
|
||||||
|
|
||||||
expr.eyelid.left_upper_y = expression.left_eye_upper_lid_y
|
|
||||||
expr.eyelid.left_lower_y = expression.left_eye_lower_lid_y
|
|
||||||
expr.eyelid.right_upper_y = expression.right_eye_upper_lid_y
|
|
||||||
expr.eyelid.right_lower_y = expression.right_eye_lower_lid_y
|
|
||||||
|
|
||||||
expr.eyeball.left_x = expression.left_eye_ball_x
|
|
||||||
expr.eyeball.left_y = expression.left_eye_ball_y
|
|
||||||
expr.eyeball.right_x = expression.right_eye_ball_x
|
|
||||||
expr.eyeball.right_y = expression.right_eye_ball_y
|
|
||||||
|
|
||||||
expr.nose.left_y = expression.left_nose_y
|
|
||||||
expr.nose.right_y = expression.right_nose_y
|
|
||||||
|
|
||||||
expr.mouth.upper_lip_y = expression.upper_lip_y
|
|
||||||
expr.mouth.upper_lip_z = expression.upper_lip_z
|
|
||||||
expr.mouth.lower_lip_y = expression.lower_lip_y
|
|
||||||
expr.mouth.lower_lip_z = expression.lower_lip_z
|
|
||||||
|
|
||||||
expr.mouth.left_lip.upper_x = expression.upper_left_lip_x
|
|
||||||
expr.mouth.left_lip.upper_y = expression.upper_left_lip_y
|
|
||||||
expr.mouth.left_lip.corner_x = expression.left_corner_lip_x
|
|
||||||
expr.mouth.left_lip.corner_y = expression.left_corner_lip_y
|
|
||||||
expr.mouth.left_lip.lower_x = expression.lower_left_lip_x
|
|
||||||
expr.mouth.left_lip.lower_y = expression.lower_left_lip_y
|
|
||||||
|
|
||||||
expr.mouth.right_lip.upper_x = expression.upper_right_lip_x
|
|
||||||
expr.mouth.right_lip.upper_y = expression.upper_right_lip_y
|
|
||||||
expr.mouth.right_lip.corner_x = expression.right_corner_lip_x
|
|
||||||
expr.mouth.right_lip.corner_y = expression.right_corner_lip_y
|
|
||||||
expr.mouth.right_lip.lower_x = expression.lower_right_lip_x
|
|
||||||
expr.mouth.right_lip.lower_y = expression.lower_right_lip_y
|
|
||||||
|
|
||||||
expr.jaw.x = expression.jaw_x
|
|
||||||
expr.jaw.y = expression.jaw_y
|
|
||||||
|
|
||||||
# 发送请求
|
|
||||||
response = self.stub.SetExpression(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 start_stream(self):
|
|
||||||
"""开始流式会话"""
|
|
||||||
try:
|
|
||||||
if not self.stream_active:
|
|
||||||
# 创建一个请求迭代器生成器
|
|
||||||
self.request_queue = []
|
|
||||||
self.stream_active = True
|
|
||||||
self.response_iterator = self.stub.StreamExpression(self._stream_request_generator())
|
|
||||||
return CMVRErrorCode.CMVR_SUCCESS
|
|
||||||
except grpc.RpcError as e:
|
|
||||||
print(f"开始流式会话失败: {e}")
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED
|
|
||||||
|
|
||||||
def _stream_request_generator(self):
|
|
||||||
"""流式请求生成器"""
|
|
||||||
generated = self._import_generated()
|
|
||||||
while self.stream_active:
|
|
||||||
if self.request_queue:
|
|
||||||
request = self.request_queue.pop(0)
|
|
||||||
yield request
|
|
||||||
if request.eof:
|
|
||||||
break
|
|
||||||
else:
|
|
||||||
# 如果没有请求，等待一小段时间
|
|
||||||
time.sleep(0.01)
|
|
||||||
time.sleep(0.1)
|
|
||||||
|
|
||||||
def stream_expression(self, expression, is_eof=False):
|
|
||||||
"""流式控制表情"""
|
|
||||||
try:
|
|
||||||
if not self.stream_active:
|
|
||||||
result = self.start_stream()
|
|
||||||
if result != CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
return result
|
|
||||||
|
|
||||||
generated = self._import_generated()
|
|
||||||
|
|
||||||
# 创建请求
|
|
||||||
request = generated.biohead_command_pb2.StreamFacialExpression.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
request.expr.CopyFrom(expression)
|
|
||||||
request.eof = is_eof
|
|
||||||
|
|
||||||
# 将请求添加到队列
|
|
||||||
self.request_queue.append(request)
|
|
||||||
|
|
||||||
# 读取最新的响应
|
|
||||||
try:
|
|
||||||
feedback = next(self.response_iterator)
|
|
||||||
return CMVRErrorCode.CMVR_SUCCESS if feedback.header.success else CMVRErrorCode.CMVR_RPC_FAILED
|
|
||||||
except StopIteration:
|
|
||||||
return CMVRErrorCode.CMVR_SUCCESS
|
|
||||||
|
|
||||||
except grpc.RpcError as e:
|
|
||||||
print(f"流式控制表情失败: {e}")
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED
|
|
||||||
|
|
||||||
def end_stream(self):
|
|
||||||
"""结束流式会话"""
|
|
||||||
try:
|
|
||||||
if self.stream_active:
|
|
||||||
# 发送结束信号
|
|
||||||
generated = self._import_generated()
|
|
||||||
request = generated.biohead_command_pb2.StreamFacialExpression.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
request.eof = True
|
|
||||||
|
|
||||||
self.request_queue.append(request)
|
|
||||||
|
|
||||||
# 等待所有响应
|
|
||||||
for feedback in self.response_iterator:
|
|
||||||
if not feedback.header.success:
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED
|
|
||||||
|
|
||||||
self.stream_active = False
|
|
||||||
self.request_queue = []
|
|
||||||
self.response_iterator = None
|
|
||||||
|
|
||||||
return CMVRErrorCode.CMVR_SUCCESS
|
|
||||||
except grpc.RpcError as e:
|
|
||||||
print(f"结束流式会话失败: {e}")
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED
|
|
||||||
|
|
||||||
def get_system_status(self):
|
|
||||||
"""获取系统状态"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
request = generated.biohead_command_pb2.GetStatus.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
|
|
||||||
response = self.stub.GetSystemStatus(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 emergency_stop(self):
|
|
||||||
"""紧急停止"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
request = generated.biohead_command_pb2.EmergencyStop.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
|
|
||||||
response = self.stub.EmergencyStop(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 close(self):
|
|
||||||
"""关闭连接"""
|
|
||||||
if self.stream_active:
|
|
||||||
self.end_stream()
|
|
||||||
@ -1,443 +0,0 @@
|
|||||||
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)
|
|
||||||
156
cmvr/client.py
156
cmvr/client.py
@ -1,156 +0,0 @@
|
|||||||
import grpc
|
|
||||||
from typing import Dict
|
|
||||||
|
|
||||||
from .enums import CMVRErrorCode
|
|
||||||
from .biohead_client import BioHeadClient
|
|
||||||
from .camera_client import CameraClient
|
|
||||||
from .dexhand_client import DexHandClient
|
|
||||||
from .micphone_client import MicphoneClient
|
|
||||||
from .speaker_client import SpeakerClient
|
|
||||||
from .humanoid_robot import HumanoidRobotClient
|
|
||||||
class CMVRGrpcClient:
|
|
||||||
"""CMVR gRPC 主客户端"""
|
|
||||||
|
|
||||||
def __init__(self, server_address: str):
|
|
||||||
self.server_address = server_address
|
|
||||||
self.connected = False
|
|
||||||
|
|
||||||
# 创建 gRPC 通道
|
|
||||||
# 设置通道选项，增加最大接收和发送消息大小
|
|
||||||
max_message_size: int = 20 * 1024 * 1024
|
|
||||||
options = [
|
|
||||||
('grpc.max_send_message_length', max_message_size),
|
|
||||||
('grpc.max_receive_message_length', max_message_size),
|
|
||||||
]
|
|
||||||
self.channel = grpc.insecure_channel(server_address,options=options)
|
|
||||||
|
|
||||||
# 初始化各服务的存根
|
|
||||||
self.generated = None # 推迟导入
|
|
||||||
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] = {}
|
|
||||||
|
|
||||||
self.micphone_stub = None
|
|
||||||
self.micphone_map: Dict[str, MicphoneClient] = {}
|
|
||||||
|
|
||||||
self.speaker_stub = None
|
|
||||||
self.speaker_map: Dict[str, SpeakerClient] = {}
|
|
||||||
|
|
||||||
self.humanoid_robot_stub = None
|
|
||||||
self.humanoid_robot_map: Dict[str, HumanoidRobotClient] = {}
|
|
||||||
|
|
||||||
# 检查连接状态
|
|
||||||
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)
|
|
||||||
|
|
||||||
self.speaker_stub = self.generated.speaker_service_pb2_grpc.SpeakerServiceStub(self.channel)
|
|
||||||
|
|
||||||
self.micphone_stub = self.generated.microphone_service_pb2_grpc.MicPhoneServiceStub(self.channel)
|
|
||||||
|
|
||||||
self.humanoid_robot_stub = self.generated.humanoid_robot_sevice_pb2_grpc.HumanoidRobotServiceStub(self.channel)
|
|
||||||
|
|
||||||
except grpc.FutureTimeoutError:
|
|
||||||
print(f"连接服务器超时: {server_address}")
|
|
||||||
self.connected = False
|
|
||||||
|
|
||||||
def _import_generated(self):
|
|
||||||
"""推迟导入 generated"""
|
|
||||||
if self.generated is None:
|
|
||||||
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
|
|
||||||
from generated.cmvr.api import speaker_command_pb2
|
|
||||||
from generated.cmvr.api import speaker_service_pb2_grpc
|
|
||||||
from generated.cmvr.api import microphone_command_pb2
|
|
||||||
from generated.cmvr.api import microphone_service_pb2_grpc
|
|
||||||
|
|
||||||
from generated.cmvr.api import humanoid_robot_command_pb2
|
|
||||||
from generated.cmvr.api import humanoid_robot_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,
|
|
||||||
'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,
|
|
||||||
'speaker_service_pb2_grpc': speaker_service_pb2_grpc,
|
|
||||||
'speaker_command_pb2': speaker_command_pb2,
|
|
||||||
'microphone_service_pb2_grpc': microphone_service_pb2_grpc,
|
|
||||||
'microphone_command_pb2': microphone_command_pb2,
|
|
||||||
'humanoid_robot_sevice_pb2_grpc': humanoid_robot_service_pb2_grpc,
|
|
||||||
'humanoid_robot_command_pb2': humanoid_robot_command_pb2
|
|
||||||
})
|
|
||||||
|
|
||||||
except ImportError as e:
|
|
||||||
print(f"导入生成的模块失败: {e}")
|
|
||||||
print("请确保已生成 protobuf 代码")
|
|
||||||
raise
|
|
||||||
return self.generated
|
|
||||||
|
|
||||||
def is_connected(self) -> bool:
|
|
||||||
"""检查连接状态"""
|
|
||||||
return self.connected
|
|
||||||
|
|
||||||
def get_biohead(self, device_id: str) -> BioHeadClient:
|
|
||||||
"""获取仿生头客户端"""
|
|
||||||
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 get_speaker(self, device_id: str) -> SpeakerClient:
|
|
||||||
if device_id not in self.speaker_map:
|
|
||||||
self.speaker_map[device_id] = SpeakerClient(device_id, self.speaker_stub)
|
|
||||||
return self.speaker_map[device_id]
|
|
||||||
|
|
||||||
def get_micphone(self, device_id: str) -> MicphoneClient:
|
|
||||||
if device_id not in self.micphone_map:
|
|
||||||
self.micphone_map[device_id] = MicphoneClient(device_id, self.micphone_stub)
|
|
||||||
return self.micphone_map[device_id]
|
|
||||||
|
|
||||||
def get_humanoid_robot(self, device_id: str) -> HumanoidRobotClient:
|
|
||||||
if device_id not in self.micphone_map:
|
|
||||||
self.humanoid_robot_map[device_id] = HumanoidRobotClient(device_id, self.humanoid_robot_stub)
|
|
||||||
return self.humanoid_robot_map[device_id]
|
|
||||||
|
|
||||||
def close(self):
|
|
||||||
"""关闭所有连接"""
|
|
||||||
# 关闭所有流式连接
|
|
||||||
for biohead in self.biohead_map.values():
|
|
||||||
biohead.close()
|
|
||||||
|
|
||||||
# 关闭 gRPC 通道
|
|
||||||
self.channel.close()
|
|
||||||
|
|
||||||
# 清空所有映射
|
|
||||||
self.biohead_map.clear()
|
|
||||||
|
|
||||||
self.connected = False
|
|
||||||
@ -1,339 +0,0 @@
|
|||||||
import grpc
|
|
||||||
import numpy as np
|
|
||||||
from typing import Tuple
|
|
||||||
from typing import Dict
|
|
||||||
|
|
||||||
from .enums import CMVRErrorCode, FingerType
|
|
||||||
from .models import DexHandState, FreedomState, HandTactileSensors
|
|
||||||
|
|
||||||
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()
|
|
||||||
return header
|
|
||||||
|
|
||||||
def get_status(self) -> Tuple[CMVRErrorCode, DexHandState]:
|
|
||||||
"""获取灵巧手状态"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
request = generated.dexhand_command_pb2.GetDexHandStateCommand.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
|
|
||||||
response = self.stub.GetStatus(request)
|
|
||||||
|
|
||||||
if not response.header.success:
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED, DexHandState()
|
|
||||||
|
|
||||||
state = DexHandState(
|
|
||||||
is_initialized=response.state.is_initialized
|
|
||||||
)
|
|
||||||
|
|
||||||
# 填充手指状态
|
|
||||||
for i, hand_state in enumerate(response.state.hands):
|
|
||||||
if i < len(state.hands):
|
|
||||||
state.hands[i] = FreedomState(
|
|
||||||
angle=hand_state.angle,
|
|
||||||
speed=hand_state.speed,
|
|
||||||
force=hand_state.force,
|
|
||||||
position=hand_state.position,
|
|
||||||
current=hand_state.current,
|
|
||||||
temperature=hand_state.temperature,
|
|
||||||
error=hand_state.error,
|
|
||||||
error_message=list(hand_state.error_message)
|
|
||||||
)
|
|
||||||
|
|
||||||
return CMVRErrorCode.CMVR_SUCCESS, state
|
|
||||||
|
|
||||||
except grpc.RpcError as e:
|
|
||||||
print(f"获取灵巧手状态失败: {e}")
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED, DexHandState()
|
|
||||||
|
|
||||||
def set_angle(self, angle_map: Dict[str, float]) -> CMVRErrorCode:
|
|
||||||
"""
|
|
||||||
设置DexHand的角度
|
|
||||||
|
|
||||||
参数:
|
|
||||||
angle_map: 字典，键为自由度ID（字符串），值为角度百分比 (0-1)
|
|
||||||
有效字符串ID："little_finger"（小拇指）、"ring_finger"（无名指）、
|
|
||||||
"middle_finger"（中指）、"index_finger"（食指）、
|
|
||||||
"thumb_bend"（大拇指弯曲）、"thumb_rotate"（大拇指旋转）
|
|
||||||
对应proto中的FreedomValue (id和value)
|
|
||||||
|
|
||||||
返回:
|
|
||||||
CMVRErrorCode: 操作结果状态码
|
|
||||||
"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
# 有效自由度字符串ID列表（与proto定义一致）
|
|
||||||
valid_dof_ids = {
|
|
||||||
"little_finger", "ring_finger", "middle_finger",
|
|
||||||
"index_finger", "thumb_bend", "thumb_rotate"
|
|
||||||
}
|
|
||||||
|
|
||||||
# 创建角度设置请求对象
|
|
||||||
request = generated.dexhand_command_pb2.SetDexHandAnglesCommand.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
|
|
||||||
# 将字典转换为protobuf的FreedomValue列表
|
|
||||||
for dof_id, angle_value in angle_map.items():
|
|
||||||
# 验证自由度ID有效性（字符串）
|
|
||||||
if dof_id not in valid_dof_ids:
|
|
||||||
print(f"警告: 无效的自由度ID '{dof_id}'，有效ID为：{valid_dof_ids}")
|
|
||||||
continue
|
|
||||||
# 验证角度值范围（0-1）
|
|
||||||
if not (0 <= angle_value <= 1):
|
|
||||||
print(f"警告: 角度值 {angle_value} 超出范围，必须在0-1之间")
|
|
||||||
angle_value = max(0, min(1, angle_value)) # 限制在有效范围
|
|
||||||
|
|
||||||
# 添加自由度角度设置（直接使用字符串ID）
|
|
||||||
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"设置DexHand角度失败: {e}")
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED
|
|
||||||
|
|
||||||
def get_sensor_data(self) -> Tuple[CMVRErrorCode, HandTactileSensors]:
|
|
||||||
"""
|
|
||||||
获取灵巧手所有触觉传感器数据
|
|
||||||
将Protobuf的SensorData转换为HandTactileSensors对象
|
|
||||||
|
|
||||||
返回:
|
|
||||||
Tuple[CMVRErrorCode, HandTactileSensors]: 错误码和传感器数据对象
|
|
||||||
"""
|
|
||||||
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()
|
|
||||||
|
|
||||||
# 初始化传感器数据容器
|
|
||||||
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, HandTactileSensors()
|
|
||||||
|
|
||||||
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()
|
|
||||||
@ -1,52 +0,0 @@
|
|||||||
from enum import Enum
|
|
||||||
|
|
||||||
class CMVRErrorCode(Enum):
|
|
||||||
"""错误码定义"""
|
|
||||||
CMVR_SUCCESS = 0
|
|
||||||
CMVR_CONNECT_FAILED = 1
|
|
||||||
CMVR_INVALID_PARAM = 2
|
|
||||||
CMVR_RPC_FAILED = 3
|
|
||||||
CMVR_NOT_CONNECTED = 4
|
|
||||||
CMVR_INTERNAL_ERROR = 5
|
|
||||||
|
|
||||||
class DeviceState(Enum):
|
|
||||||
"""设备状态"""
|
|
||||||
STATE_INIT = 0
|
|
||||||
STATE_READY = 1
|
|
||||||
STATE_RUNNING = 2
|
|
||||||
STATE_ERROR = 3
|
|
||||||
STATE_ESTOP = 4
|
|
||||||
STATE_STOP = 5
|
|
||||||
|
|
||||||
class FingerType(Enum):
|
|
||||||
"""手指类型枚举"""
|
|
||||||
PINKY = 0 # 小拇指
|
|
||||||
RING = 1 # 无名指
|
|
||||||
MIDDLE = 2 # 中指
|
|
||||||
INDEX = 3 # 食指
|
|
||||||
THUMB = 4 # 大拇指
|
|
||||||
PALM = 5 # 掌心
|
|
||||||
|
|
||||||
# 机器人相关枚举定义
|
|
||||||
class RobotCartesian(Enum):
|
|
||||||
"""机器人笛卡尔坐标系枚举"""
|
|
||||||
X = 0
|
|
||||||
Y = 1
|
|
||||||
Z = 2
|
|
||||||
RX = 3 # 绕X轴旋转
|
|
||||||
RY = 4 # 绕Y轴旋转
|
|
||||||
RZ = 5 # 绕Z轴旋转
|
|
||||||
|
|
||||||
class RobotJointIndexDirection(Enum):
|
|
||||||
"""机器人关节索引方向枚举"""
|
|
||||||
FORWARD = 0
|
|
||||||
BACKWARD = 1
|
|
||||||
X_POSITIVE = 2 # X轴正向
|
|
||||||
X_NEGATIVE = 3 # X轴负向
|
|
||||||
Y_POSITIVE = 4 # Y轴正向
|
|
||||||
Y_NEGATIVE = 5 # Y轴负向
|
|
||||||
Z_POSITIVE = 6 # Z轴正向
|
|
||||||
Z_NEGATIVE = 7 # Z轴负向
|
|
||||||
ROTATE_X = 8 # 绕X轴旋转
|
|
||||||
ROTATE_Y = 9 # 绕Y轴旋转
|
|
||||||
ROTATE_Z = 10 # 绕Z轴旋转
|
|
||||||
@ -1,279 +0,0 @@
|
|||||||
import grpc
|
|
||||||
import numpy as np
|
|
||||||
from typing import List, Optional, Tuple
|
|
||||||
from typing import Dict
|
|
||||||
|
|
||||||
from .enums import CMVRErrorCode, RobotCartesian, RobotJointIndexDirection
|
|
||||||
from .models import JointCmd, Pose3D, JointState
|
|
||||||
|
|
||||||
class HumanoidRobotClient:
|
|
||||||
"""人形机器人客户端"""
|
|
||||||
|
|
||||||
def __init__(self, device_id: str, stub):
|
|
||||||
"""
|
|
||||||
初始化机器人客户端
|
|
||||||
|
|
||||||
参数:
|
|
||||||
device_id: 设备ID
|
|
||||||
stub: gRPC 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, humanoid_robot_command_pb2
|
|
||||||
self.generated = type('GeneratedModules', (), {
|
|
||||||
'common_pb2': common_pb2,
|
|
||||||
'humanoid_robot_command_pb2': humanoid_robot_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 torqueOn(self) -> CMVRErrorCode:
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
request = generated.common_pb2.CommandHeader.Request()
|
|
||||||
request.CopyFrom(self._create_command_header())
|
|
||||||
|
|
||||||
response = self.stub.torqueOn(request)
|
|
||||||
|
|
||||||
return CMVRErrorCode.CMVR_SUCCESS if response.success else CMVRErrorCode.CMVR_RPC_FAILED
|
|
||||||
|
|
||||||
except grpc.RpcError as e:
|
|
||||||
print(f"开启使能失败: {e}")
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED
|
|
||||||
|
|
||||||
def torqueOff(self) -> CMVRErrorCode:
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
request = generated.common_pb2.CommandHeader.Request()
|
|
||||||
request.CopyFrom(self._create_command_header())
|
|
||||||
|
|
||||||
response = self.stub.torqueOff(request)
|
|
||||||
|
|
||||||
return CMVRErrorCode.CMVR_SUCCESS if response.success else CMVRErrorCode.CMVR_RPC_FAILED
|
|
||||||
|
|
||||||
except grpc.RpcError as e:
|
|
||||||
print(f"关闭使能失败: {e}")
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED
|
|
||||||
|
|
||||||
# ==================== 关节空间运动 ====================
|
|
||||||
|
|
||||||
def moveJ(self, joint_commands: List[JointCmd],
|
|
||||||
overall_vel: float = 0.1, overall_acc: float = 0.5) -> Tuple[CMVRErrorCode, Optional[str]]:
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
# 创建MoveJ请求
|
|
||||||
request = generated.humanoid_robot_command_pb2.MoveJ.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
request.vel = overall_vel
|
|
||||||
request.acc = overall_acc
|
|
||||||
|
|
||||||
# 添加关节命令
|
|
||||||
for cmd in joint_commands:
|
|
||||||
joint_cmd = request.cmds.add()
|
|
||||||
joint_cmd.joint_name = cmd.joint_name
|
|
||||||
joint_cmd.rad = cmd.rad
|
|
||||||
joint_cmd.vel = cmd.vel
|
|
||||||
|
|
||||||
# 调用RPC接口
|
|
||||||
response = self.stub.moveJ(request)
|
|
||||||
|
|
||||||
return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED
|
|
||||||
|
|
||||||
except grpc.RpcError as e:
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED
|
|
||||||
|
|
||||||
# ==================== 直线空间运动 ====================
|
|
||||||
|
|
||||||
def moveL(self, ee_link: str, target_pose: Pose3D,
|
|
||||||
vel: float = 0.1, acc: float = 0.5) -> Tuple[CMVRErrorCode, Optional[str]]:
|
|
||||||
"""
|
|
||||||
直线空间运动 (MoveL)
|
|
||||||
|
|
||||||
参数:
|
|
||||||
ee_link: 末端执行器连杆名称
|
|
||||||
target_pose: 目标位姿 (Pose3D)
|
|
||||||
vel: 速度 (m/s)
|
|
||||||
acc: 加速度 (m/s²)
|
|
||||||
|
|
||||||
返回:
|
|
||||||
Tuple[CMVRErrorCode, Optional[str]]: 错误码和错误消息
|
|
||||||
"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
# 创建MoveL请求
|
|
||||||
request = generated.humanoid_robot_command_pb2.MoveL.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
request.ee_link = ee_link
|
|
||||||
request.vel = vel
|
|
||||||
request.acc = acc
|
|
||||||
|
|
||||||
# 设置目标位姿
|
|
||||||
request.target_pose.x = target_pose.x
|
|
||||||
request.target_pose.y = target_pose.y
|
|
||||||
request.target_pose.z = target_pose.z
|
|
||||||
request.target_pose.rx = target_pose.rx
|
|
||||||
request.target_pose.ry = target_pose.ry
|
|
||||||
request.target_pose.rz = target_pose.rz
|
|
||||||
|
|
||||||
# 调用RPC接口
|
|
||||||
response = self.stub.moveL(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 speedJ(self, joint_name: str, velocity: float,
|
|
||||||
direction: RobotJointIndexDirection = RobotJointIndexDirection.FORWARD,
|
|
||||||
acc: float = 0.5) -> Tuple[CMVRErrorCode, Optional[str]]:
|
|
||||||
"""
|
|
||||||
关节速度控制 (SpeedJ)
|
|
||||||
|
|
||||||
参数:
|
|
||||||
joint_name: 关节名称
|
|
||||||
velocity: 速度 (rad/s)
|
|
||||||
direction: 运动方向
|
|
||||||
acc: 加速度 (rad/s²)
|
|
||||||
|
|
||||||
返回:
|
|
||||||
Tuple[CMVRErrorCode, Optional[str]]: 错误码和错误消息
|
|
||||||
"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
# 创建SpeedJ请求
|
|
||||||
request = generated.humanoid_robot_command_pb2.SpeedJ.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
request.joint_name = joint_name
|
|
||||||
request.vel = abs(velocity) # 确保速度为正值
|
|
||||||
request.acc = acc
|
|
||||||
request.direction = direction.value
|
|
||||||
|
|
||||||
# 调用RPC接口
|
|
||||||
response = self.stub.speedJ(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, str(e)
|
|
||||||
|
|
||||||
# ==================== 笛卡尔速度控制 ====================
|
|
||||||
|
|
||||||
def speedL(self, ee_link: str, velocity: float,
|
|
||||||
cartesian: RobotCartesian = RobotCartesian.X,
|
|
||||||
direction: RobotJointIndexDirection = RobotJointIndexDirection.FORWARD,
|
|
||||||
acc: float = 0.5) -> Tuple[CMVRErrorCode, Optional[str]]:
|
|
||||||
"""
|
|
||||||
笛卡尔速度控制 (SpeedL)
|
|
||||||
|
|
||||||
参数:
|
|
||||||
ee_link: 末端执行器连杆名称
|
|
||||||
velocity: 速度 (m/s 或 rad/s，取决于cartesian类型)
|
|
||||||
cartesian: 笛卡尔坐标轴
|
|
||||||
direction: 运动方向
|
|
||||||
acc: 加速度 (m/s² 或 rad/s²)
|
|
||||||
|
|
||||||
返回:
|
|
||||||
Tuple[CMVRErrorCode, Optional[str]]: 错误码和错误消息
|
|
||||||
"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
# 创建SpeedL请求
|
|
||||||
request = generated.humanoid_robot_command_pb2.SpeedL.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
request.ee_link = ee_link
|
|
||||||
request.vel = abs(velocity)
|
|
||||||
request.acc = acc
|
|
||||||
request.cartesian = cartesian.value
|
|
||||||
request.direction = direction.value
|
|
||||||
|
|
||||||
# 调用RPC接口
|
|
||||||
response = self.stub.speedL(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, str(e)
|
|
||||||
|
|
||||||
# ==================== 状态获取 ====================
|
|
||||||
|
|
||||||
def get_joint_states(self) -> Tuple[CMVRErrorCode, List[JointState]]:
|
|
||||||
"""
|
|
||||||
获取机器人所有关节状态
|
|
||||||
|
|
||||||
返回:
|
|
||||||
Tuple[CMVRErrorCode, List[JointState]]: 错误码和关节状态对象列表
|
|
||||||
"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
# 创建关节状态请求
|
|
||||||
request = generated.humanoid_robot_command_pb2.JointRequest()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
|
|
||||||
# 调用RPC接口
|
|
||||||
response = self.stub.getJointState(request)
|
|
||||||
|
|
||||||
if not response.header.success:
|
|
||||||
print(f"RPC调用失败: {response.header}")
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED, []
|
|
||||||
|
|
||||||
# 检查是否有状态数据
|
|
||||||
if not response.state:
|
|
||||||
print("响应中没有关节状态数据")
|
|
||||||
return CMVRErrorCode.CMVR_NO_DATA, []
|
|
||||||
|
|
||||||
# 解析所有JointState
|
|
||||||
joint_states = []
|
|
||||||
for proto_joint_state in response.state:
|
|
||||||
# 解析响应数据到JointState对象
|
|
||||||
# 注意proto字段名和Python dataclass字段名的映射
|
|
||||||
joint_state = JointState(
|
|
||||||
name=list(proto_joint_state.name), # proto: name -> Python: name
|
|
||||||
position=list(proto_joint_state.position), # proto: position -> Python: position
|
|
||||||
velocity=list(proto_joint_state.velocity), # proto: velocity -> Python: velocity
|
|
||||||
effort=list(proto_joint_state.effort), # proto: effort -> Python: effort
|
|
||||||
timestamp=proto_joint_state.timestamp # proto: timestamp -> Python: timestamp
|
|
||||||
)
|
|
||||||
joint_states.append(joint_state)
|
|
||||||
|
|
||||||
if not joint_states:
|
|
||||||
print("解析后没有有效的关节状态数据")
|
|
||||||
return CMVRErrorCode.CMVR_NO_DATA, []
|
|
||||||
|
|
||||||
return CMVRErrorCode.CMVR_SUCCESS, joint_states
|
|
||||||
|
|
||||||
except grpc.RpcError as e:
|
|
||||||
print(f"gRPC错误 - 获取关节状态失败: {e}")
|
|
||||||
if hasattr(e, 'code'):
|
|
||||||
if e.code() == grpc.StatusCode.NOT_FOUND:
|
|
||||||
return CMVRErrorCode.CMVR_NOT_FOUND, []
|
|
||||||
elif e.code() == grpc.StatusCode.UNAVAILABLE:
|
|
||||||
return CMVRErrorCode.CMVR_CONNECTION_FAILED, []
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED, []
|
|
||||||
|
|
||||||
except Exception as e:
|
|
||||||
print(f"未知错误 - 获取关节状态失败: {e}")
|
|
||||||
import traceback
|
|
||||||
traceback.print_exc()
|
|
||||||
return CMVRErrorCode.CMVR_UNKNOWN_ERROR, []
|
|
||||||
@ -1,94 +0,0 @@
|
|||||||
import grpc
|
|
||||||
from typing import Tuple
|
|
||||||
|
|
||||||
from .enums import CMVRErrorCode
|
|
||||||
from .models import MicState
|
|
||||||
|
|
||||||
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()
|
|
||||||
return header
|
|
||||||
|
|
||||||
def get_status(self) -> Tuple[CMVRErrorCode, MicState]:
|
|
||||||
"""获取麦克风状态"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
request = generated.microphone_command_pb2.GetMicStateCommand.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
|
|
||||||
response = self.stub.GetStatus(request)
|
|
||||||
|
|
||||||
if not response.header.success:
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED, MicState()
|
|
||||||
|
|
||||||
state = MicState(
|
|
||||||
is_initialized=response.state.is_initialized,
|
|
||||||
is_running=response.state.is_running,
|
|
||||||
is_recording=response.state.is_recording,
|
|
||||||
volume=response.state.volume,
|
|
||||||
error_message=response.state.error_message
|
|
||||||
)
|
|
||||||
|
|
||||||
return CMVRErrorCode.CMVR_SUCCESS, state
|
|
||||||
|
|
||||||
except grpc.RpcError as e:
|
|
||||||
print(f"获取麦克风状态失败: {e}")
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED, MicState()
|
|
||||||
|
|
||||||
def start_record(self, audio_path: str) -> CMVRErrorCode:
|
|
||||||
"""开始录音"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
request = generated.microphone_command_pb2.StartMicRecordingCommand.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
request.file_path = audio_path
|
|
||||||
|
|
||||||
response = self.stub.StartRecord(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.microphone_command_pb2.StopMicRecordingCommand.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
|
|
||||||
response = self.stub.StopRecord(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
|
|
||||||
277
cmvr/models.py
277
cmvr/models.py
@ -1,277 +0,0 @@
|
|||||||
from dataclasses import dataclass, field
|
|
||||||
from typing import List
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class MicState:
|
|
||||||
"""麦克风状态"""
|
|
||||||
is_initialized: bool = False
|
|
||||||
is_running: bool = False
|
|
||||||
is_recording: bool = False
|
|
||||||
volume: int = 0
|
|
||||||
error_message: str = ""
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class SpeakerState:
|
|
||||||
"""扬声器状态"""
|
|
||||||
is_initialized: bool = False
|
|
||||||
is_running: bool = False
|
|
||||||
is_decoding: bool = False
|
|
||||||
is_paused: bool = False
|
|
||||||
volume: int = 0
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class CameraState:
|
|
||||||
"""相机状态"""
|
|
||||||
is_initialized: bool = False
|
|
||||||
is_opened: bool = False
|
|
||||||
is_streaming: bool = False
|
|
||||||
is_recording: bool = False
|
|
||||||
width: int = 0
|
|
||||||
height: int = 0
|
|
||||||
fps: int = 0
|
|
||||||
@dataclass
|
|
||||||
class CameraIntrinsics:
|
|
||||||
"""相机内参（与Protobuf CameraIntrinsics对齐）"""
|
|
||||||
cx: float = 0.0 # 对应Protobuf tag=1
|
|
||||||
cy: float = 0.0 # 对应Protobuf tag=2
|
|
||||||
fx: float = 0.0 # 对应Protobuf tag=3
|
|
||||||
fy: float = 0.0 # 对应Protobuf tag=4
|
|
||||||
# 畸变系数：固定5个元素（k1, k2, p1, p2, k3），与Protobuf tag=5对齐
|
|
||||||
coeffs: List[float] = field(default_factory=lambda: [0.0]*5)
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class FreedomState:
|
|
||||||
"""灵巧手自由度状态"""
|
|
||||||
dof_id: str = ""
|
|
||||||
angle: int = 0
|
|
||||||
speed: int = 0
|
|
||||||
force: int = 0
|
|
||||||
position: int = 0
|
|
||||||
current: int = 0
|
|
||||||
temperature: int = 0
|
|
||||||
error: int = 0
|
|
||||||
error_message: List[str] = field(default_factory=list)
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class DexHandState:
|
|
||||||
"""灵巧手整体状态"""
|
|
||||||
is_initialized: bool = False
|
|
||||||
hands: List[FreedomState] = field(default_factory=lambda: [FreedomState() for _ in range(6)])
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class FacialExpressionState:
|
|
||||||
"""面部表情状态"""
|
|
||||||
# 眉毛
|
|
||||||
left_eyebrow_outside_y: float = 0.0
|
|
||||||
left_eyebrow_inside_y: float = 0.0
|
|
||||||
right_eyebrow_outside_y: float = 0.0
|
|
||||||
right_eyebrow_inside_y: float = 0.0
|
|
||||||
|
|
||||||
# 眼睑
|
|
||||||
left_eye_upper_lid_y: float = 0.0
|
|
||||||
left_eye_lower_lid_y: float = 0.0
|
|
||||||
right_eye_upper_lid_y: float = 0.0
|
|
||||||
right_eye_lower_lid_y: float = 0.0
|
|
||||||
|
|
||||||
# 眼球
|
|
||||||
left_eye_ball_x: float = 0.0
|
|
||||||
left_eye_ball_y: float = 0.0
|
|
||||||
right_eye_ball_x: float = 0.0
|
|
||||||
right_eye_ball_y: float = 0.0
|
|
||||||
|
|
||||||
# 鼻子
|
|
||||||
left_nose_y: float = 0.0
|
|
||||||
right_nose_y: float = 0.0
|
|
||||||
|
|
||||||
# 嘴巴
|
|
||||||
upper_lip_y: float = 0.0
|
|
||||||
upper_lip_z: float = 0.0
|
|
||||||
lower_lip_y: float = 0.0
|
|
||||||
lower_lip_z: float = 0.0
|
|
||||||
|
|
||||||
# 左唇角
|
|
||||||
upper_left_lip_x: float = 0.0
|
|
||||||
upper_left_lip_y: float = 0.0
|
|
||||||
left_corner_lip_x: float = 0.0
|
|
||||||
left_corner_lip_y: float = 0.0
|
|
||||||
lower_left_lip_x: float = 0.0
|
|
||||||
lower_left_lip_y: float = 0.0
|
|
||||||
|
|
||||||
# 右唇角
|
|
||||||
upper_right_lip_x: float = 0.0
|
|
||||||
upper_right_lip_y: float = 0.0
|
|
||||||
right_corner_lip_x: float = 0.0
|
|
||||||
right_corner_lip_y: float = 0.0
|
|
||||||
lower_right_lip_x: float = 0.0
|
|
||||||
lower_right_lip_y: float = 0.0
|
|
||||||
|
|
||||||
# 下巴
|
|
||||||
jaw_x: float = 0.0
|
|
||||||
jaw_y: float = 0.0
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class FingerTactileData:
|
|
||||||
"""手指触觉数据"""
|
|
||||||
data: np.ndarray = None
|
|
||||||
rows: int = 0
|
|
||||||
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)
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class PalmTactileData:
|
|
||||||
"""手掌触觉数据"""
|
|
||||||
data: np.ndarray = None
|
|
||||||
rows: int = 8
|
|
||||||
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)
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
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: "无名指",
|
|
||||||
FingerType.MIDDLE: "中指",
|
|
||||||
FingerType.INDEX: "食指",
|
|
||||||
FingerType.THUMB: "大拇指"
|
|
||||||
}
|
|
||||||
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)")
|
|
||||||
|
|
||||||
# 机器人相关数据结构
|
|
||||||
@dataclass
|
|
||||||
class JointCmd:
|
|
||||||
"""关节控制命令"""
|
|
||||||
joint_name: str = "" # 关节名称
|
|
||||||
rad: float = 0.0 # 弧度
|
|
||||||
vel: float = 0.0 # 角速度，rad/s
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class Pose3D:
|
|
||||||
"""3D位姿"""
|
|
||||||
x: float = 0.0 # X坐标
|
|
||||||
y: float = 0.0 # Y坐标
|
|
||||||
z: float = 0.0 # Z坐标
|
|
||||||
rx: float = 0.0 # 绕X轴旋转（弧度）
|
|
||||||
ry: float = 0.0 # 绕Y轴旋转（弧度）
|
|
||||||
rz: float = 0.0 # 绕Z轴旋转（弧度）
|
|
||||||
|
|
||||||
def to_array(self) -> np.ndarray:
|
|
||||||
"""转换为numpy数组"""
|
|
||||||
return np.array([self.x, self.y, self.z, self.rx, self.ry, self.rz])
|
|
||||||
|
|
||||||
@classmethod
|
|
||||||
def from_array(cls, arr: np.ndarray) -> 'Pose3D':
|
|
||||||
"""从numpy数组创建Pose3D"""
|
|
||||||
return cls(arr[0], arr[1], arr[2], arr[3], arr[4], arr[5])
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class JointState:
|
|
||||||
"""关节状态"""
|
|
||||||
name: List[str] = field(default_factory=list) # 关节名称列表
|
|
||||||
position: List[float] = field(default_factory=list) # 关节位置列表（弧度）
|
|
||||||
velocity: List[float] = field(default_factory=list) # 关节速度列表（rad/s）
|
|
||||||
effort: List[float] = field(default_factory=list) # 关节力矩列表
|
|
||||||
timestamp: float = 0.0 # 时间戳
|
|
||||||
|
|
||||||
def __post_init__(self):
|
|
||||||
"""确保列表长度一致"""
|
|
||||||
if len(self.name) > 0:
|
|
||||||
n_joints = len(self.name)
|
|
||||||
self.position = self.position[:n_joints] + [0.0] * (n_joints - len(self.position))
|
|
||||||
self.velocity = self.velocity[:n_joints] + [0.0] * (n_joints - len(self.velocity))
|
|
||||||
self.effort = self.effort[:n_joints] + [0.0] * (n_joints - len(self.effort))
|
|
||||||
|
|
||||||
def get_joint_by_name(self, joint_name: str) -> tuple:
|
|
||||||
"""根据关节名称获取状态"""
|
|
||||||
if joint_name in self.name:
|
|
||||||
idx = self.name.index(joint_name)
|
|
||||||
return (self.position[idx], self.velocity[idx], self.effort[idx])
|
|
||||||
return (0.0, 0.0, 0.0)
|
|
||||||
|
|
||||||
def to_dict(self) -> dict:
|
|
||||||
"""转换为字典格式"""
|
|
||||||
return {
|
|
||||||
'name': self.name,
|
|
||||||
'positions': self.position,
|
|
||||||
'velocities': self.velocity,
|
|
||||||
'efforts': self.effort,
|
|
||||||
'timestamp': self.timestamp
|
|
||||||
}
|
|
||||||
@ -1,158 +0,0 @@
|
|||||||
import grpc
|
|
||||||
from typing import Tuple
|
|
||||||
|
|
||||||
from .enums import CMVRErrorCode
|
|
||||||
from .models import SpeakerState
|
|
||||||
|
|
||||||
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()
|
|
||||||
return header
|
|
||||||
|
|
||||||
def get_status(self) -> Tuple[CMVRErrorCode, SpeakerState]:
|
|
||||||
"""获取扬声器状态"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
request = generated.speaker_command_pb2.GetSpeakerStateCommand.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
|
|
||||||
response = self.stub.GetStatus(request)
|
|
||||||
|
|
||||||
if not response.header.success:
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED, SpeakerState()
|
|
||||||
|
|
||||||
state = SpeakerState(
|
|
||||||
is_initialized=response.state.is_initialized,
|
|
||||||
is_running=response.state.is_running,
|
|
||||||
is_decoding=response.state.is_decoding,
|
|
||||||
is_paused=response.state.is_paused,
|
|
||||||
volume=response.state.volume
|
|
||||||
)
|
|
||||||
|
|
||||||
return CMVRErrorCode.CMVR_SUCCESS, state
|
|
||||||
|
|
||||||
except grpc.RpcError as e:
|
|
||||||
print(f"获取扬声器状态失败: {e}")
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED, SpeakerState()
|
|
||||||
|
|
||||||
def play_audio(self, audio_path: str) -> CMVRErrorCode:
|
|
||||||
"""播放音频"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
request = generated.speaker_command_pb2.PlayAudioCommand.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
request.audio_path = audio_path
|
|
||||||
|
|
||||||
response = self.stub.PlayAudio(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 pause_audio(self) -> CMVRErrorCode:
|
|
||||||
"""暂停播放"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
request = generated.speaker_command_pb2.PauseSpeakerCommand.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
|
|
||||||
response = self.stub.PausePlayback(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 resume_audio(self) -> CMVRErrorCode:
|
|
||||||
"""继续播放"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
request = generated.speaker_command_pb2.ResumeSpeakerCommand.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
|
|
||||||
response = self.stub.ResumePlayback(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_audio(self) -> CMVRErrorCode:
|
|
||||||
"""停止播放"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
request = generated.speaker_command_pb2.StopSpeakerCommand.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
|
|
||||||
response = self.stub.StopPlayback(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 set_volume(self, volume: float) -> CMVRErrorCode:
|
|
||||||
"""设置音量"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
request = generated.speaker_command_pb2.SetSpeakerVolumeCommand.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
request.volume = volume
|
|
||||||
|
|
||||||
response = self.stub.SetVolume(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_volume(self) -> Tuple[CMVRErrorCode, float]:
|
|
||||||
"""获取音量"""
|
|
||||||
try:
|
|
||||||
generated = self._import_generated()
|
|
||||||
request = generated.speaker_command_pb2.GetSpeakerVolumeCommand.Request()
|
|
||||||
request.header.CopyFrom(self._create_command_header())
|
|
||||||
|
|
||||||
response = self.stub.GetVolume(request)
|
|
||||||
|
|
||||||
if not response.header.success:
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED, 0.0
|
|
||||||
|
|
||||||
return CMVRErrorCode.CMVR_SUCCESS, response.volume
|
|
||||||
|
|
||||||
except grpc.RpcError as e:
|
|
||||||
print(f"获取音量失败: {e}")
|
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED, 0.0
|
|
||||||
879
example_usage.py
879
example_usage.py
@ -1,879 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
|
|
||||||
import sys
|
|
||||||
import os
|
|
||||||
import time
|
|
||||||
import random
|
|
||||||
import grpc # 新增grpc导入
|
|
||||||
import cv2
|
|
||||||
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,JointCmd, Pose3D,RobotCartesian, RobotJointIndexDirection
|
|
||||||
|
|
||||||
def test_biohead():
|
|
||||||
"""测试仿生头功能"""
|
|
||||||
print("=== 测试仿生头功能 ===")
|
|
||||||
|
|
||||||
# 初始化客户端
|
|
||||||
client = CMVRGrpcClient("192.168.1.222: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.jaw_x = 0.5
|
|
||||||
expression.jaw_y = 0.5
|
|
||||||
# # 设置眉毛，使用随机数并打印
|
|
||||||
# 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}")
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
# 闭眼，使用随机数并打印
|
|
||||||
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}")
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
# 结束流式会话
|
|
||||||
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.222:50060")
|
|
||||||
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_rgb_image_stream():
|
|
||||||
"""测试双向流视频帧数据接口"""
|
|
||||||
print("=== 测试双向流视频帧数据接口 ===")
|
|
||||||
|
|
||||||
# 初始化GRPC客户端
|
|
||||||
client = CMVRGrpcClient("192.168.1.222:50060")
|
|
||||||
if not client.is_connected():
|
|
||||||
print("连接服务器失败")
|
|
||||||
return
|
|
||||||
|
|
||||||
# 获取camera客户端
|
|
||||||
camera = client.get_camera("cam4")
|
|
||||||
if not camera:
|
|
||||||
print("获取camera客户端失败")
|
|
||||||
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)
|
|
||||||
try:
|
|
||||||
# 1. 启动传感器数据流
|
|
||||||
print("\n--- 启动rgb数据流 ---")
|
|
||||||
# 创建请求生成器（双向流不需要参数，发送空请求）
|
|
||||||
def request_generator() -> Generator:
|
|
||||||
# 发送初始请求启动流
|
|
||||||
yield camera.create_rgb_stream_request()
|
|
||||||
# 保持流连接，每5秒发送一次心跳（可选）
|
|
||||||
try:
|
|
||||||
while True:
|
|
||||||
time.sleep(5)
|
|
||||||
# 发送空请求保持连接
|
|
||||||
yield camera.create_rgb_stream_request()
|
|
||||||
except GeneratorExit:
|
|
||||||
print("请求生成器已关闭")
|
|
||||||
return
|
|
||||||
|
|
||||||
# 2. 接收并处理流数据
|
|
||||||
print("开始接收rgb流数据（5秒后自动停止）...")
|
|
||||||
start_time = time.time()
|
|
||||||
stream_duration = 15 # 接收15秒数据
|
|
||||||
|
|
||||||
keyframe_count = 0 # 关键帧计数器
|
|
||||||
total_frame_count = 0 # 总帧数计数器
|
|
||||||
# 存储流迭代器的变量
|
|
||||||
stream_iterator = camera.get_rgb_stream(request_generator())
|
|
||||||
# 调用双向流接口
|
|
||||||
for feedback in stream_iterator:
|
|
||||||
# 检查是否超时
|
|
||||||
if time.time() - start_time > stream_duration:
|
|
||||||
print("\n达到预设接收时间，停止接收")
|
|
||||||
break
|
|
||||||
total_frame_count += 1 # 累加总帧数
|
|
||||||
# 检查是否为关键帧并计数
|
|
||||||
if feedback.color_frame.is_key_frame:
|
|
||||||
keyframe_count += 1
|
|
||||||
print(f"[关键帧 #{keyframe_count}] 收到关键帧 (总帧数: {total_frame_count})")
|
|
||||||
stream_iterator.cancel()
|
|
||||||
print(f"\n流接收结束 - 总帧数: {total_frame_count}, 关键帧数: {keyframe_count}, 关键帧占比: {keyframe_count/total_frame_count:.2%}")
|
|
||||||
|
|
||||||
|
|
||||||
except grpc.RpcError as e:
|
|
||||||
print(f"双向流通信错误: {e}")
|
|
||||||
except Exception as e:
|
|
||||||
print(f"处理流数据时发生错误: {e}")
|
|
||||||
finally:
|
|
||||||
# 关闭连接
|
|
||||||
client.close()
|
|
||||||
print("\n=== 双向流传感器数据测试完成 ===")
|
|
||||||
|
|
||||||
def test_dexhand():
|
|
||||||
"""测试DexHand功能（获取状态、设置角度和传感器数据）"""
|
|
||||||
print("=== 测试DexHand功能 ===")
|
|
||||||
|
|
||||||
# 初始化GRPC客户端（使用实际服务器地址）
|
|
||||||
client = CMVRGrpcClient("192.168.0.222: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
|
|
||||||
|
|
||||||
# 定义：字符串ID与手指名称的映射（与proto/服务端一致）
|
|
||||||
dof_id_to_name = {
|
|
||||||
"little_finger": "小拇指",
|
|
||||||
"ring_finger": "无名指",
|
|
||||||
"middle_finger": "中指",
|
|
||||||
"index_finger": "食指",
|
|
||||||
"thumb_bend": "大拇指弯曲",
|
|
||||||
"thumb_rotate": "大拇指旋转"
|
|
||||||
}
|
|
||||||
# 有效字符串ID列表（用于后续判断）
|
|
||||||
valid_dof_ids = set(dof_id_to_name.keys())
|
|
||||||
|
|
||||||
# 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:
|
|
||||||
# 匹配字符串ID对应的手指名称，未知ID显示原始值
|
|
||||||
finger_name = dof_id_to_name.get(hand.dof_id, f"未知自由度({hand.dof_id})")
|
|
||||||
print(f" {finger_name} (ID:{hand.dof_id}): 角度={hand.angle}, 速度={hand.speed}, "
|
|
||||||
f"受力={hand.force}, 位置={hand.position}, 温度={hand.temperature}°C")
|
|
||||||
if hand.error != 0:
|
|
||||||
print(f" 警告: {finger_name} 存在故障: {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-1百分比（与set_angle函数参数要求一致）
|
|
||||||
test_angles = {
|
|
||||||
"little_finger": 0.9, # 小拇指
|
|
||||||
"ring_finger": 0.9, # 无名指
|
|
||||||
"middle_finger": 0.9, # 中指
|
|
||||||
"index_finger": 0.9, # 食指
|
|
||||||
"thumb_bend": 0.9, # 大拇指弯曲
|
|
||||||
"thumb_rotate": 0.9 # 大拇指旋转
|
|
||||||
}
|
|
||||||
|
|
||||||
# 打印设置信息（通过映射表显示中文名称）
|
|
||||||
print("准备设置角度:")
|
|
||||||
for dof_id, value in test_angles.items():
|
|
||||||
finger_name = dof_id_to_name[dof_id] # 已确保ID有效，直接获取名称
|
|
||||||
print(f" {finger_name} (ID:{dof_id}): {value*100}%")
|
|
||||||
|
|
||||||
# 执行角度设置（直接传入字符串ID的字典）
|
|
||||||
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 hasattr(sensors_after, 'thumb_tip') and 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:
|
|
||||||
# 只打印我们设置过的自由度（字符串ID对比）
|
|
||||||
if hand.dof_id in test_angles:
|
|
||||||
finger_name = dof_id_to_name[hand.dof_id]
|
|
||||||
print(f" {finger_name} (ID:{hand.dof_id}): 新角度={hand.angle}, 当前位置={hand.position}")
|
|
||||||
else:
|
|
||||||
print(f"获取设置后状态失败，错误码: {error_code}")
|
|
||||||
|
|
||||||
# 4. 恢复默认角度（复位操作）
|
|
||||||
print("\n--- 恢复默认角度 ---")
|
|
||||||
# 复位字典：键为字符串ID，值为默认百分比
|
|
||||||
default_angles = {
|
|
||||||
"little_finger": 0.1,
|
|
||||||
"ring_finger": 0.1,
|
|
||||||
"middle_finger": 0.1,
|
|
||||||
"index_finger": 0.1,
|
|
||||||
"thumb_bend": 0.1,
|
|
||||||
"thumb_rotate": 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("关键自由度最终位置:")
|
|
||||||
# 遍历需要确认的自由度（字符串ID）
|
|
||||||
for dof_id in test_angles.keys():
|
|
||||||
for hand in state.hands:
|
|
||||||
if hand.dof_id == dof_id:
|
|
||||||
finger_name = dof_id_to_name[dof_id]
|
|
||||||
print(f" {finger_name} (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=== 双向流传感器数据测试完成 ===")
|
|
||||||
|
|
||||||
def test_speaker():
|
|
||||||
"""测试扬声器功能"""
|
|
||||||
print("\n=== 测试扬声器功能 ===")
|
|
||||||
|
|
||||||
# 初始化GRPC客户端
|
|
||||||
client = CMVRGrpcClient("192.168.1.222:50060")
|
|
||||||
if not client.is_connected():
|
|
||||||
print("连接服务器失败")
|
|
||||||
return
|
|
||||||
|
|
||||||
speaker = client.get_speaker("spk1")
|
|
||||||
if speaker is None:
|
|
||||||
print("获取扬声器失败")
|
|
||||||
return
|
|
||||||
|
|
||||||
# 获取状态
|
|
||||||
result, status = speaker.get_status()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"扬声器状态: 初始化={status.is_initialized}, 运行中={status.is_running}")
|
|
||||||
else:
|
|
||||||
print(f"获取扬声器状态失败，错误码: {result}")
|
|
||||||
|
|
||||||
# 设置音量
|
|
||||||
result = speaker.set_volume(80) # 80%音量
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("设置音量成功")
|
|
||||||
else:
|
|
||||||
print(f"设置音量失败，错误码: {result}")
|
|
||||||
|
|
||||||
# 获取音量
|
|
||||||
result, volume = speaker.get_volume()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"当前音量: {volume}%")
|
|
||||||
else:
|
|
||||||
print(f"获取音量失败，错误码: {result}")
|
|
||||||
|
|
||||||
# 播放音频 (需要替换为实际存在的音频文件路径)
|
|
||||||
audio_path = "/home/share/assets/upload/员工女_车内温度有点低，是否需要调高空调？.wav"
|
|
||||||
print(f"尝试播放音频: {audio_path}")
|
|
||||||
result = speaker.play_audio(audio_path)
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("开始播放音频")
|
|
||||||
|
|
||||||
# 等待3秒
|
|
||||||
time.sleep(3)
|
|
||||||
|
|
||||||
# 暂停播放
|
|
||||||
result = speaker.pause_audio()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("暂停播放")
|
|
||||||
|
|
||||||
# 等待1秒
|
|
||||||
time.sleep(1)
|
|
||||||
|
|
||||||
# 继续播放
|
|
||||||
result = speaker.resume_audio()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("继续播放")
|
|
||||||
|
|
||||||
# 等待2秒
|
|
||||||
time.sleep(2)
|
|
||||||
|
|
||||||
# 停止播放
|
|
||||||
result = speaker.stop_audio()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("停止播放")
|
|
||||||
else:
|
|
||||||
print(f"停止播放失败，错误码: {result}")
|
|
||||||
else:
|
|
||||||
print(f"继续播放失败，错误码: {result}")
|
|
||||||
else:
|
|
||||||
print(f"暂停播放失败，错误码: {result}")
|
|
||||||
else:
|
|
||||||
print(f"播放音频失败，错误码: {result}")
|
|
||||||
|
|
||||||
def test_microphone():
|
|
||||||
"""测试麦克风功能"""
|
|
||||||
print("\n=== 测试麦克风功能 ===")
|
|
||||||
# 初始化GRPC客户端
|
|
||||||
client = CMVRGrpcClient("192.168.1.222:50060")
|
|
||||||
if not client.is_connected():
|
|
||||||
print("连接服务器失败")
|
|
||||||
return
|
|
||||||
|
|
||||||
microphone = client.get_micphone("mic2")
|
|
||||||
if microphone is None:
|
|
||||||
print("获取麦克风失败")
|
|
||||||
return
|
|
||||||
|
|
||||||
# 获取状态
|
|
||||||
result, status = microphone.get_status()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"麦克风状态: 初始化={status.is_initialized}, 录音中={status.is_recording}")
|
|
||||||
else:
|
|
||||||
print(f"获取麦克风状态失败，错误码: {result}")
|
|
||||||
|
|
||||||
# 开始录音
|
|
||||||
audio_path = "/home/linbo/Data/Audio/recorded_audio.mp3"
|
|
||||||
result = microphone.start_record(audio_path)
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"开始录音，保存到: {audio_path}")
|
|
||||||
|
|
||||||
# 录音5秒
|
|
||||||
time.sleep(5)
|
|
||||||
|
|
||||||
# 停止录音
|
|
||||||
result = microphone.stop_record()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("停止录音")
|
|
||||||
else:
|
|
||||||
print(f"停止录音失败，错误码: {result}")
|
|
||||||
else:
|
|
||||||
print(f"开始录音失败，错误码: {result}")
|
|
||||||
|
|
||||||
def test_humanoid_robot():
|
|
||||||
"""测试机器人功能"""
|
|
||||||
print("\n=== 测试机器人功能 ===")
|
|
||||||
# 初始化GRPC客户端
|
|
||||||
client = CMVRGrpcClient("192.168.0.222:50052")
|
|
||||||
if not client.is_connected():
|
|
||||||
print("连接服务器失败")
|
|
||||||
return
|
|
||||||
|
|
||||||
humanoidRobot = client.get_humanoid_robot("hc01")
|
|
||||||
if humanoidRobot is None:
|
|
||||||
print("获取机器人失败")
|
|
||||||
return
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
# 使能
|
|
||||||
# print("\n1. 开启机器人使能...")
|
|
||||||
# result = humanoidRobot.torqueOn()
|
|
||||||
# if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
# print("✅ 使能开启成功")
|
|
||||||
# else:
|
|
||||||
# print(f"❌ 使能开启失败，错误码: {result}")
|
|
||||||
# return
|
|
||||||
|
|
||||||
# 等待使能生效
|
|
||||||
import time
|
|
||||||
time.sleep(1)
|
|
||||||
|
|
||||||
try:
|
|
||||||
# 获取初始状态
|
|
||||||
print("\n2. 获取初始关节状态...")
|
|
||||||
# 获取状态
|
|
||||||
result, joint_states = humanoidRobot.get_joint_states()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"成功获取 {len(joint_states)} 个关节状态组")
|
|
||||||
|
|
||||||
for i, status in enumerate(joint_states):
|
|
||||||
print(f"\n=== 关节状态组 {i+1} ===")
|
|
||||||
print(f"时间戳: {status.timestamp:.3f}s")
|
|
||||||
print(f"关节数量: {len(status.name)}")
|
|
||||||
|
|
||||||
if len(status.name) > 0:
|
|
||||||
print("\n关节详情:")
|
|
||||||
print(f"{'关节名称':<20} {'位置(rad)':<12} {'速度(rad/s)':<12} {'力矩':<10}")
|
|
||||||
print("-" * 55)
|
|
||||||
|
|
||||||
for j, name in enumerate(status.name):
|
|
||||||
pos = status.position[j] if j < len(status.position) else 0.0
|
|
||||||
vel = status.velocity[j] if j < len(status.velocity) else 0.0
|
|
||||||
eff = status.effort[j] if j < len(status.effort) else 0.0
|
|
||||||
|
|
||||||
print(f"{name:<20} {pos:<12.4f} {vel:<12.4f} {eff:<10.4f}")
|
|
||||||
else:
|
|
||||||
print("该状态组没有关节数据")
|
|
||||||
|
|
||||||
# 打印所有状态的汇总信息
|
|
||||||
print(f"\n=== 汇总信息 ===")
|
|
||||||
total_joints = sum(len(state.name) for state in joint_states)
|
|
||||||
timestamps = [state.timestamp for state in joint_states]
|
|
||||||
print(f"总关节数: {total_joints}")
|
|
||||||
print(f"时间戳范围: {min(timestamps):.3f}s - {max(timestamps):.3f}s")
|
|
||||||
|
|
||||||
else:
|
|
||||||
print(f"获取关节状态失败，错误码: {result}")
|
|
||||||
|
|
||||||
return
|
|
||||||
# 测试moveJ - 关节空间运动
|
|
||||||
print("\n3. 测试关节空间运动 (moveJ)...")
|
|
||||||
|
|
||||||
# 创建关节命令列表,回到零位
|
|
||||||
joint_commands = [
|
|
||||||
JointCmd(joint_name="L_SHOULDER_P", rad=0, vel=0.8),
|
|
||||||
JointCmd(joint_name="L_SHOULDER_R", rad=0, vel=0.8),
|
|
||||||
JointCmd(joint_name="L_SHOULDER_Y", rad=0, vel=0.8),
|
|
||||||
JointCmd(joint_name="L_ELBOW_R", rad=0, vel=0.8),
|
|
||||||
JointCmd(joint_name="L_WRIST_P", rad=0, vel=0.8),
|
|
||||||
JointCmd(joint_name="L_WRIST_Y", rad=0, vel=0.8),
|
|
||||||
JointCmd(joint_name="L_WRIST_R", rad=0, vel=0.8),
|
|
||||||
|
|
||||||
JointCmd(joint_name="R_SHOULDER_P", rad=0, vel=0.8),
|
|
||||||
JointCmd(joint_name="R_SHOULDER_R", rad=0, vel=0.8),
|
|
||||||
JointCmd(joint_name="R_SHOULDER_Y", rad=0, vel=0.8),
|
|
||||||
JointCmd(joint_name="R_ELBOW_R", rad=0, vel=0.8),
|
|
||||||
JointCmd(joint_name="R_WRIST_P", rad=0, vel=0.8),
|
|
||||||
JointCmd(joint_name="R_WRIST_Y", rad=0, vel=0.8),
|
|
||||||
JointCmd(joint_name="R_WRIST_R", rad=0, vel=0.8),
|
|
||||||
|
|
||||||
JointCmd(joint_name="WAIST_P", rad=0, vel=0.8),
|
|
||||||
JointCmd(joint_name="WAIST_Y", rad=0, vel=0.8)
|
|
||||||
]
|
|
||||||
|
|
||||||
result, _ = humanoidRobot.moveJ(joint_commands, overall_vel=0.8, overall_acc=0.4)
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("✅ moveJ 命令发送成功")
|
|
||||||
time.sleep(3) # 等待运动完成
|
|
||||||
else:
|
|
||||||
print(f"❌ moveJ 命令发送失败，错误码: {result}")
|
|
||||||
|
|
||||||
# 获取运动后的状态
|
|
||||||
print("\n 获取moveJ后的关节状态...")
|
|
||||||
result, status = humanoidRobot.get_joint_state()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f" 运动后时间戳: {status.timestamp:.3f}s")
|
|
||||||
print(f" 关节数量: {len(status.name)}")
|
|
||||||
|
|
||||||
# 测试moveL - 直线空间运动
|
|
||||||
print("\n4. 测试直线空间运动 (moveL)...")
|
|
||||||
# 创建目标位姿 (相对于当前位姿的小幅移动)
|
|
||||||
target_pose = Pose3D(x=0.05, y=0.0, z=0.0, rx=0.0, ry=0.0, rz=0.0)
|
|
||||||
|
|
||||||
result, _ = humanoidRobot.moveL(ee_link="R_WRIST_R", target_pose=target_pose,
|
|
||||||
vel=0.05, acc=0.2)
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("✅ moveL 命令发送成功")
|
|
||||||
time.sleep(3) # 等待运动完成
|
|
||||||
else:
|
|
||||||
print(f"❌ moveL 命令发送失败，错误码: {result}")
|
|
||||||
|
|
||||||
# 测试speedJ - 关节速度控制
|
|
||||||
print("\n5. 测试关节速度控制 (speedJ)...")
|
|
||||||
result, error_msg = humanoidRobot.speedJ(joint_name="R_SHOULDER_P",
|
|
||||||
velocity=0.1,
|
|
||||||
direction=RobotJointIndexDirection.FORWARD,
|
|
||||||
acc=0.3)
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("✅ speedJ 命令发送成功")
|
|
||||||
time.sleep(2) # 运行2秒
|
|
||||||
# 停止运动
|
|
||||||
stop_result, _ = humanoidRobot.speedJ(joint_name="R_SHOULDER_P",
|
|
||||||
velocity=0.0,
|
|
||||||
direction=RobotJointIndexDirection.FORWARD,
|
|
||||||
acc=0.3)
|
|
||||||
print(" 停止speedJ运动")
|
|
||||||
else:
|
|
||||||
print(f"❌ speedJ 命令发送失败，错误码: {result}")
|
|
||||||
if error_msg:
|
|
||||||
print(f" 错误信息: {error_msg}")
|
|
||||||
|
|
||||||
# 测试speedL - 笛卡尔速度控制
|
|
||||||
print("\n6. 测试笛卡尔速度控制 (speedL)...")
|
|
||||||
result, error_msg = humanoidRobot.speedL(ee_link="R_WRIST_R",
|
|
||||||
velocity=0.02,
|
|
||||||
cartesian=RobotCartesian.X,
|
|
||||||
direction=RobotJointIndexDirection.FORWARD,
|
|
||||||
acc=0.1)
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("✅ speedL 命令发送成功")
|
|
||||||
time.sleep(2) # 运行2秒
|
|
||||||
# 停止运动
|
|
||||||
stop_result, _ = humanoidRobot.speedL(ee_link="R_WRIST_R",
|
|
||||||
velocity=0.0,
|
|
||||||
cartesian=RobotCartesian.X,
|
|
||||||
direction=RobotJointIndexDirection.FORWARD,
|
|
||||||
acc=0.1)
|
|
||||||
print(" 停止speedL运动")
|
|
||||||
else:
|
|
||||||
print(f"❌ speedL 命令发送失败，错误码: {result}")
|
|
||||||
if error_msg:
|
|
||||||
print(f" 错误信息: {error_msg}")
|
|
||||||
|
|
||||||
# 获取最终状态
|
|
||||||
print("\n7. 获取最终关节状态...")
|
|
||||||
result, status = humanoidRobot.get_joint_state()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("\n=== 最终关节状态信息 ===")
|
|
||||||
print(f"时间戳: {status.timestamp:.3f}s")
|
|
||||||
print(f"关节数量: {len(status.name)}")
|
|
||||||
|
|
||||||
if len(status.name) > 0:
|
|
||||||
print("\n关节详情:")
|
|
||||||
print(f"{'关节名称':<15} {'位置(rad)':<12} {'速度(rad/s)':<12} {'力矩':<10}")
|
|
||||||
print("-" * 50)
|
|
||||||
|
|
||||||
for i, name in enumerate(status.name):
|
|
||||||
pos = status.position[i] if i < len(status.position) else 0.0
|
|
||||||
vel = status.velocity[i] if i < len(status.velocity) else 0.0
|
|
||||||
eff = status.effort[i] if i < len(status.effort) else 0.0
|
|
||||||
|
|
||||||
print(f"{name:<15} {pos:<12.4f} {vel:<12.4f} {eff:<10.4f}")
|
|
||||||
else:
|
|
||||||
print("没有关节数据")
|
|
||||||
else:
|
|
||||||
print(f"获取最终状态失败，错误码: {result}")
|
|
||||||
|
|
||||||
except Exception as e:
|
|
||||||
print(f"\n❌ 测试过程中发生异常: {e}")
|
|
||||||
import traceback
|
|
||||||
traceback.print_exc()
|
|
||||||
|
|
||||||
finally:
|
|
||||||
# 去使能
|
|
||||||
print("\n8. 关闭机器人使能...")
|
|
||||||
result = humanoidRobot.torqueOff()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("✅ 使能关闭成功")
|
|
||||||
else:
|
|
||||||
print(f"❌ 使能关闭失败，错误码: {result}")
|
|
||||||
|
|
||||||
print("\n=== 机器人功能测试完成 ===")
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
# test_biohead()
|
|
||||||
# test_camera()
|
|
||||||
# test_rgb_image_stream()
|
|
||||||
test_dexhand()
|
|
||||||
# test_sensor_data_stream()
|
|
||||||
# test_speaker()
|
|
||||||
# test_microphone()
|
|
||||||
# test_humanoid_robot()
|
|
||||||
@ -1,840 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
|
|
||||||
import sys
|
|
||||||
import os
|
|
||||||
import time
|
|
||||||
import random
|
|
||||||
import grpc # 新增grpc导入
|
|
||||||
import cv2
|
|
||||||
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,JointCmd, Pose3D,RobotCartesian, RobotJointIndexDirection
|
|
||||||
|
|
||||||
def test_biohead():
|
|
||||||
"""测试仿生头功能"""
|
|
||||||
print("=== 测试仿生头功能 ===")
|
|
||||||
|
|
||||||
# 初始化客户端
|
|
||||||
client = CMVRGrpcClient("192.168.1.222: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.jaw_x = 0.5
|
|
||||||
expression.jaw_y = 0.5
|
|
||||||
# # 设置眉毛，使用随机数并打印
|
|
||||||
# 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}")
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
# 闭眼，使用随机数并打印
|
|
||||||
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}")
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
# 结束流式会话
|
|
||||||
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.222:50060")
|
|
||||||
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_rgb_image_stream():
|
|
||||||
"""测试双向流视频帧数据接口"""
|
|
||||||
print("=== 测试双向流视频帧数据接口 ===")
|
|
||||||
|
|
||||||
# 初始化GRPC客户端
|
|
||||||
client = CMVRGrpcClient("192.168.1.222:50060")
|
|
||||||
if not client.is_connected():
|
|
||||||
print("连接服务器失败")
|
|
||||||
return
|
|
||||||
|
|
||||||
# 获取camera客户端
|
|
||||||
camera = client.get_camera("cam4")
|
|
||||||
if not camera:
|
|
||||||
print("获取camera客户端失败")
|
|
||||||
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)
|
|
||||||
try:
|
|
||||||
# 1. 启动传感器数据流
|
|
||||||
print("\n--- 启动rgb数据流 ---")
|
|
||||||
# 创建请求生成器（双向流不需要参数，发送空请求）
|
|
||||||
def request_generator() -> Generator:
|
|
||||||
# 发送初始请求启动流
|
|
||||||
yield camera.create_rgb_stream_request()
|
|
||||||
# 保持流连接，每5秒发送一次心跳（可选）
|
|
||||||
try:
|
|
||||||
while True:
|
|
||||||
time.sleep(5)
|
|
||||||
# 发送空请求保持连接
|
|
||||||
yield camera.create_rgb_stream_request()
|
|
||||||
except GeneratorExit:
|
|
||||||
print("请求生成器已关闭")
|
|
||||||
return
|
|
||||||
|
|
||||||
# 2. 接收并处理流数据
|
|
||||||
print("开始接收rgb流数据（5秒后自动停止）...")
|
|
||||||
start_time = time.time()
|
|
||||||
stream_duration = 15 # 接收15秒数据
|
|
||||||
|
|
||||||
keyframe_count = 0 # 关键帧计数器
|
|
||||||
total_frame_count = 0 # 总帧数计数器
|
|
||||||
# 存储流迭代器的变量
|
|
||||||
stream_iterator = camera.get_rgb_stream(request_generator())
|
|
||||||
# 调用双向流接口
|
|
||||||
for feedback in stream_iterator:
|
|
||||||
# 检查是否超时
|
|
||||||
if time.time() - start_time > stream_duration:
|
|
||||||
print("\n达到预设接收时间，停止接收")
|
|
||||||
break
|
|
||||||
total_frame_count += 1 # 累加总帧数
|
|
||||||
# 检查是否为关键帧并计数
|
|
||||||
if feedback.color_frame.is_key_frame:
|
|
||||||
keyframe_count += 1
|
|
||||||
print(f"[关键帧 #{keyframe_count}] 收到关键帧 (总帧数: {total_frame_count})")
|
|
||||||
stream_iterator.cancel()
|
|
||||||
print(f"\n流接收结束 - 总帧数: {total_frame_count}, 关键帧数: {keyframe_count}, 关键帧占比: {keyframe_count/total_frame_count:.2%}")
|
|
||||||
|
|
||||||
|
|
||||||
except grpc.RpcError as e:
|
|
||||||
print(f"双向流通信错误: {e}")
|
|
||||||
except Exception as e:
|
|
||||||
print(f"处理流数据时发生错误: {e}")
|
|
||||||
finally:
|
|
||||||
# 关闭连接
|
|
||||||
client.close()
|
|
||||||
print("\n=== 双向流传感器数据测试完成 ===")
|
|
||||||
|
|
||||||
def test_dexhand():
|
|
||||||
"""测试DexHand功能（获取状态、设置角度和传感器数据）"""
|
|
||||||
print("=== 测试DexHand功能 ===")
|
|
||||||
|
|
||||||
# 初始化GRPC客户端（使用实际服务器地址）
|
|
||||||
client = CMVRGrpcClient("192.168.1.222:50060")
|
|
||||||
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=== 双向流传感器数据测试完成 ===")
|
|
||||||
|
|
||||||
def test_speaker():
|
|
||||||
"""测试扬声器功能"""
|
|
||||||
print("\n=== 测试扬声器功能 ===")
|
|
||||||
|
|
||||||
# 初始化GRPC客户端
|
|
||||||
client = CMVRGrpcClient("192.168.1.222:50060")
|
|
||||||
if not client.is_connected():
|
|
||||||
print("连接服务器失败")
|
|
||||||
return
|
|
||||||
|
|
||||||
speaker = client.get_speaker("spk1")
|
|
||||||
if speaker is None:
|
|
||||||
print("获取扬声器失败")
|
|
||||||
return
|
|
||||||
|
|
||||||
# 获取状态
|
|
||||||
result, status = speaker.get_status()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"扬声器状态: 初始化={status.is_initialized}, 运行中={status.is_running}")
|
|
||||||
else:
|
|
||||||
print(f"获取扬声器状态失败，错误码: {result}")
|
|
||||||
|
|
||||||
# 设置音量
|
|
||||||
result = speaker.set_volume(80) # 80%音量
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("设置音量成功")
|
|
||||||
else:
|
|
||||||
print(f"设置音量失败，错误码: {result}")
|
|
||||||
|
|
||||||
# 获取音量
|
|
||||||
result, volume = speaker.get_volume()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"当前音量: {volume}%")
|
|
||||||
else:
|
|
||||||
print(f"获取音量失败，错误码: {result}")
|
|
||||||
|
|
||||||
# 播放音频 (需要替换为实际存在的音频文件路径)
|
|
||||||
audio_path = "/home/share/assets/upload/员工女_车内温度有点低，是否需要调高空调？.wav"
|
|
||||||
print(f"尝试播放音频: {audio_path}")
|
|
||||||
result = speaker.play_audio(audio_path)
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("开始播放音频")
|
|
||||||
|
|
||||||
# 等待3秒
|
|
||||||
time.sleep(3)
|
|
||||||
|
|
||||||
# 暂停播放
|
|
||||||
result = speaker.pause_audio()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("暂停播放")
|
|
||||||
|
|
||||||
# 等待1秒
|
|
||||||
time.sleep(1)
|
|
||||||
|
|
||||||
# 继续播放
|
|
||||||
result = speaker.resume_audio()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("继续播放")
|
|
||||||
|
|
||||||
# 等待2秒
|
|
||||||
time.sleep(2)
|
|
||||||
|
|
||||||
# 停止播放
|
|
||||||
result = speaker.stop_audio()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("停止播放")
|
|
||||||
else:
|
|
||||||
print(f"停止播放失败，错误码: {result}")
|
|
||||||
else:
|
|
||||||
print(f"继续播放失败，错误码: {result}")
|
|
||||||
else:
|
|
||||||
print(f"暂停播放失败，错误码: {result}")
|
|
||||||
else:
|
|
||||||
print(f"播放音频失败，错误码: {result}")
|
|
||||||
|
|
||||||
def test_microphone():
|
|
||||||
"""测试麦克风功能"""
|
|
||||||
print("\n=== 测试麦克风功能 ===")
|
|
||||||
# 初始化GRPC客户端
|
|
||||||
client = CMVRGrpcClient("192.168.1.222:50060")
|
|
||||||
if not client.is_connected():
|
|
||||||
print("连接服务器失败")
|
|
||||||
return
|
|
||||||
|
|
||||||
microphone = client.get_micphone("mic2")
|
|
||||||
if microphone is None:
|
|
||||||
print("获取麦克风失败")
|
|
||||||
return
|
|
||||||
|
|
||||||
# 获取状态
|
|
||||||
result, status = microphone.get_status()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"麦克风状态: 初始化={status.is_initialized}, 录音中={status.is_recording}")
|
|
||||||
else:
|
|
||||||
print(f"获取麦克风状态失败，错误码: {result}")
|
|
||||||
|
|
||||||
# 开始录音
|
|
||||||
audio_path = "/home/linbo/Data/Audio/recorded_audio.mp3"
|
|
||||||
result = microphone.start_record(audio_path)
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f"开始录音，保存到: {audio_path}")
|
|
||||||
|
|
||||||
# 录音5秒
|
|
||||||
time.sleep(5)
|
|
||||||
|
|
||||||
# 停止录音
|
|
||||||
result = microphone.stop_record()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("停止录音")
|
|
||||||
else:
|
|
||||||
print(f"停止录音失败，错误码: {result}")
|
|
||||||
else:
|
|
||||||
print(f"开始录音失败，错误码: {result}")
|
|
||||||
|
|
||||||
def test_humanoid_robot():
|
|
||||||
"""测试机器人功能"""
|
|
||||||
print("\n=== 测试机器人功能 ===")
|
|
||||||
# 初始化GRPC客户端
|
|
||||||
client = CMVRGrpcClient("192.168.0.222:50052")
|
|
||||||
if not client.is_connected():
|
|
||||||
print("连接服务器失败")
|
|
||||||
return
|
|
||||||
|
|
||||||
humanoidRobot = client.get_humanoid_robot("hc01")
|
|
||||||
if humanoidRobot is None:
|
|
||||||
print("获取机器人失败")
|
|
||||||
return
|
|
||||||
|
|
||||||
# 获取初始状态
|
|
||||||
print("\n1. 获取初始关节状态...")
|
|
||||||
result, status = humanoidRobot.get_joint_state()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
# 打印关节状态详细信息
|
|
||||||
print("\n=== 初始关节状态信息 ===")
|
|
||||||
print(f"时间戳: {status.timestamp:.3f}s")
|
|
||||||
print(f"关节数量: {len(status.name)}")
|
|
||||||
|
|
||||||
if len(status.name) > 0:
|
|
||||||
print("\n关节详情:")
|
|
||||||
print(f"{'关节名称':<15} {'位置(rad)':<12} {'速度(rad/s)':<12} {'力矩':<10}")
|
|
||||||
print("-" * 50)
|
|
||||||
|
|
||||||
for i, name in enumerate(status.name):
|
|
||||||
pos = status.position[i] if i < len(status.position) else 0.0
|
|
||||||
vel = status.velocity[i] if i < len(status.velocity) else 0.0
|
|
||||||
eff = status.effort[i] if i < len(status.effort) else 0.0
|
|
||||||
|
|
||||||
print(f"{name:<15} {pos:<12.4f} {vel:<12.4f} {eff:<10.4f}")
|
|
||||||
else:
|
|
||||||
print("没有关节数据")
|
|
||||||
else:
|
|
||||||
print(f"获取关节状态失败，错误码: {result}")
|
|
||||||
|
|
||||||
# 使能
|
|
||||||
print("\n2. 开启机器人使能...")
|
|
||||||
result = humanoidRobot.torqueOn()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("✅ 使能开启成功")
|
|
||||||
else:
|
|
||||||
print(f"❌ 使能开启失败，错误码: {result}")
|
|
||||||
return
|
|
||||||
|
|
||||||
# 等待使能生效
|
|
||||||
import time
|
|
||||||
time.sleep(1)
|
|
||||||
|
|
||||||
try:
|
|
||||||
# 测试moveJ - 关节空间运动
|
|
||||||
print("\n3. 测试关节空间运动 (moveJ)...")
|
|
||||||
# 创建关节命令列表
|
|
||||||
joint_commands = [
|
|
||||||
JointCmd(joint_name="right_shoulder_pitch", rad=0.5, vel=0.3),
|
|
||||||
JointCmd(joint_name="right_shoulder_roll", rad=0.3, vel=0.3),
|
|
||||||
JointCmd(joint_name="right_elbow_pitch", rad=0.2, vel=0.3),
|
|
||||||
]
|
|
||||||
|
|
||||||
result, _ = humanoidRobot.moveJ(joint_commands, overall_vel=0.2, overall_acc=0.4)
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("✅ moveJ 命令发送成功")
|
|
||||||
time.sleep(3) # 等待运动完成
|
|
||||||
else:
|
|
||||||
print(f"❌ moveJ 命令发送失败，错误码: {result}")
|
|
||||||
|
|
||||||
# 获取运动后的状态
|
|
||||||
print("\n 获取moveJ后的关节状态...")
|
|
||||||
result, status = humanoidRobot.get_joint_state()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print(f" 运动后时间戳: {status.timestamp:.3f}s")
|
|
||||||
print(f" 关节数量: {len(status.name)}")
|
|
||||||
|
|
||||||
# 测试moveL - 直线空间运动
|
|
||||||
print("\n4. 测试直线空间运动 (moveL)...")
|
|
||||||
# 创建目标位姿 (相对于当前位姿的小幅移动)
|
|
||||||
target_pose = Pose3D(x=0.1, y=0.0, z=0.0, rx=0.0, ry=0.0, rz=0.0)
|
|
||||||
|
|
||||||
result, _ = humanoidRobot.moveL(ee_link="right_hand", target_pose=target_pose,
|
|
||||||
vel=0.05, acc=0.2)
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("✅ moveL 命令发送成功")
|
|
||||||
time.sleep(3) # 等待运动完成
|
|
||||||
else:
|
|
||||||
print(f"❌ moveL 命令发送失败，错误码: {result}")
|
|
||||||
|
|
||||||
# 测试speedJ - 关节速度控制
|
|
||||||
print("\n5. 测试关节速度控制 (speedJ)...")
|
|
||||||
result, error_msg = humanoidRobot.speedJ(joint_name="right_shoulder_pitch",
|
|
||||||
velocity=0.1,
|
|
||||||
direction=RobotJointIndexDirection.FORWARD,
|
|
||||||
acc=0.3)
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("✅ speedJ 命令发送成功")
|
|
||||||
time.sleep(2) # 运行2秒
|
|
||||||
# 停止运动
|
|
||||||
stop_result, _ = humanoidRobot.speedJ(joint_name="right_shoulder_pitch",
|
|
||||||
velocity=0.0,
|
|
||||||
direction=RobotJointIndexDirection.FORWARD,
|
|
||||||
acc=0.3)
|
|
||||||
print(" 停止speedJ运动")
|
|
||||||
else:
|
|
||||||
print(f"❌ speedJ 命令发送失败，错误码: {result}")
|
|
||||||
if error_msg:
|
|
||||||
print(f" 错误信息: {error_msg}")
|
|
||||||
|
|
||||||
# 测试speedL - 笛卡尔速度控制
|
|
||||||
print("\n6. 测试笛卡尔速度控制 (speedL)...")
|
|
||||||
result, error_msg = humanoidRobot.speedL(ee_link="right_hand",
|
|
||||||
velocity=0.02,
|
|
||||||
cartesian=RobotCartesian.X,
|
|
||||||
direction=RobotJointIndexDirection.FORWARD,
|
|
||||||
acc=0.1)
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("✅ speedL 命令发送成功")
|
|
||||||
time.sleep(2) # 运行2秒
|
|
||||||
# 停止运动
|
|
||||||
stop_result, _ = humanoidRobot.speedL(ee_link="right_hand",
|
|
||||||
velocity=0.0,
|
|
||||||
cartesian=RobotCartesian.X,
|
|
||||||
direction=RobotJointIndexDirection.FORWARD,
|
|
||||||
acc=0.1)
|
|
||||||
print(" 停止speedL运动")
|
|
||||||
else:
|
|
||||||
print(f"❌ speedL 命令发送失败，错误码: {result}")
|
|
||||||
if error_msg:
|
|
||||||
print(f" 错误信息: {error_msg}")
|
|
||||||
|
|
||||||
# 获取最终状态
|
|
||||||
print("\n7. 获取最终关节状态...")
|
|
||||||
result, status = humanoidRobot.get_joint_state()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("\n=== 最终关节状态信息 ===")
|
|
||||||
print(f"时间戳: {status.timestamp:.3f}s")
|
|
||||||
print(f"关节数量: {len(status.name)}")
|
|
||||||
|
|
||||||
if len(status.name) > 0:
|
|
||||||
print("\n关节详情:")
|
|
||||||
print(f"{'关节名称':<15} {'位置(rad)':<12} {'速度(rad/s)':<12} {'力矩':<10}")
|
|
||||||
print("-" * 50)
|
|
||||||
|
|
||||||
for i, name in enumerate(status.name):
|
|
||||||
pos = status.position[i] if i < len(status.position) else 0.0
|
|
||||||
vel = status.velocity[i] if i < len(status.velocity) else 0.0
|
|
||||||
eff = status.effort[i] if i < len(status.effort) else 0.0
|
|
||||||
|
|
||||||
print(f"{name:<15} {pos:<12.4f} {vel:<12.4f} {eff:<10.4f}")
|
|
||||||
else:
|
|
||||||
print("没有关节数据")
|
|
||||||
else:
|
|
||||||
print(f"获取最终状态失败，错误码: {result}")
|
|
||||||
|
|
||||||
except Exception as e:
|
|
||||||
print(f"\n❌ 测试过程中发生异常: {e}")
|
|
||||||
import traceback
|
|
||||||
traceback.print_exc()
|
|
||||||
|
|
||||||
finally:
|
|
||||||
# 去使能
|
|
||||||
print("\n8. 关闭机器人使能...")
|
|
||||||
result = humanoidRobot.torqueOff()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS:
|
|
||||||
print("✅ 使能关闭成功")
|
|
||||||
else:
|
|
||||||
print(f"❌ 使能关闭失败，错误码: {result}")
|
|
||||||
|
|
||||||
print("\n=== 机器人功能测试完成 ===")
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
# test_biohead()
|
|
||||||
# test_camera()
|
|
||||||
# test_rgb_image_stream()
|
|
||||||
# test_dexhand()
|
|
||||||
# test_sensor_data_stream()
|
|
||||||
# test_speaker()
|
|
||||||
# test_microphone()
|
|
||||||
test_humanoid_robot()
|
|
||||||
@ -1,74 +0,0 @@
|
|||||||
import os
|
|
||||||
import subprocess
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
def generate_proto():
|
|
||||||
# 获取当前目录
|
|
||||||
current_dir = Path(__file__).parent
|
|
||||||
proto_dir = current_dir / "protos"
|
|
||||||
generated_dir = current_dir / "generated"
|
|
||||||
|
|
||||||
# 创建生成的目录
|
|
||||||
generated_dir.mkdir(parents=True, exist_ok=True)
|
|
||||||
|
|
||||||
# 编译所有 .proto 文件
|
|
||||||
proto_files = []
|
|
||||||
for root, _, files in os.walk(proto_dir):
|
|
||||||
for file in files:
|
|
||||||
if file.endswith(".proto"):
|
|
||||||
proto_files.append(os.path.join(root, file))
|
|
||||||
|
|
||||||
for proto_path in proto_files:
|
|
||||||
# 计算相对路径
|
|
||||||
rel_path = os.path.relpath(proto_path, proto_dir)
|
|
||||||
output_dir = generated_dir
|
|
||||||
|
|
||||||
cmd = [
|
|
||||||
"python", "-m", "grpc_tools.protoc",
|
|
||||||
f"-I{proto_dir}",
|
|
||||||
f"--python_out={output_dir}",
|
|
||||||
f"--grpc_python_out={output_dir}",
|
|
||||||
proto_path
|
|
||||||
]
|
|
||||||
|
|
||||||
print(f"Generating code for {rel_path}...")
|
|
||||||
result = subprocess.run(cmd, capture_output=True, text=True)
|
|
||||||
|
|
||||||
if result.returncode != 0:
|
|
||||||
print(f"Error generating {rel_path}:")
|
|
||||||
print(result.stderr)
|
|
||||||
return False
|
|
||||||
|
|
||||||
print("Proto files generated successfully!")
|
|
||||||
|
|
||||||
# 修复生成的代码中的导入路径
|
|
||||||
fix_imports(generated_dir)
|
|
||||||
|
|
||||||
return True
|
|
||||||
|
|
||||||
def fix_imports(generated_dir):
|
|
||||||
"""修复生成的代码中的导入路径"""
|
|
||||||
for root, _, files in os.walk(generated_dir):
|
|
||||||
for file in files:
|
|
||||||
if file.endswith(".py"):
|
|
||||||
file_path = os.path.join(root, file)
|
|
||||||
with open(file_path, 'r', encoding='utf-8') as f:
|
|
||||||
content = f.read()
|
|
||||||
|
|
||||||
# 替换导入路径
|
|
||||||
content = content.replace(
|
|
||||||
"from cmvr.api import",
|
|
||||||
"from generated.cmvr.api import"
|
|
||||||
)
|
|
||||||
content = content.replace(
|
|
||||||
"import cmvr.api.",
|
|
||||||
"import generated.cmvr.api."
|
|
||||||
)
|
|
||||||
|
|
||||||
with open(file_path, 'w', encoding='utf-8') as f:
|
|
||||||
f.write(content)
|
|
||||||
|
|
||||||
print(f"Fixed imports in {file_path}")
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
generate_proto()
|
|
||||||
@ -1,59 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
|
|
||||||
import sys
|
|
||||||
import os
|
|
||||||
import time
|
|
||||||
import keyboard
|
|
||||||
import grpc
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
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, JointCmd, Pose3D, RobotCartesian, RobotJointIndexDirection
|
|
||||||
|
|
||||||
def test_humanoid_robot():
|
|
||||||
"""循环获取所有电机状态，按q退出，每个电机信息单独一行并按指定格式打印"""
|
|
||||||
print("电机状态监控 (按q退出)\n")
|
|
||||||
client = CMVRGrpcClient("192.168.0.222:50052")
|
|
||||||
|
|
||||||
if not client.is_connected():
|
|
||||||
print("连接服务器失败")
|
|
||||||
return
|
|
||||||
|
|
||||||
humanoidRobot = client.get_humanoid_robot("hc01")
|
|
||||||
if humanoidRobot is None:
|
|
||||||
print("获取机器人失败")
|
|
||||||
return
|
|
||||||
|
|
||||||
try:
|
|
||||||
while True:
|
|
||||||
if keyboard.is_pressed('q'):
|
|
||||||
print("\n退出程序...")
|
|
||||||
break
|
|
||||||
|
|
||||||
# 获取所有关节状态
|
|
||||||
result, joint_states = humanoidRobot.get_joint_states()
|
|
||||||
if result == CMVRErrorCode.CMVR_SUCCESS and joint_states:
|
|
||||||
# 遍历所有状态组和电机
|
|
||||||
for status in joint_states:
|
|
||||||
for j, name in enumerate(status.name):
|
|
||||||
pos = status.position[j] if j < len(status.position) else 0.0
|
|
||||||
# 每个电机信息单独一行，按指定格式打印并以逗号结尾
|
|
||||||
print(f'{{"{name}", {pos:.6f}}},')
|
|
||||||
|
|
||||||
time.sleep(0.5)
|
|
||||||
# 如需每次刷新屏幕可取消下面一行注释
|
|
||||||
# os.system('cls' if os.name == 'nt' else 'clear')
|
|
||||||
|
|
||||||
except Exception as e:
|
|
||||||
print(f"\n错误: {e}")
|
|
||||||
import traceback
|
|
||||||
traceback.print_exc()
|
|
||||||
|
|
||||||
finally:
|
|
||||||
print("监控结束")
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
test_humanoid_robot()
|
|
||||||
@ -1,177 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
import "cmvr/api/common.proto"; // 导入通用命令头定义，确保与其他服务接口一致
|
|
||||||
|
|
||||||
package cmvr.api;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 面部表情控制参数定义
|
|
||||||
* 包含生物头部机器人面部各部位的运动参数，用于精确控制面部表情
|
|
||||||
*/
|
|
||||||
message FacialExpression {
|
|
||||||
/**
|
|
||||||
* 眉毛控制参数
|
|
||||||
* 左右眉毛的内外侧垂直位置（0.0-1.0标准化值）
|
|
||||||
*/
|
|
||||||
message Eyebrow {
|
|
||||||
float left_outside_y = 1; // 左眉外侧垂直位置
|
|
||||||
float left_inside_y = 2; // 左眉内侧垂直位置
|
|
||||||
float right_outside_y = 3; // 右眉外侧垂直位置
|
|
||||||
float right_inside_y = 4; // 右眉内侧垂直位置
|
|
||||||
}
|
|
||||||
Eyebrow eyebrow = 3; // 眉毛参数封装
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 眼睑控制参数
|
|
||||||
* 左右眼睑的上下位置（0.0-1.0标准化值）
|
|
||||||
*/
|
|
||||||
message Eyelid {
|
|
||||||
float left_upper_y = 1; // 左上眼睑垂直位置
|
|
||||||
float left_lower_y = 2; // 左下眼睑垂直位置
|
|
||||||
float right_upper_y = 3; // 右上眼睑垂直位置
|
|
||||||
float right_lower_y = 4; // 右下眼睑垂直位置
|
|
||||||
}
|
|
||||||
Eyelid eyelid = 4; // 眼睑参数封装
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 眼球控制参数
|
|
||||||
* 左右眼球的二维坐标（-1.0到1.0标准化值，中心为0）
|
|
||||||
*/
|
|
||||||
message Eyeball {
|
|
||||||
float left_x = 1; // 左眼球水平位置
|
|
||||||
float left_y = 2; // 左眼球垂直位置
|
|
||||||
float right_x = 3; // 右眼球水平位置
|
|
||||||
float right_y = 4; // 右眼球垂直位置
|
|
||||||
}
|
|
||||||
Eyeball eyeball = 5; // 眼球参数封装
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 鼻子控制参数
|
|
||||||
* 鼻子左右侧的垂直位置（0.0-1.0标准化值）
|
|
||||||
*/
|
|
||||||
message Nose {
|
|
||||||
float left_y = 1; // 鼻子左侧垂直位置
|
|
||||||
float right_y = 2; // 鼻子右侧垂直位置
|
|
||||||
}
|
|
||||||
Nose nose = 6; // 鼻子参数封装
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 嘴巴控制参数
|
|
||||||
* 包含嘴唇整体位置和左右唇角细节
|
|
||||||
*/
|
|
||||||
message Mouth {
|
|
||||||
float upper_lip_y = 1; // 上唇垂直位置
|
|
||||||
float upper_lip_z = 2; // 上唇前后位置（Z轴）
|
|
||||||
float lower_lip_y = 3; // 下唇垂直位置
|
|
||||||
float lower_lip_z = 4; // 下唇前后位置（Z轴）
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 左唇角控制参数
|
|
||||||
* 包含上唇、唇角、下唇的坐标点
|
|
||||||
*/
|
|
||||||
message LeftLip {
|
|
||||||
float upper_x = 1; // 左上唇水平位置
|
|
||||||
float upper_y = 2; // 左上唇垂直位置
|
|
||||||
float corner_x = 3; // 左唇角水平位置
|
|
||||||
float corner_y = 4; // 左唇角垂直位置
|
|
||||||
float lower_x = 5; // 左下唇水平位置
|
|
||||||
float lower_y = 6; // 左下唇垂直位置
|
|
||||||
}
|
|
||||||
LeftLip left_lip = 5; // 左唇角参数封装
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 右唇角控制参数
|
|
||||||
* 结构与左唇角对称
|
|
||||||
*/
|
|
||||||
message RightLip {
|
|
||||||
float upper_x = 1; // 右上唇水平位置
|
|
||||||
float upper_y = 2; // 右上唇垂直位置
|
|
||||||
float corner_x = 3; // 右唇角水平位置
|
|
||||||
float corner_y = 4; // 右唇角垂直位置
|
|
||||||
float lower_x = 5; // 右下唇水平位置
|
|
||||||
float lower_y = 6; // 右下唇垂直位置
|
|
||||||
}
|
|
||||||
RightLip right_lip = 6; // 右唇角参数封装
|
|
||||||
}
|
|
||||||
Mouth mouth = 7; // 嘴巴参数封装
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 下巴控制参数
|
|
||||||
* 下巴的二维移动坐标（X/Y轴）
|
|
||||||
*/
|
|
||||||
message Jaw {
|
|
||||||
float x = 1; // 下巴水平位置
|
|
||||||
float y = 2; // 下巴垂直位置
|
|
||||||
}
|
|
||||||
Jaw jaw = 8; // 下巴参数封装
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 设置面部表情命令（集成通用命令头）
|
|
||||||
* 用于单次设置生物头部的面部表情
|
|
||||||
*/
|
|
||||||
message SetFacialExpression {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1; // 通用命令头，包含设备ID和时间戳
|
|
||||||
FacialExpression expression = 2; // 具体面部表情参数
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1; // 通用反馈头，包含操作状态和时间戳
|
|
||||||
string execution_id = 2; // 表情执行任务ID
|
|
||||||
float execution_time_ms = 3; // 表情执行耗时（毫秒）
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 流式面部表情命令（集成通用命令头）
|
|
||||||
* 用于连续发送多个面部表情，实现表情动画效果
|
|
||||||
*/
|
|
||||||
message StreamFacialExpression {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1; // 通用命令头
|
|
||||||
FacialExpression expr = 2; // 执行
|
|
||||||
bool eof = 3; // 标记是否为流结束
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1; // 通用反馈头
|
|
||||||
FacialExpression expr_diff = 2; // 执行误差
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 获取生物头部状态命令（集成通用命令头）
|
|
||||||
* 用于查询当前头部各部位的状态和位置
|
|
||||||
*/
|
|
||||||
message GetStatus {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1; // 通用命令头
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1; // 通用反馈头
|
|
||||||
bool is_moving = 2; // 是否有部位在移动
|
|
||||||
string last_request_id = 3; // 上一次请求的ID
|
|
||||||
repeated float current_positions = 4; // 当前各部位位置参数
|
|
||||||
bool camera_recording = 5; // 相机是否在录制
|
|
||||||
string active_recording_id = 6; // 活跃录制任务的ID
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 紧急停止命令（集成通用命令头）
|
|
||||||
* 用于立即停止所有面部动作
|
|
||||||
*/
|
|
||||||
message EmergencyStop {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1; // 通用命令头
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1; // 通用反馈头
|
|
||||||
string stopped_processes = 2; // 已停止的进程列表
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,22 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
package cmvr.api;
|
|
||||||
import "cmvr/api/biohead_command.proto";
|
|
||||||
|
|
||||||
// 生物头部机器人服务接口
|
|
||||||
service BioHeadService {
|
|
||||||
// 设置面部表情
|
|
||||||
rpc SetExpression(SetFacialExpression.Request) returns (SetFacialExpression.Feedback){};
|
|
||||||
|
|
||||||
// 流式表情控制
|
|
||||||
//rpc StreamExpression(StreamFacialExpression.Request) returns (StreamFacialExpression.Feedback){};
|
|
||||||
|
|
||||||
rpc StreamExpression (stream StreamFacialExpression.Request) returns (stream StreamFacialExpression.Feedback);
|
|
||||||
|
|
||||||
|
|
||||||
// 获取状态
|
|
||||||
rpc GetSystemStatus(GetStatus.Request) returns (GetStatus.Feedback){};
|
|
||||||
|
|
||||||
// 紧急停止
|
|
||||||
rpc EmergencyStop(EmergencyStop.Request) returns (EmergencyStop.Feedback){};
|
|
||||||
}
|
|
||||||
@ -1,175 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
import "cmvr/api/common.proto";
|
|
||||||
|
|
||||||
package cmvr.api;
|
|
||||||
|
|
||||||
message FrameData {
|
|
||||||
enum FrameType {
|
|
||||||
U8C1 = 0;
|
|
||||||
U16C1 = 1;
|
|
||||||
U8C3 = 2;
|
|
||||||
U16C3 = 3;
|
|
||||||
F16C1 = 4;
|
|
||||||
F32C1 = 5;
|
|
||||||
}
|
|
||||||
bytes data = 1;
|
|
||||||
int32 width = 2;
|
|
||||||
int32 height = 3;
|
|
||||||
FrameType type = 4;
|
|
||||||
string codec = 5;
|
|
||||||
bool is_key_frame = 6;
|
|
||||||
}
|
|
||||||
|
|
||||||
message CameraIntrinsics {
|
|
||||||
float cx = 1; // 主点水平坐标（从左边缘的像素偏移）
|
|
||||||
float cy = 2; // 主点垂直坐标（从上边缘的像素偏移）
|
|
||||||
float fx = 3; // x方向焦距（像素宽度的倍数）
|
|
||||||
float fy = 4; // y方向焦距（像素高度的倍数）
|
|
||||||
repeated float coeffs = 5 [packed = true]; // 畸变系数（固定5个元素）
|
|
||||||
}
|
|
||||||
|
|
||||||
message CameraState {
|
|
||||||
bool is_initialized = 1;
|
|
||||||
bool is_opened = 2;
|
|
||||||
bool is_streaming = 3;
|
|
||||||
bool is_recording = 4;
|
|
||||||
bool is_error = 5;
|
|
||||||
string error_message = 6;
|
|
||||||
int32 fps = 7;
|
|
||||||
int32 width = 8;
|
|
||||||
int32 height = 9;
|
|
||||||
}
|
|
||||||
|
|
||||||
message GetCameraStateCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
CameraState state = 2;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message StartCameraCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message StopCameraCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message GetRGBImageCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
FrameData color_frame = 2;
|
|
||||||
CameraIntrinsics intrinsics = 3;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message GetDepthImageCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
FrameData depth_frame = 2;
|
|
||||||
CameraIntrinsics intrinsics = 3;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message GetRGBDImagesCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
FrameData color_frame = 2;
|
|
||||||
FrameData depth_frame = 3;
|
|
||||||
CameraIntrinsics intrinsics = 4;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message StartCameraRecordingCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
string video_path = 2;
|
|
||||||
}
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message StopCameraRecordingCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message GetRGBImageStreamCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
bool eof = 2;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
FrameData color_frame = 2;
|
|
||||||
CameraIntrinsics intrinsics = 3;
|
|
||||||
int32 seq_no = 4;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message GetDepthImageStreamCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
bool eof = 2;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
FrameData depth_frame = 2;
|
|
||||||
CameraIntrinsics intrinsics = 3;
|
|
||||||
int32 seq_no = 4;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message GetRGBDImagesStreamCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
bool eof = 2;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
FrameData color_frame = 2;
|
|
||||||
FrameData depth_frame = 3;
|
|
||||||
CameraIntrinsics intrinsics = 4;
|
|
||||||
int32 seq_no = 5;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,21 +0,0 @@
|
|||||||
|
|
||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
import "cmvr/api/camera_command.proto";
|
|
||||||
|
|
||||||
package cmvr.api;
|
|
||||||
|
|
||||||
service CameraService {
|
|
||||||
rpc GetStatus(GetCameraStateCommand.Request) returns (GetCameraStateCommand.Feedback) {}
|
|
||||||
rpc StartCamera(StartCameraCommand.Request) returns (StartCameraCommand.Feedback) {}
|
|
||||||
rpc StopCamera(StopCameraCommand.Request) returns (StopCameraCommand.Feedback) {}
|
|
||||||
rpc GetRGBImage(GetRGBImageCommand.Request) returns (GetRGBImageCommand.Feedback) {}
|
|
||||||
rpc GetDepthImage(GetDepthImageCommand.Request) returns (GetDepthImageCommand.Feedback) {}
|
|
||||||
rpc GetRGBDImages(GetRGBDImagesCommand.Request) returns (GetRGBDImagesCommand.Feedback) {}
|
|
||||||
rpc StartRecording(StartCameraRecordingCommand.Request) returns (StartCameraRecordingCommand.Feedback) {}
|
|
||||||
rpc StopRecording(StopCameraRecordingCommand.Request) returns (StopCameraRecordingCommand.Feedback) {}
|
|
||||||
|
|
||||||
rpc GetRGBImageStream(stream GetRGBImageStreamCommand.Request) returns (stream GetRGBImageStreamCommand.Feedback) {}
|
|
||||||
rpc GetDepthImageStream(stream GetDepthImageStreamCommand.Request) returns (stream GetDepthImageStreamCommand.Feedback) {}
|
|
||||||
rpc GetRGBDImagesStream(stream GetRGBDImagesStreamCommand.Request) returns (stream GetRGBDImagesStreamCommand.Feedback) {}
|
|
||||||
}
|
|
||||||
@ -1,43 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
package cmvr.api;
|
|
||||||
|
|
||||||
import "google/protobuf/timestamp.proto";
|
|
||||||
|
|
||||||
message DeviceLifecycle {
|
|
||||||
enum Lifecycle {
|
|
||||||
STATE_INIT = 0;
|
|
||||||
STATE_READY = 1;
|
|
||||||
STATE_RUNNING = 2;
|
|
||||||
STATE_ERROR = 3;
|
|
||||||
STATE_ESTOP = 4;
|
|
||||||
STATE_STOP = 5;
|
|
||||||
}
|
|
||||||
Lifecycle state = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
message CommandHeader {
|
|
||||||
message Request {
|
|
||||||
string device_id = 1; // 目标设备名称
|
|
||||||
google.protobuf.Timestamp timestamp = 2; // 请求时间
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
bool success = 1; // 是否成功
|
|
||||||
string error_message = 2; // 错误信息（成功时为空）
|
|
||||||
google.protobuf.Timestamp timestamp = 3; // 回复时间
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message ConfigParam {
|
|
||||||
|
|
||||||
string param_name = 1;
|
|
||||||
|
|
||||||
oneof param_value {
|
|
||||||
int32 int_value = 2; // 整数类型
|
|
||||||
double double_value = 3; // 浮点数类型
|
|
||||||
string string_value = 4; // 字符串类型
|
|
||||||
bool bool_value = 5; // 布尔类型
|
|
||||||
bytes bytes_value = 6; // 二进制数据类型
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,172 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
import "cmvr/api/common.proto";
|
|
||||||
|
|
||||||
package cmvr.api;
|
|
||||||
|
|
||||||
// 自由度值（用于设置指令）
|
|
||||||
message FreedomValue {
|
|
||||||
string id = 1; // 自由度id，可选值：
|
|
||||||
// little_finger（小拇指）、
|
|
||||||
// ring_finger（无名指）、
|
|
||||||
// middle_finger（中指）、
|
|
||||||
// index_finger（食指）、
|
|
||||||
// thumb_bend（大拇指弯曲）、
|
|
||||||
// thumb_rotate（大拇指旋转）
|
|
||||||
float value = 2; // 值 0-1百分比（对应实际物理量的比例）
|
|
||||||
}
|
|
||||||
|
|
||||||
// 自由度状态（用于获取状态指令的返回）
|
|
||||||
message FreedomState {
|
|
||||||
string dof_id = 1; // 自由度id，与FreedomValue.id一致，可选值：
|
|
||||||
// little_finger（小拇指）、
|
|
||||||
// ring_finger（无名指）、
|
|
||||||
// middle_finger（中指）、
|
|
||||||
// index_finger（食指）、
|
|
||||||
// thumb_bend（大拇指弯曲）、
|
|
||||||
// thumb_rotate（大拇指旋转）
|
|
||||||
int32 angle = 2; // 自由度当前角度（单位：根据设备定义，如0.1°/单位）
|
|
||||||
int32 speed = 3; // 自由度当前速度（单位：根据设备定义，如0.1°/s/单位）
|
|
||||||
int32 force = 4; // 实际受力（单位：根据设备定义，如0.01N/单位）
|
|
||||||
int32 position = 5; // 执行器当前位置（单位：根据设备定义，如步长/毫米）
|
|
||||||
int32 current = 6; // 执行器实际电流（单位：根据设备定义，如mA）
|
|
||||||
int32 temperature = 7; // 执行器温度（单位：℃）
|
|
||||||
int32 error = 8; // 执行器故障码（0表示无故障，非0对应具体故障）
|
|
||||||
repeated string error_message = 9; // 错误消息列表（详细描述故障原因）
|
|
||||||
}
|
|
||||||
|
|
||||||
// 传感器数据
|
|
||||||
message SensorData {
|
|
||||||
// 手指类型（掌心作为特殊类型）
|
|
||||||
enum FingerType {
|
|
||||||
PINKY = 0; // 小拇指
|
|
||||||
RING = 1; // 无名指
|
|
||||||
MIDDLE_FINGER = 2; // 中指（避免与PartType冲突）
|
|
||||||
INDEX = 3; // 食指
|
|
||||||
THUMB = 4; // 大拇指
|
|
||||||
PALM = 5; // 掌心
|
|
||||||
}
|
|
||||||
|
|
||||||
// 部位类型（区分手指的不同部位）
|
|
||||||
enum PartType {
|
|
||||||
TIP = 0; // 指端（指尖最前端）
|
|
||||||
FINGER = 1; // 指节（手指中段）
|
|
||||||
PAD = 2; // 指腹（手指接触物体的面）
|
|
||||||
THUMB_MIDDLE = 3; // 大拇指指中（仅大拇指特有部位）
|
|
||||||
PALM_PAD = 4; // 掌心部位（掌心接触区域）
|
|
||||||
}
|
|
||||||
|
|
||||||
FingerType finger_type = 4; // 所属手指（或掌心）
|
|
||||||
PartType part_type = 5; // 所属部位
|
|
||||||
string sensor_name = 6; // 传感器名称（如"little_finger_tip"，便于调试）
|
|
||||||
|
|
||||||
// 传感器单行数据
|
|
||||||
message RowData {
|
|
||||||
repeated int32 values = 1 [packed = true]; // 单行传感器数据（原始采样值）
|
|
||||||
}
|
|
||||||
repeated RowData data = 1; // 二维传感器数据（按行列组织）
|
|
||||||
int32 rows = 2; // 数据行数（传感器阵列行数）
|
|
||||||
int32 cols = 3; // 数据列数（传感器阵列列数）
|
|
||||||
}
|
|
||||||
|
|
||||||
// 灵巧手整体状态
|
|
||||||
message DexHandState {
|
|
||||||
bool is_initialized = 1; // 设备是否初始化完成（true：已就绪，false：未就绪）
|
|
||||||
repeated FreedomState hands = 2; // 所有自由度的状态列表（与FreedomValue.id一一对应）
|
|
||||||
}
|
|
||||||
|
|
||||||
// 获取灵巧手状态指令
|
|
||||||
message GetDexHandStateCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1; // 通用请求头（包含设备ID等）
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1; // 通用响应头（包含成功状态、时间戳等）
|
|
||||||
DexHandState state = 2; // 灵巧手状态数据
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 设置灵巧手位置指令
|
|
||||||
message SetDexHandPositionsCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1; // 通用请求头
|
|
||||||
repeated FreedomValue values = 2; // 待设置的位置参数（百分比）
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1; // 通用响应头（反馈设置结果）
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 设置灵巧手角度指令
|
|
||||||
message SetDexHandAnglesCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1; // 通用请求头
|
|
||||||
repeated FreedomValue values = 2; // 待设置的角度参数（百分比）
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1; // 通用响应头
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 设置灵巧手力值指令
|
|
||||||
message SetDexHandForceCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1; // 通用请求头
|
|
||||||
repeated FreedomValue values = 2; // 待设置的力值参数（百分比）
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1; // 通用响应头
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 设置灵巧手速度指令
|
|
||||||
message SetDexHandSpeedCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1; // 通用请求头
|
|
||||||
repeated FreedomValue values = 2; // 待设置的速度参数（百分比）
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1; // 通用响应头
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 设置灵巧手预设动作指令
|
|
||||||
message SetDexHandPresetActCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1; // 通用请求头
|
|
||||||
int32 presetActId = 2; // 预设动作ID（如0：握拳，1：伸开等）
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1; // 通用响应头
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 获取传感器数据指令（单次）
|
|
||||||
message GetSensorDataCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1; // 通用请求头
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1; // 通用响应头
|
|
||||||
repeated SensorData sensor = 2; // 所有传感器的实时数据
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 获取传感器数据指令（流模式）
|
|
||||||
message GetSensorDataStreamCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1; // 通用请求头
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1; // 通用响应头
|
|
||||||
repeated SensorData sensor = 2; // 流式返回的传感器数据
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,18 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
import "cmvr/api/dexhand_command.proto";
|
|
||||||
|
|
||||||
package cmvr.api;
|
|
||||||
|
|
||||||
|
|
||||||
service DexHandService {
|
|
||||||
// 基本控制
|
|
||||||
rpc GetStatus(GetDexHandStateCommand.Request) returns (GetDexHandStateCommand.Feedback);
|
|
||||||
rpc SetDexHandPos(SetDexHandPositionsCommand.Request) returns (SetDexHandPositionsCommand.Feedback);
|
|
||||||
rpc SetDexHandAngle(SetDexHandAnglesCommand.Request) returns (SetDexHandAnglesCommand.Feedback);
|
|
||||||
rpc SetDexHandForce(SetDexHandForceCommand.Request) returns (SetDexHandForceCommand.Feedback);
|
|
||||||
rpc SetDexHandSpeed(SetDexHandSpeedCommand.Request) returns (SetDexHandSpeedCommand.Feedback);
|
|
||||||
rpc SetDexHandPresetAct(SetDexHandPresetActCommand.Request) returns (SetDexHandPresetActCommand.Feedback);
|
|
||||||
rpc GetSensorData(GetSensorDataCommand.Request) returns (GetSensorDataCommand.Feedback);
|
|
||||||
rpc GetSensorDataStream(stream GetSensorDataStreamCommand.Request) returns (stream GetSensorDataStreamCommand.Feedback);
|
|
||||||
}
|
|
||||||
@ -1,67 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
package cmvr.api;
|
|
||||||
|
|
||||||
|
|
||||||
message Vec2 {
|
|
||||||
double x = 1;
|
|
||||||
double y = 2;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Vec3 {
|
|
||||||
double x = 1;
|
|
||||||
double y = 2;
|
|
||||||
double z = 3;
|
|
||||||
}
|
|
||||||
|
|
||||||
message SE2Pose {
|
|
||||||
Vec2 position = 1; // (m)
|
|
||||||
double angle = 2; // (rad)
|
|
||||||
}
|
|
||||||
|
|
||||||
message SE2Velocity {
|
|
||||||
Vec2 linear = 1; // (m/s)
|
|
||||||
double angular = 2; // (rad/s)
|
|
||||||
}
|
|
||||||
|
|
||||||
message Quaternion {
|
|
||||||
double x = 1;
|
|
||||||
double y = 2;
|
|
||||||
double z = 3;
|
|
||||||
double w = 4;
|
|
||||||
}
|
|
||||||
|
|
||||||
message EulerAngleZYX {
|
|
||||||
double z = 1;
|
|
||||||
double y = 2;
|
|
||||||
double x = 3;
|
|
||||||
}
|
|
||||||
|
|
||||||
message SE3Pose {
|
|
||||||
Vec3 position = 1; // (m)
|
|
||||||
oneof rotation {
|
|
||||||
Quaternion quaternion = 2;
|
|
||||||
EulerAngleZYX euler = 3;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message Inertial {
|
|
||||||
// Mass (kg)
|
|
||||||
double mass = 1;
|
|
||||||
|
|
||||||
// Center of mass (m)
|
|
||||||
Vec3 center_of_mass = 2;
|
|
||||||
|
|
||||||
// Inertia tensor
|
|
||||||
Inertia inertia = 3;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Inertia tensor components (kg*m^2)
|
|
||||||
message Inertia {
|
|
||||||
double ixx = 1;
|
|
||||||
double iyy = 2;
|
|
||||||
double izz = 3;
|
|
||||||
double ixy = 4;
|
|
||||||
double ixz = 5;
|
|
||||||
double iyz = 6;
|
|
||||||
}
|
|
||||||
@ -1,24 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
import "cmvr/api/common.proto";
|
|
||||||
|
|
||||||
package cmvr.api;
|
|
||||||
|
|
||||||
|
|
||||||
message Touch{
|
|
||||||
message Request{
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
// 触点坐标，unit: ms
|
|
||||||
int32 u = 2; // 水平坐标
|
|
||||||
int32 v = 3; // 垂直坐标
|
|
||||||
|
|
||||||
// 触控时允许的最大作用力，unit: N
|
|
||||||
double max_force = 4;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Response{
|
|
||||||
CommandHeader.Feedback header= 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
@ -1,9 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
import "cmvr/api/hlc_command.proto";
|
|
||||||
|
|
||||||
package cmvr.api;
|
|
||||||
|
|
||||||
service HlcService{
|
|
||||||
rpc touch(Touch.Request) returns (Touch.Response);
|
|
||||||
}
|
|
||||||
@ -1,32 +0,0 @@
|
|||||||
|
|
||||||
|
|
||||||
syntax = "proto3";
|
|
||||||
package cmvr.api;
|
|
||||||
import "cmvr/api/common.proto";
|
|
||||||
|
|
||||||
message JointCmd {
|
|
||||||
string joint_name = 1; // 关节名称
|
|
||||||
double rad = 2; // 弧度
|
|
||||||
double vel = 3; // 角速度，rad/s
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
message MoveJ{
|
|
||||||
message Request{
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
repeated JointCmd cmds = 2;
|
|
||||||
double vel = 3;
|
|
||||||
double acc = 4;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Response{
|
|
||||||
CommandHeader.Feedback header= 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
service HumanoidRobotService{
|
|
||||||
rpc torqueOff(CommandHeader.Request) returns (CommandHeader.Feedback);
|
|
||||||
rpc torqueOn(CommandHeader.Request) returns (CommandHeader.Feedback);
|
|
||||||
rpc moveJ(MoveJ.Request) returns (MoveJ.Response);
|
|
||||||
}
|
|
||||||
@ -1,122 +0,0 @@
|
|||||||
|
|
||||||
|
|
||||||
syntax = "proto3";
|
|
||||||
package cmvr.api;
|
|
||||||
import "cmvr/api/common.proto";
|
|
||||||
|
|
||||||
message JointCmd {
|
|
||||||
string joint_name = 1; // 关节名称
|
|
||||||
double rad = 2; // 弧度
|
|
||||||
double vel = 3; // 角速度，rad/s
|
|
||||||
}
|
|
||||||
|
|
||||||
message Pose3D{
|
|
||||||
double x = 1;
|
|
||||||
double y = 2;
|
|
||||||
double z = 3;
|
|
||||||
double rx = 4;
|
|
||||||
double ry = 5;
|
|
||||||
double rz = 6;
|
|
||||||
}
|
|
||||||
|
|
||||||
enum RobotCartesian{
|
|
||||||
X = 0;
|
|
||||||
Y = 1;
|
|
||||||
Z = 2;
|
|
||||||
RX = 3;
|
|
||||||
RY = 4;
|
|
||||||
RZ = 5;
|
|
||||||
} ;
|
|
||||||
enum RobotJointIndexDirection{
|
|
||||||
FORWARD = 0;
|
|
||||||
BACKWARD = 1;
|
|
||||||
X_POSITIVE = 2; // X轴正向
|
|
||||||
X_NEGATIVE = 3; // X轴负向
|
|
||||||
Y_POSITIVE = 4; // Y轴正向
|
|
||||||
Y_NEGATIVE = 5; // Y轴负向
|
|
||||||
Z_POSITIVE = 6; // Z轴正向
|
|
||||||
Z_NEGATIVE = 7; // Z轴负向
|
|
||||||
ROTATE_X = 8; // 绕X轴旋转
|
|
||||||
ROTATE_Y = 9; // 绕Y轴旋转
|
|
||||||
ROTATE_Z = 10; // 绕Z轴旋转
|
|
||||||
}
|
|
||||||
message MoveJ{
|
|
||||||
message Request{
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
repeated JointCmd cmds = 2;
|
|
||||||
double vel = 3;
|
|
||||||
double acc = 4;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Response{
|
|
||||||
CommandHeader.Feedback header= 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
message MoveL{
|
|
||||||
|
|
||||||
message Request{
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
string ee_link = 2; //连杆名称
|
|
||||||
Pose3D targetPose = 3;
|
|
||||||
double vel = 4;
|
|
||||||
double acc = 5;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Response{
|
|
||||||
CommandHeader.Feedback header= 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
message SpeedJ{
|
|
||||||
|
|
||||||
message Request{
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
string joint_name = 2;
|
|
||||||
double vel = 3;
|
|
||||||
double acc = 4;
|
|
||||||
RobotJointIndexDirection dir = 5;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Response{
|
|
||||||
CommandHeader.Feedback header= 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message SpeedL{
|
|
||||||
message Request{
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
string ee_link = 2;
|
|
||||||
double vel = 3;
|
|
||||||
double acc = 4;
|
|
||||||
RobotJointIndexDirection dir = 5;
|
|
||||||
RobotCartesian cart = 6;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Response{
|
|
||||||
CommandHeader.Feedback header= 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 关节状态消息
|
|
||||||
message JointState {
|
|
||||||
repeated string name = 1; // 关节名称
|
|
||||||
repeated double position = 2; // 关节位置
|
|
||||||
repeated double velocity = 3; // 可选：关节速度
|
|
||||||
repeated double effort = 4; // 可选：关节力矩
|
|
||||||
double timestamp = 5; // 可选：时间戳
|
|
||||||
}
|
|
||||||
|
|
||||||
// 返回消息
|
|
||||||
message JointResponse {
|
|
||||||
CommandHeader.Feedback header= 1;
|
|
||||||
repeated JointState state = 2;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 请求消息（可选）
|
|
||||||
message JointRequest {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
@ -1,16 +0,0 @@
|
|||||||
|
|
||||||
|
|
||||||
syntax = "proto3";
|
|
||||||
package cmvr.api;
|
|
||||||
import "cmvr/api/common.proto";
|
|
||||||
import "cmvr/api/humanoid_robot_command.proto";
|
|
||||||
|
|
||||||
service HumanoidRobotService{
|
|
||||||
rpc torqueOff(CommandHeader.Request) returns (CommandHeader.Feedback);
|
|
||||||
rpc torqueOn(CommandHeader.Request) returns (CommandHeader.Feedback);
|
|
||||||
rpc moveJ(MoveJ.Request) returns (MoveJ.Response);
|
|
||||||
rpc moveL(MoveL.Request) returns (MoveL.Response);
|
|
||||||
rpc speedJ(SpeedJ.Request) returns (SpeedJ.Response);
|
|
||||||
rpc speedL(SpeedL.Request) returns (SpeedL.Response);
|
|
||||||
rpc getJointState(JointRequest) returns (JointResponse);
|
|
||||||
}
|
|
||||||
@ -1,81 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
import "cmvr/api/common.proto";
|
|
||||||
|
|
||||||
package cmvr.api;
|
|
||||||
|
|
||||||
message MicState {
|
|
||||||
bool is_initialized = 1;
|
|
||||||
bool is_running = 2;
|
|
||||||
bool is_recording = 3;
|
|
||||||
int32 volume = 4;
|
|
||||||
string error_message = 5;
|
|
||||||
}
|
|
||||||
message GetMicStateCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
MicState state = 2;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
message StartMicRecordingCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
string file_path = 2;
|
|
||||||
}
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message StopMicRecordingCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message PauseMicRecordingCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message ResumeMicRecordingCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message SetMicPhoneVolumeCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
int32 volume = 2; // 音量值（0 ~ 100）
|
|
||||||
}
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message GetMicPhoneVolumeCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
int32 volume = 2; // 当前音量值
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
@ -1,20 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
import "cmvr/api/microphone_command.proto";
|
|
||||||
|
|
||||||
package cmvr.api;
|
|
||||||
|
|
||||||
|
|
||||||
service MicPhoneService {
|
|
||||||
// 基本控制
|
|
||||||
rpc GetStatus(GetMicStateCommand.Request) returns (GetMicStateCommand.Feedback);
|
|
||||||
rpc StartRecord(StartMicRecordingCommand.Request) returns (StartMicRecordingCommand.Feedback);
|
|
||||||
rpc StopRecord(StopMicRecordingCommand.Request) returns (StopMicRecordingCommand.Feedback);
|
|
||||||
rpc PauseRecord(PauseMicRecordingCommand.Request) returns (PauseMicRecordingCommand.Feedback);
|
|
||||||
rpc ResumeRecord(ResumeMicRecordingCommand.Request) returns (ResumeMicRecordingCommand.Feedback);
|
|
||||||
|
|
||||||
// 音量控制
|
|
||||||
rpc SetVolume(SetMicPhoneVolumeCommand.Request) returns (SetMicPhoneVolumeCommand.Feedback);
|
|
||||||
rpc GetVolume(GetMicPhoneVolumeCommand.Request) returns (GetMicPhoneVolumeCommand.Feedback);
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -1,99 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
import "cmvr/api/common.proto";
|
|
||||||
|
|
||||||
package cmvr.api;
|
|
||||||
|
|
||||||
|
|
||||||
// 音频数据格式（与麦克风类似，但用于输出）
|
|
||||||
message AudioData {
|
|
||||||
enum AudioFormat {
|
|
||||||
PCM = 0;
|
|
||||||
MP3 = 1;
|
|
||||||
AAC = 2;
|
|
||||||
WAV = 3;
|
|
||||||
}
|
|
||||||
bytes data = 1; // 音频二进制数据
|
|
||||||
int32 sample_rate = 2; // 采样率（Hz）
|
|
||||||
int32 channels = 3; // 声道数
|
|
||||||
AudioFormat format = 4; // 音频格式
|
|
||||||
string codec = 5; // 编码方式
|
|
||||||
}
|
|
||||||
|
|
||||||
// 扬声器状态
|
|
||||||
message SpeakerState {
|
|
||||||
bool is_initialized = 1; // 是否已初始化
|
|
||||||
bool is_running = 2; // 是否正在播放
|
|
||||||
bool is_decoding = 3;
|
|
||||||
bool is_paused = 5;
|
|
||||||
int32 volume = 6; // 当前音量（0 ~ 100）
|
|
||||||
string error_message = 7; // 错误信息
|
|
||||||
}
|
|
||||||
|
|
||||||
// 各种扬声器命令
|
|
||||||
message GetSpeakerStateCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
SpeakerState state = 2;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message PlayAudioCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
string audio_path = 2;
|
|
||||||
}
|
|
||||||
message Feedback { CommandHeader.Feedback header = 1; }
|
|
||||||
}
|
|
||||||
|
|
||||||
message StopSpeakerCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message PauseSpeakerCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message ResumeSpeakerCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message SetSpeakerVolumeCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
int32 volume = 2; // 音量值（0 ~ 100）
|
|
||||||
}
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message GetSpeakerVolumeCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
}
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
int32 volume = 2; // 当前音量值
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
@ -1,20 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
import "cmvr/api/speaker_command.proto";
|
|
||||||
|
|
||||||
package cmvr.api;
|
|
||||||
|
|
||||||
|
|
||||||
service SpeakerService {
|
|
||||||
// 基本控制
|
|
||||||
rpc GetStatus(GetSpeakerStateCommand.Request) returns (GetSpeakerStateCommand.Feedback);
|
|
||||||
rpc PlayAudio(PlayAudioCommand.Request) returns (PlayAudioCommand.Feedback);
|
|
||||||
rpc StopPlayback(StopSpeakerCommand.Request) returns (StopSpeakerCommand.Feedback);
|
|
||||||
rpc PausePlayback(PauseSpeakerCommand.Request) returns (PauseSpeakerCommand.Feedback);
|
|
||||||
rpc ResumePlayback(ResumeSpeakerCommand.Request) returns (ResumeSpeakerCommand.Feedback);
|
|
||||||
|
|
||||||
// 音量控制
|
|
||||||
rpc SetVolume(SetSpeakerVolumeCommand.Request) returns (SetSpeakerVolumeCommand.Feedback);
|
|
||||||
rpc GetVolume(GetSpeakerVolumeCommand.Request) returns (GetSpeakerVolumeCommand.Feedback);
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -1,61 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
import "cmvr/api/common.proto";
|
|
||||||
|
|
||||||
package cmvr.api;
|
|
||||||
|
|
||||||
enum DeviceType {
|
|
||||||
AGV = 0;
|
|
||||||
Battery = 1;
|
|
||||||
Camera = 2;
|
|
||||||
DexHand = 3;
|
|
||||||
Gripper = 4;
|
|
||||||
Microphone = 5;
|
|
||||||
Robot = 6;
|
|
||||||
Speaker = 7;
|
|
||||||
Unknown = 20;
|
|
||||||
}
|
|
||||||
|
|
||||||
message DeviceList {
|
|
||||||
string device_id = 1;
|
|
||||||
DeviceType device_type = 2;
|
|
||||||
}
|
|
||||||
|
|
||||||
message GetSystemInfoCommand {
|
|
||||||
message Request {}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
string system_name = 2;
|
|
||||||
string version = 3;
|
|
||||||
string description = 4;
|
|
||||||
string os = 5;
|
|
||||||
string kernel_version = 6;
|
|
||||||
string architecture = 7;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message GetSystemStatusCommand {
|
|
||||||
message Request {}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
float cpu_usage = 2;
|
|
||||||
float mem_total_mb = 3;
|
|
||||||
float mem_used_mb = 4;
|
|
||||||
float disk_total_gb = 5;
|
|
||||||
float disk_used_gb = 6;
|
|
||||||
repeated DeviceList device_list = 7;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
message UpdateParamsCommand {
|
|
||||||
message Request {
|
|
||||||
CommandHeader.Request header = 1;
|
|
||||||
repeated ConfigParam params = 2;
|
|
||||||
}
|
|
||||||
|
|
||||||
message Feedback {
|
|
||||||
CommandHeader.Feedback header = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,13 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
import "cmvr/api/system_command.proto";
|
|
||||||
|
|
||||||
package cmvr.api;
|
|
||||||
|
|
||||||
|
|
||||||
service SystemService {
|
|
||||||
rpc GetSystemInfo(GetSystemInfoCommand.Request) returns (GetSystemInfoCommand.Feedback) {}
|
|
||||||
rpc GetSystemStatus(GetSystemStatusCommand.Request) returns (GetSystemStatusCommand.Feedback) {}
|
|
||||||
|
|
||||||
rpc UpdateParams(UpdateParamsCommand.Request) returns (UpdateParamsCommand.Feedback) {}
|
|
||||||
}
|
|
||||||
@ -1,17 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
package cmvr.api;
|
|
||||||
|
|
||||||
message TestReqeust {
|
|
||||||
string data = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
message TestResponse {
|
|
||||||
string data = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
service TestService{
|
|
||||||
rpc Call (TestReqeust) returns (TestResponse);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,55 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
package cmvr.msgs;
|
|
||||||
|
|
||||||
message CANCardParameter {
|
|
||||||
enum CANCardBrand {
|
|
||||||
FAKE_CAN = 0;
|
|
||||||
ESD_CAN = 1;
|
|
||||||
SOCKET_CAN_RAW = 2;
|
|
||||||
HERMES_CAN = 3;
|
|
||||||
}
|
|
||||||
|
|
||||||
enum CANCardType {
|
|
||||||
PCI_CARD = 0;
|
|
||||||
USB_CARD = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
enum CANChannelId {
|
|
||||||
CHANNEL_ID_ZERO = 0;
|
|
||||||
CHANNEL_ID_ONE = 1;
|
|
||||||
CHANNEL_ID_TWO = 2;
|
|
||||||
CHANNEL_ID_THREE = 3;
|
|
||||||
CHANNEL_ID_FOUR = 4;
|
|
||||||
CHANNEL_ID_FIVE = 5;
|
|
||||||
CHANNEL_ID_SIX = 6;
|
|
||||||
CHANNEL_ID_SEVEN = 7;
|
|
||||||
}
|
|
||||||
|
|
||||||
enum CANInterface {
|
|
||||||
NATIVE = 0;
|
|
||||||
VIRTUAL = 1;
|
|
||||||
SLCAN = 2;
|
|
||||||
}
|
|
||||||
|
|
||||||
enum BAUDRATE {
|
|
||||||
BCAN_BAUDRATE_1M = 0;
|
|
||||||
BCAN_BAUDRATE_500K = 1;
|
|
||||||
BCAN_BAUDRATE_250K = 2;
|
|
||||||
BCAN_BAUDRATE_150K = 3;
|
|
||||||
BCAN_BAUDRATE_NUM = 4;
|
|
||||||
}
|
|
||||||
|
|
||||||
// CAN卡驱动类型配置 | 根据所用的CAN卡硬件型号或驱动类型配置
|
|
||||||
optional CANCardBrand brand = 1;
|
|
||||||
// CAN卡硬件接口类型配置 | 根据所用的CAN卡硬件接口类型或驱动类型配置
|
|
||||||
optional CANCardType type = 2;
|
|
||||||
// CAN卡端口号配置 | 根据所连接的CAN卡端口号配置
|
|
||||||
optional CANChannelId channel_id = 3;
|
|
||||||
// CAN卡软件接口配置
|
|
||||||
optional CANInterface interface = 4;
|
|
||||||
// CAN卡端口数量配置
|
|
||||||
optional uint32 num_ports = 5;
|
|
||||||
// CAN卡波特率配置
|
|
||||||
optional BAUDRATE baudrate = 6;
|
|
||||||
}
|
|
||||||
@ -1,202 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
package cmvr.msgs;
|
|
||||||
|
|
||||||
message SdoFrame {
|
|
||||||
uint32 node_id = 1; // 节点ID
|
|
||||||
CommandSpecifier cs = 2; // SDO命令字
|
|
||||||
ObIndex index = 3; // 对象字典索引
|
|
||||||
ObSubIndex sub_index = 4; // 子索引
|
|
||||||
uint32 data = 5; // 数据区
|
|
||||||
}
|
|
||||||
|
|
||||||
enum PdoBaseId{
|
|
||||||
|
|
||||||
PDO_BASE_ID_UNSPECIFIED = 0;
|
|
||||||
|
|
||||||
RPDO1_BASE_ID_200 = 0x200;
|
|
||||||
RPDO2_BASE_ID_300 = 0x300;
|
|
||||||
RPDO3_BASE_ID_400 = 0x400;
|
|
||||||
RPDO4_BASE_ID_500 = 0x500;
|
|
||||||
|
|
||||||
|
|
||||||
TPDO1_BASE_ID_180 = 0x180;
|
|
||||||
TPDO2_BASE_ID_280 = 0x280;
|
|
||||||
TPDO3_BASE_ID_380 = 0x380;
|
|
||||||
TPDO4_BASE_ID_480 = 0x480;
|
|
||||||
}
|
|
||||||
|
|
||||||
// PDO 传输类型
|
|
||||||
enum TransmissionType {
|
|
||||||
|
|
||||||
// 同步传输
|
|
||||||
SYNC_EVENT_DRIVEN = 0x00; // 非循环同步
|
|
||||||
SYNC_CYCLIC = 0x01; // 循环同步
|
|
||||||
|
|
||||||
// 远程请求触发
|
|
||||||
REMOTE_SYNC = 0xFC; // 远程同步
|
|
||||||
REMOTE_ASYNC = 0xFD; // 远程异步
|
|
||||||
|
|
||||||
// 异步传输
|
|
||||||
ASYNC_MANUFACTURER_SPECIFIC = 0xFE; // 异步，制造商特定事件
|
|
||||||
ASYNC_DEVICE_SPECIFIC = 0xFF; // 异步，设备子协议特定事件
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
enum CommandSpecifier {
|
|
||||||
CS_NO = 0; // 默认无效
|
|
||||||
CS_WRITE_ONE_BYTE = 0x2F; // 写 1 字节
|
|
||||||
CS_WRITE_TWO_BYTES = 0x2B; // 写 2 字节
|
|
||||||
CS_WRITE_THREE_BYTES = 0x27; // 写 3 字节
|
|
||||||
CS_WRITE_FOUR_BYTES = 0x23; // 写 4 字节
|
|
||||||
CS_WRITE_SUCCESS_RESPONSE = 0x60; // 写成功响应
|
|
||||||
|
|
||||||
CS_READ_REQUEST = 0x40; // 发起读取请求
|
|
||||||
CS_READ_RESPONSE_ONE_BYTE = 0x4F; // 响应 1 字节
|
|
||||||
CS_READ_RESPONSE_TWO_BYTES = 0x4B;// 响应 2 字节
|
|
||||||
CS_READ_RESPONSE_THREE_BYTES = 0x47;
|
|
||||||
CS_READ_RESPONSE_FOUR_BYTES = 0x43;
|
|
||||||
|
|
||||||
CS_EXCEPTION_RESPONSE = 0x80; // 异常响应
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
enum NmtState {
|
|
||||||
// 0x00 - 正在初始化（Initializing）
|
|
||||||
NMT_INITIALIZING = 0x00;
|
|
||||||
|
|
||||||
// 0x01 - 未连接 / 应用复位（Reset Application）
|
|
||||||
NMT_RESET_APPLICATION = 0x01;
|
|
||||||
|
|
||||||
// 0x02 - 正在连接（Connecting）
|
|
||||||
NMT_CONNECTING = 0x02;
|
|
||||||
|
|
||||||
// 0x03 - 准备（Preparing）
|
|
||||||
NMT_PREPARING = 0x03;
|
|
||||||
|
|
||||||
// 0x04 - 停止状态（Stopped），开启 SDO，关闭 PDO
|
|
||||||
NMT_STOPPED = 0x04;
|
|
||||||
|
|
||||||
// 0x05 - 运行状态（Operational），开启 SDO、PDO、NMT
|
|
||||||
NMT_OPERATIONAL = 0x05;
|
|
||||||
|
|
||||||
// 0x7F - 预运行状态（Pre-Operational），开启 SDO，关闭 PDO
|
|
||||||
NMT_PRE_OPERATIONAL = 0x7F;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
enum NmtCommand {
|
|
||||||
// 未指定命令
|
|
||||||
NMT_COMMAND_UNSPECIFIED = 0x00;
|
|
||||||
|
|
||||||
// 0x01 - 启动远程节点（Start Remote Node）
|
|
||||||
NMT_START_REMOTE_NODE = 0x01;
|
|
||||||
|
|
||||||
// 0x02 - 停止远程节点（Stop Remote Node）
|
|
||||||
NMT_STOP_REMOTE_NODE = 0x02;
|
|
||||||
|
|
||||||
// 0x80 - 进入预操作状态（Enter Pre-Operational）
|
|
||||||
NMT_ENTER_PRE_OPERATIONAL = 0x80;
|
|
||||||
|
|
||||||
// 0x81 - 复位节点（Reset Node）
|
|
||||||
NMT_RESET_NODE = 0x81;
|
|
||||||
|
|
||||||
// 0x82 - 复位通信（Reset Communication）
|
|
||||||
NMT_RESET_COMMUNICATION = 0x82;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// 索引
|
|
||||||
enum ObIndex {
|
|
||||||
INDEX_ZERO = 0;
|
|
||||||
USER_SAVE_PARA_2000 = 0x2000; // 下发命令 1 保存参数
|
|
||||||
POSITION_OFFSET_2008 = 0x2008; // 位置偏移，子索引 0x00，用于设置零点、起始位置
|
|
||||||
// Error Codes
|
|
||||||
ERROR_CODE_6007 = 0x6007;
|
|
||||||
ERROR_CODE_603F = 0x603F;
|
|
||||||
|
|
||||||
// Control and Status
|
|
||||||
CONTROL_WORD_6040 = 0x6040;
|
|
||||||
STATUS_WORD_6041 = 0x6041;
|
|
||||||
|
|
||||||
// Operation Modes
|
|
||||||
OPERATION_MODE_6060 = 0x6060;
|
|
||||||
MODE_DISPLAY_6061 = 0x6061;
|
|
||||||
|
|
||||||
// Actual Values
|
|
||||||
ACTUAL_POSITION_6064 = 0x6064;
|
|
||||||
ACTUAL_SPEED_606C = 0x606C;
|
|
||||||
ACTUAL_CURRENT_6078 = 0x6078;
|
|
||||||
|
|
||||||
// Torque-related
|
|
||||||
TARGET_TORQUE_6071 = 0x6071;
|
|
||||||
MAX_TORQUE_6072 = 0x6072;
|
|
||||||
DEMAND_TORQUE_6074 = 0x6074;
|
|
||||||
|
|
||||||
// Position-related
|
|
||||||
TARGET_POSITION_607A = 0x607A;
|
|
||||||
SOFTWARE_POSITION_LIMIT_607D = 0x607D; // Sub-indexes: 1, 2
|
|
||||||
|
|
||||||
// Speed-related
|
|
||||||
MAX_SPEED_607F = 0x607F;
|
|
||||||
PROFILE_SPEED_6081 = 0x6081;
|
|
||||||
PROFILE_ACCELERATION_6083 = 0x6083;
|
|
||||||
PROFILE_DECELERATION_6084 = 0x6084;
|
|
||||||
|
|
||||||
// Same as DEMAND_TORQUE? Verify correctness.
|
|
||||||
// TORQUE_SLOPE_6074 = 0x6074;
|
|
||||||
|
|
||||||
// PID Control
|
|
||||||
CURRENT_LOOP_PID_60F6 = 0x60F6; // Sub-indexes: 1, 2
|
|
||||||
SPEED_LOOP_PID_60F9 = 0x60F9; // Sub-indexes: 1, 2
|
|
||||||
POSITION_LOOP_PID_60FB = 0x60FB; // Sub-indexes: 1, 2, 3
|
|
||||||
|
|
||||||
// Target Speed
|
|
||||||
TARGET_SPEED_60FF = 0x60FF;
|
|
||||||
|
|
||||||
QUICK_STOP_OPTION_605A = 0x605A;
|
|
||||||
QUICK_STOP_DECEL_6085 = 0x6085;
|
|
||||||
|
|
||||||
// -------------------------
|
|
||||||
// PDO 通信参数对象（Communication Object）
|
|
||||||
RPDO1_COMM_1400 = 0x1400;
|
|
||||||
RPDO2_COMM_1401 = 0x1401;
|
|
||||||
RPDO3_COMM_1402 = 0x1402;
|
|
||||||
RPDO4_COMM_1403 = 0x1403;
|
|
||||||
|
|
||||||
TPDO1_COMM_1800 = 0x1800;
|
|
||||||
TPDO2_COMM_1801 = 0x1801;
|
|
||||||
TPDO3_COMM_1802 = 0x1802;
|
|
||||||
TPDO4_COMM_1803 = 0x1803;
|
|
||||||
|
|
||||||
// PDO 映射对象（Mapping Object）
|
|
||||||
RPDO1_MAP_1600 = 0x1600;
|
|
||||||
RPDO2_MAP_1601 = 0x1601;
|
|
||||||
RPDO3_MAP_1602 = 0x1602;
|
|
||||||
RPDO4_MAP_1603 = 0x1603;
|
|
||||||
|
|
||||||
TPDO1_MAP_1A00 = 0x1A00;
|
|
||||||
TPDO2_MAP_1A01 = 0x1A01;
|
|
||||||
TPDO3_MAP_1A02 = 0x1A02;
|
|
||||||
TPDO4_MAP_1A03 = 0x1A03;
|
|
||||||
|
|
||||||
PRODUCER_HEARTBEAT_TIME = 0x1017;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 子索引
|
|
||||||
enum ObSubIndex {
|
|
||||||
SUB_INDEX_0 = 0;
|
|
||||||
SUB_INDEX_1 = 1;
|
|
||||||
SUB_INDEX_2 = 2;
|
|
||||||
SUB_INDEX_3 = 3;
|
|
||||||
SUB_INDEX_4 = 4;
|
|
||||||
SUB_INDEX_5 = 5;
|
|
||||||
SUB_INDEX_6 = 6;
|
|
||||||
SUB_INDEX_7 = 7;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,29 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
package cmvr.msgs;
|
|
||||||
|
|
||||||
// Error codes enum for API's categorized by modules.
|
|
||||||
enum ErrorCode {
|
|
||||||
// No error, returns on success.
|
|
||||||
OK = 0;
|
|
||||||
|
|
||||||
// Canbus module error codes start from here.
|
|
||||||
CANBUS_ERROR = 2000;
|
|
||||||
CAN_CLIENT_ERROR_BASE = 2100;
|
|
||||||
CAN_CLIENT_ERROR_OPEN_DEVICE_FAILED = 2101;
|
|
||||||
CAN_CLIENT_ERROR_FRAME_NUM = 2102;
|
|
||||||
CAN_CLIENT_ERROR_SEND_FAILED = 2103;
|
|
||||||
CAN_CLIENT_ERROR_RECV_FAILED = 2104;
|
|
||||||
|
|
||||||
// motor
|
|
||||||
MOTOR_ERROR = 3000;
|
|
||||||
MOTOR_ERROR_SET_ZERO = 3001;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
message StatusPb {
|
|
||||||
ErrorCode error_code = 1;
|
|
||||||
string msg = 2;
|
|
||||||
}
|
|
||||||
@ -1,118 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
import "cmvr/msgs/canopen.proto";
|
|
||||||
package cmvr.msgs;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/* --------------------------------------------------------
|
|
||||||
* CAN OPEN 定义
|
|
||||||
* --------------------------------------------------------*/
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// 电机控制模式（Operation Mode）
|
|
||||||
enum RunMode {
|
|
||||||
// 未指定
|
|
||||||
RUN_MODE_UNSPECIFIED = 0;
|
|
||||||
|
|
||||||
// Profile Position Mode - 位置模式
|
|
||||||
RUN_MODE_PROFILE_POSITION = 1;
|
|
||||||
|
|
||||||
// Velocity Mode - 简单速度模式
|
|
||||||
RUN_MODE_VELOCITY = 2;
|
|
||||||
|
|
||||||
// Profile Velocity Mode - 速度模式（带轮廓）
|
|
||||||
RUN_MODE_PROFILE_VELOCITY = 3;
|
|
||||||
|
|
||||||
// Torque Mode - 转矩模式
|
|
||||||
RUN_MODE_TORQUE = 4;
|
|
||||||
|
|
||||||
// Homing Mode - 回零模式
|
|
||||||
RUN_MODE_HOMING = 5;
|
|
||||||
|
|
||||||
// Interpolation Mode - 位置插补模式
|
|
||||||
RUN_MODE_INTERPOLATED_POSITION = 7;
|
|
||||||
|
|
||||||
// Cyclic Synchronous Position Mode - 周期位置模式
|
|
||||||
RUN_MODE_CYCLIC_SYNC_POSITION = 8;
|
|
||||||
|
|
||||||
// Cyclic Synchronous Velocity Mode - 周期速度模式
|
|
||||||
RUN_MODE_CYCLIC_SYNC_VELOCITY = 9;
|
|
||||||
|
|
||||||
// Cyclic Synchronous Current Mode - 周期转矩模式
|
|
||||||
RUN_MODE_CYCLIC_SYNC_CURRENT = 10;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// 电机状态数据
|
|
||||||
message MotorStatus {
|
|
||||||
RunMode run_mode = 1; // 当前运行模式
|
|
||||||
|
|
||||||
|
|
||||||
int32 current = 2; // 当前电流（单位：mA）
|
|
||||||
int32 target_current = 3; // 目标电流（单位：mA）
|
|
||||||
int32 speed = 4; // 当前速度，转换为度每秒：(speed / 100 / 减速比) * 360
|
|
||||||
int32 target_speed = 5; // 目标速度，单位同上
|
|
||||||
int32 position = 6; // 当前电机位置（位置转角度：(position / 65536 / 减速比) * 360）
|
|
||||||
int32 target_position = 7; // 目标位置（非位置模式无效）
|
|
||||||
uint32 error_state = 8; // 错误状态位（见位说明）
|
|
||||||
|
|
||||||
int32 speed_kp = 9; // 速度环Kp，含模式和速度信息（bit复用）
|
|
||||||
int32 speed_ki = 10; // 速度环Ki，含模式和速度信息（bit复用）
|
|
||||||
int32 speed_kd = 11; // 速度环Kd
|
|
||||||
|
|
||||||
int32 position_kp = 12; // 位置环Kp
|
|
||||||
int32 position_ki = 13; // 位置环Ki
|
|
||||||
int32 position_kd = 14; // 位置环Kd
|
|
||||||
|
|
||||||
int32 bus_voltage = 15; // 母线电压
|
|
||||||
|
|
||||||
int32 max_abs_current = 16; // 最大电流绝对值（单位：mA）
|
|
||||||
int32 max_pos_current = 17; // 最大正电流（单位：mA）
|
|
||||||
int32 min_neg_current = 18; // 最小负电流（单位：mA）
|
|
||||||
|
|
||||||
int32 max_pos_accel = 19; // 最大正向加速度
|
|
||||||
int32 min_neg_accel = 20; // 最小负向加速度
|
|
||||||
int32 max_pos_velocity = 21; // 最大正向速度（转化为度/秒）
|
|
||||||
int32 min_neg_velocity = 22; // 最小负向速度（转化为度/秒）
|
|
||||||
|
|
||||||
int32 max_pos_position = 23; // 最大正向位置（角度）
|
|
||||||
int32 min_neg_position = 24; // 最小负向位置（角度）
|
|
||||||
|
|
||||||
int32 motor_temp = 25; // 电机温度（单位：摄氏度）
|
|
||||||
int32 board_temp = 26; // 电路板温度（单位：摄氏度）
|
|
||||||
|
|
||||||
int32 current_kp = 27; // 电流P
|
|
||||||
int32 current_ki = 28; // 电流I
|
|
||||||
int32 current_kd = 29; // 电流D
|
|
||||||
|
|
||||||
int32 motor_type = 30; // 电机型号（复合字段，包含刹车、ACTUATOR_TYPE等）
|
|
||||||
int32 motor_version = 31; // 电机版本号（软件+硬件，16进制格式）
|
|
||||||
int32 software_version = 32;// 软件版本号（16进制格式）
|
|
||||||
|
|
||||||
int32 position_offset = 33; // 当前位置偏移（编码器位置 - 偏移值）
|
|
||||||
bytes csp_data = 34; // 获取CSP，8字节数据
|
|
||||||
int32 encoder_voltage = 35; // 编码器电池电压（新版硬件支持）
|
|
||||||
int32 encoder_state = 36; // 编码器状态（双编码器=外圈位置，单编码器=状态）
|
|
||||||
|
|
||||||
int32 overvoltage_limit = 37; // 过压阈值（单位：V）
|
|
||||||
int32 undervoltage_limit = 38; // 欠压阈值（单位：V）
|
|
||||||
int32 coil_over_temp = 39; // 电机线圈过温阈值（单位：℃）
|
|
||||||
int32 driver_over_temp = 40; // 驱动板过温阈值（单位：℃）
|
|
||||||
|
|
||||||
/* ---------------------
|
|
||||||
* CANOPEN 特有的参数
|
|
||||||
* --------------------*/
|
|
||||||
SdoFrame sdo_response = 41;
|
|
||||||
NmtState nmt_state = 42;
|
|
||||||
// 只保留值，具体值在使用的时候使用联合体去解析
|
|
||||||
uint32 ctrl_word = 43;
|
|
||||||
uint32 status_word = 44;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,14 +0,0 @@
|
|||||||
syntax = "proto3";
|
|
||||||
|
|
||||||
import "cmvr/msgs/canopen.proto";
|
|
||||||
import "cmvr/msgs/motor.proto";
|
|
||||||
package cmvr.msgs;
|
|
||||||
|
|
||||||
|
|
||||||
// 包含机器人的所有信息
|
|
||||||
|
|
||||||
message RobotDetail{
|
|
||||||
|
|
||||||
// key 电机 nodeid
|
|
||||||
map<uint32, MotorStatus> motors = 1; // node_id => status
|
|
||||||
}
|
|
||||||
@ -1,74 +0,0 @@
|
|||||||
import os
|
|
||||||
import subprocess
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
def generate_proto():
|
|
||||||
# 获取当前目录
|
|
||||||
current_dir = Path(__file__).parent
|
|
||||||
proto_dir = current_dir / "protos"
|
|
||||||
generated_dir = current_dir / "generated"
|
|
||||||
|
|
||||||
# 创建生成的目录
|
|
||||||
generated_dir.mkdir(parents=True, exist_ok=True)
|
|
||||||
|
|
||||||
# 编译所有 .proto 文件
|
|
||||||
proto_files = []
|
|
||||||
for root, _, files in os.walk(proto_dir):
|
|
||||||
for file in files:
|
|
||||||
if file.endswith(".proto"):
|
|
||||||
proto_files.append(os.path.join(root, file))
|
|
||||||
|
|
||||||
for proto_path in proto_files:
|
|
||||||
# 计算相对路径
|
|
||||||
rel_path = os.path.relpath(proto_path, proto_dir)
|
|
||||||
output_dir = generated_dir
|
|
||||||
|
|
||||||
cmd = [
|
|
||||||
"python", "-m", "grpc_tools.protoc",
|
|
||||||
f"-I{proto_dir}",
|
|
||||||
f"--python_out={output_dir}",
|
|
||||||
f"--grpc_python_out={output_dir}",
|
|
||||||
proto_path
|
|
||||||
]
|
|
||||||
|
|
||||||
print(f"Generating code for {rel_path}...")
|
|
||||||
result = subprocess.run(cmd, capture_output=True, text=True)
|
|
||||||
|
|
||||||
if result.returncode != 0:
|
|
||||||
print(f"Error generating {rel_path}:")
|
|
||||||
print(result.stderr)
|
|
||||||
return False
|
|
||||||
|
|
||||||
print("Proto files generated successfully!")
|
|
||||||
|
|
||||||
# 修复生成的代码中的导入路径
|
|
||||||
fix_imports(generated_dir)
|
|
||||||
|
|
||||||
return True
|
|
||||||
|
|
||||||
def fix_imports(generated_dir):
|
|
||||||
"""修复生成的代码中的导入路径"""
|
|
||||||
for root, _, files in os.walk(generated_dir):
|
|
||||||
for file in files:
|
|
||||||
if file.endswith(".py"):
|
|
||||||
file_path = os.path.join(root, file)
|
|
||||||
with open(file_path, 'r', encoding='utf-8') as f:
|
|
||||||
content = f.read()
|
|
||||||
|
|
||||||
# 替换导入路径
|
|
||||||
content = content.replace(
|
|
||||||
"from cmvr.api import",
|
|
||||||
"from generated.cmvr.api import"
|
|
||||||
)
|
|
||||||
content = content.replace(
|
|
||||||
"import cmvr.api.",
|
|
||||||
"import generated.cmvr.api."
|
|
||||||
)
|
|
||||||
|
|
||||||
with open(file_path, 'w', encoding='utf-8') as f:
|
|
||||||
f.write(content)
|
|
||||||
|
|
||||||
print(f"Fixed imports in {file_path}")
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
generate_proto()
|
|
||||||
@ -1,6 +0,0 @@
|
|||||||
grpcio==1.60.0
|
|
||||||
grpcio-tools==1.60.0
|
|
||||||
protobuf==4.25.3
|
|
||||||
Pillow==10.1.0
|
|
||||||
numpy==1.24.4
|
|
||||||
opencv-python==4.9.0.80
|
|
||||||
Loading…
Reference in New Issue
Block a user