60 lines
1.9 KiB
Python
60 lines
1.9 KiB
Python
#!/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()
|