update robot grpc
This commit is contained in:
parent
7d4e87f25e
commit
882a9d9133
557
biohead.py
Normal file
557
biohead.py
Normal file
@ -0,0 +1,557 @@
|
|||||||
|
#!/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
Normal file
344
biohead_test.py
Normal file
@ -0,0 +1,344 @@
|
|||||||
|
#!/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 +1,15 @@
|
|||||||
from .client import CMVRGrpcClient
|
from .client import CMVRGrpcClient
|
||||||
from .enums import CMVRErrorCode, DeviceState, FingerType
|
from .enums import CMVRErrorCode, DeviceState, FingerType, RobotCartesian, RobotJointIndexDirection
|
||||||
from .models import (
|
from .models import (
|
||||||
MicState, SpeakerState, CameraState,
|
MicState, SpeakerState, CameraState,
|
||||||
FreedomState, DexHandState, FacialExpressionState,
|
FreedomState, DexHandState, FacialExpressionState,
|
||||||
FingerTactileData, PalmTactileData, HandTactileSensors
|
FingerTactileData, PalmTactileData, HandTactileSensors,JointCmd,Pose3D,JointState
|
||||||
)
|
)
|
||||||
|
|
||||||
__all__ = [
|
__all__ = [
|
||||||
'CMVRGrpcClient',
|
'CMVRGrpcClient',
|
||||||
'CMVRErrorCode', 'DeviceState', 'FingerType',
|
'CMVRErrorCode', 'DeviceState', 'FingerType',"RobotCartesian", "RobotJointIndexDirection",
|
||||||
'MicState', 'SpeakerState', 'CameraState',
|
'MicState', 'SpeakerState', 'CameraState',
|
||||||
'FreedomState', 'DexHandState', 'FacialExpressionState',
|
'FreedomState', 'DexHandState', 'FacialExpressionState',
|
||||||
'FingerTactileData', 'PalmTactileData', 'HandTactileSensors'
|
'FingerTactileData', 'PalmTactileData', 'HandTactileSensors',"JointCmd","Pose3D","JointState"
|
||||||
]
|
]
|
||||||
@ -147,6 +147,213 @@ class CameraClient:
|
|||||||
print(f"获取图像失败: {e}")
|
print(f"获取图像失败: {e}")
|
||||||
return CMVRErrorCode.CMVR_RPC_FAILED, np.array([]), 0, 0
|
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:
|
def start_record(self, video_path: str) -> CMVRErrorCode:
|
||||||
"""开始录像"""
|
"""开始录像"""
|
||||||
try:
|
try:
|
||||||
|
|||||||
@ -7,6 +7,7 @@ from .camera_client import CameraClient
|
|||||||
from .dexhand_client import DexHandClient
|
from .dexhand_client import DexHandClient
|
||||||
from .micphone_client import MicphoneClient
|
from .micphone_client import MicphoneClient
|
||||||
from .speaker_client import SpeakerClient
|
from .speaker_client import SpeakerClient
|
||||||
|
from .humanoid_robot import HumanoidRobotClient
|
||||||
class CMVRGrpcClient:
|
class CMVRGrpcClient:
|
||||||
"""CMVR gRPC 主客户端"""
|
"""CMVR gRPC 主客户端"""
|
||||||
|
|
||||||
@ -15,7 +16,13 @@ class CMVRGrpcClient:
|
|||||||
self.connected = False
|
self.connected = False
|
||||||
|
|
||||||
# 创建 gRPC 通道
|
# 创建 gRPC 通道
|
||||||
self.channel = grpc.insecure_channel(server_address)
|
# 设置通道选项，增加最大接收和发送消息大小
|
||||||
|
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.generated = None # 推迟导入
|
||||||
@ -34,6 +41,9 @@ class CMVRGrpcClient:
|
|||||||
self.speaker_stub = None
|
self.speaker_stub = None
|
||||||
self.speaker_map: Dict[str, SpeakerClient] = {}
|
self.speaker_map: Dict[str, SpeakerClient] = {}
|
||||||
|
|
||||||
|
self.humanoid_robot_stub = None
|
||||||
|
self.humanoid_robot_map: Dict[str, HumanoidRobotClient] = {}
|
||||||
|
|
||||||
# 检查连接状态
|
# 检查连接状态
|
||||||
try:
|
try:
|
||||||
grpc.channel_ready_future(self.channel).result(timeout=5)
|
grpc.channel_ready_future(self.channel).result(timeout=5)
|
||||||
@ -49,6 +59,8 @@ class CMVRGrpcClient:
|
|||||||
|
|
||||||
self.micphone_stub = self.generated.microphone_service_pb2_grpc.MicPhoneServiceStub(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:
|
except grpc.FutureTimeoutError:
|
||||||
print(f"连接服务器超时: {server_address}")
|
print(f"连接服务器超时: {server_address}")
|
||||||
self.connected = False
|
self.connected = False
|
||||||
@ -69,6 +81,9 @@ class CMVRGrpcClient:
|
|||||||
from generated.cmvr.api import microphone_command_pb2
|
from generated.cmvr.api import microphone_command_pb2
|
||||||
from generated.cmvr.api import microphone_service_pb2_grpc
|
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', (), {
|
self.generated = type('GeneratedModules', (), {
|
||||||
'biohead_service_pb2_grpc': biohead_service_pb2_grpc,
|
'biohead_service_pb2_grpc': biohead_service_pb2_grpc,
|
||||||
'common_pb2': common_pb2,
|
'common_pb2': common_pb2,
|
||||||
@ -80,7 +95,9 @@ class CMVRGrpcClient:
|
|||||||
'speaker_service_pb2_grpc': speaker_service_pb2_grpc,
|
'speaker_service_pb2_grpc': speaker_service_pb2_grpc,
|
||||||
'speaker_command_pb2': speaker_command_pb2,
|
'speaker_command_pb2': speaker_command_pb2,
|
||||||
'microphone_service_pb2_grpc': microphone_service_pb2_grpc,
|
'microphone_service_pb2_grpc': microphone_service_pb2_grpc,
|
||||||
'microphone_command_pb2': microphone_command_pb2
|
'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:
|
except ImportError as e:
|
||||||
@ -118,6 +135,11 @@ class CMVRGrpcClient:
|
|||||||
if device_id not in self.micphone_map:
|
if device_id not in self.micphone_map:
|
||||||
self.micphone_map[device_id] = MicphoneClient(device_id, self.micphone_stub)
|
self.micphone_map[device_id] = MicphoneClient(device_id, self.micphone_stub)
|
||||||
return self.micphone_map[device_id]
|
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):
|
def close(self):
|
||||||
"""关闭所有连接"""
|
"""关闭所有连接"""
|
||||||
|
|||||||
@ -25,4 +25,28 @@ class FingerType(Enum):
|
|||||||
MIDDLE = 2 # 中指
|
MIDDLE = 2 # 中指
|
||||||
INDEX = 3 # 食指
|
INDEX = 3 # 食指
|
||||||
THUMB = 4 # 大拇指
|
THUMB = 4 # 大拇指
|
||||||
PALM = 5 # 掌心
|
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轴旋转
|
||||||
279
cmvr/humanoid_robot.py
Normal file
279
cmvr/humanoid_robot.py
Normal file
@ -0,0 +1,279 @@
|
|||||||
|
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, []
|
||||||
@ -213,4 +213,65 @@ class HandTactileSensors:
|
|||||||
print(f" 食指指端: {self.index_tip.rows}x{self.index_tip.cols} (数据大小: {self.index_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_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.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)")
|
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
|
||||||
|
}
|
||||||
|
|||||||
870
example_usage.py
Normal file
870
example_usage.py
Normal file
@ -0,0 +1,870 @@
|
|||||||
|
#!/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 = 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()
|
||||||
@ -14,7 +14,7 @@ sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
|
|||||||
|
|
||||||
# 直接导入生成的模块
|
# 直接导入生成的模块
|
||||||
from generated.cmvr.api import biohead_command_pb2
|
from generated.cmvr.api import biohead_command_pb2
|
||||||
from cmvr import CMVRGrpcClient, CMVRErrorCode, FacialExpressionState
|
from cmvr import CMVRGrpcClient, CMVRErrorCode, FacialExpressionState,JointCmd, Pose3D,RobotCartesian, RobotJointIndexDirection
|
||||||
|
|
||||||
def test_biohead():
|
def test_biohead():
|
||||||
"""测试仿生头功能"""
|
"""测试仿生头功能"""
|
||||||
@ -658,11 +658,183 @@ def test_microphone():
|
|||||||
else:
|
else:
|
||||||
print(f"开始录音失败，错误码: {result}")
|
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__":
|
if __name__ == "__main__":
|
||||||
# test_biohead()
|
# test_biohead()
|
||||||
test_camera()
|
# test_camera()
|
||||||
# test_rgb_image_stream()
|
# test_rgb_image_stream()
|
||||||
# test_dexhand()
|
# test_dexhand()
|
||||||
# test_sensor_data_stream()
|
# test_sensor_data_stream()
|
||||||
# test_speaker()
|
# test_speaker()
|
||||||
# test_microphone()
|
# test_microphone()
|
||||||
|
test_humanoid_robot()
|
||||||
|
|||||||
74
generate_proto.py
Normal file
74
generate_proto.py
Normal file
@ -0,0 +1,74 @@
|
|||||||
|
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()
|
||||||
59
get_joint_state.py
Normal file
59
get_joint_state.py
Normal file
@ -0,0 +1,59 @@
|
|||||||
|
#!/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()
|
||||||
@ -40,4 +40,4 @@ message ConfigParam {
|
|||||||
bool bool_value = 5; // 布尔类型
|
bool bool_value = 5; // 布尔类型
|
||||||
bytes bytes_value = 6; // 二进制数据类型
|
bytes bytes_value = 6; // 二进制数据类型
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
24
protos/cmvr/api/hlc_command.proto
Normal file
24
protos/cmvr/api/hlc_command.proto
Normal file
@ -0,0 +1,24 @@
|
|||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
9
protos/cmvr/api/hlc_service.proto
Normal file
9
protos/cmvr/api/hlc_service.proto
Normal file
@ -0,0 +1,9 @@
|
|||||||
|
syntax = "proto3";
|
||||||
|
|
||||||
|
import "cmvr/api/hlc_command.proto";
|
||||||
|
|
||||||
|
package cmvr.api;
|
||||||
|
|
||||||
|
service HlcService{
|
||||||
|
rpc touch(Touch.Request) returns (Touch.Response);
|
||||||
|
}
|
||||||
122
protos/cmvr/api/humanoid_robot_command.proto
Normal file
122
protos/cmvr/api/humanoid_robot_command.proto
Normal file
@ -0,0 +1,122 @@
|
|||||||
|
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
16
protos/cmvr/api/humanoid_robot_service.proto
Normal file
16
protos/cmvr/api/humanoid_robot_service.proto
Normal file
@ -0,0 +1,16 @@
|
|||||||
|
|
||||||
|
|
||||||
|
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);
|
||||||
|
}
|
||||||
@ -50,10 +50,12 @@ message GetSystemStatusCommand {
|
|||||||
}
|
}
|
||||||
|
|
||||||
message UpdateParamsCommand {
|
message UpdateParamsCommand {
|
||||||
message Request {}
|
message Request {
|
||||||
|
CommandHeader.Request header = 1;
|
||||||
|
repeated ConfigParam params = 2;
|
||||||
|
}
|
||||||
|
|
||||||
message Feedback {
|
message Feedback {
|
||||||
CommandHeader.Feedback header = 1;
|
CommandHeader.Feedback header = 1;
|
||||||
repeated ConfigParam params = 2;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -10,5 +10,4 @@ service SystemService {
|
|||||||
rpc GetSystemStatus(GetSystemStatusCommand.Request) returns (GetSystemStatusCommand.Feedback) {}
|
rpc GetSystemStatus(GetSystemStatusCommand.Request) returns (GetSystemStatusCommand.Feedback) {}
|
||||||
|
|
||||||
rpc UpdateParams(UpdateParamsCommand.Request) returns (UpdateParamsCommand.Feedback) {}
|
rpc UpdateParams(UpdateParamsCommand.Request) returns (UpdateParamsCommand.Feedback) {}
|
||||||
|
|
||||||
}
|
}
|
||||||
Loading…
Reference in New Issue
Block a user