60 lines
2.1 KiB
Python
60 lines
2.1 KiB
Python
|
#!/usr/bin/env python3
|
||
|
|
import lcm
|
||
|
|
import time
|
||
|
|
|
||
|
|
from utils.robot_control_cmd_lcmt import robot_control_cmd_lcmt
|
||
|
|
from utils.robot_control_response_lcmt import robot_control_response_lcmt
|
||
|
|
|
||
|
|
class SimpleStandTest:
|
||
|
|
def __init__(self):
|
||
|
|
# LCM通信初始化
|
||
|
|
self.lc = lcm.LCM("udpm://239.255.76.67:7670?ttl=255")
|
||
|
|
self.cmd_msg = robot_control_cmd_lcmt()
|
||
|
|
|
||
|
|
# 订阅状态反馈
|
||
|
|
self.lc.subscribe("robot_control_response", self.handle_response)
|
||
|
|
|
||
|
|
def handle_response(self, channel, data):
|
||
|
|
"""处理状态反馈"""
|
||
|
|
msg = robot_control_response_lcmt().decode(data)
|
||
|
|
print(f"[状态反馈] 模式:{msg.mode} 进度:{msg.order_process_bar}% 电机状态:{msg.motor_error}")
|
||
|
|
|
||
|
|
def send_stand_command(self):
|
||
|
|
# 1. 退出当前模式
|
||
|
|
self.cmd_msg.mode = 0
|
||
|
|
self.lc.publish("robot_control_cmd", self.cmd_msg.encode())
|
||
|
|
time.sleep(1)
|
||
|
|
|
||
|
|
# 2. 进入阻尼模式（准备阶段）
|
||
|
|
self.cmd_msg.mode = 7
|
||
|
|
self.cmd_msg.pos_des = [0, 0, 0.1] # 先降到10cm高度
|
||
|
|
self.lc.publish("robot_control_cmd", self.cmd_msg.encode())
|
||
|
|
time.sleep(2) # 等待稳定
|
||
|
|
|
||
|
|
# 3. 切换到站立模式
|
||
|
|
self.cmd_msg.mode = 12
|
||
|
|
self.cmd_msg.pos_des = [0, 0, 0.3] # 目标高度30cm
|
||
|
|
self.cmd_msg.vel_des = [0, 0, 0] # 零速度
|
||
|
|
self.cmd_msg.rpy_des = [0, 0, 0] # 水平姿态
|
||
|
|
self.cmd_msg.step_height = [0.05, 0.05] # 抬腿高度5cm
|
||
|
|
self.lc.publish("robot_control_cmd", self.cmd_msg.encode())
|
||
|
|
|
||
|
|
def run(self, timeout=5):
|
||
|
|
"""运行测试"""
|
||
|
|
start_time = time.time()
|
||
|
|
while time.time() - start_time < timeout:
|
||
|
|
self.send_stand_command()
|
||
|
|
self.lc.handle() # 处理接收到的消息
|
||
|
|
time.sleep(0.1) # 控制发送频率
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
print("=== 机器狗站立测试 ===")
|
||
|
|
tester = SimpleStandTest()
|
||
|
|
|
||
|
|
try:
|
||
|
|
print("尝试站立...")
|
||
|
|
tester.run(timeout=10) # 持续运行10秒
|
||
|
|
except KeyboardInterrupt:
|
||
|
|
print("用户中断测试")
|
||
|
|
finally:
|
||
|
|
print("测试结束")
|