204 lines
5.9 KiB
Python
204 lines
5.9 KiB
Python
'''
|
||
This demo show the communication interface of MR813 motion control board based on Lcm.
|
||
Dependency:
|
||
- robot_control_cmd_lcmt.py
|
||
- robot_control_response_lcmt.py
|
||
'''
|
||
import lcm
|
||
import sys
|
||
import os
|
||
import time
|
||
import threading
|
||
from threading import Thread, Lock
|
||
|
||
from utils.robot_control_cmd_lcmt import robot_control_cmd_lcmt
|
||
from utils.robot_control_response_lcmt import robot_control_response_lcmt
|
||
|
||
def main():
|
||
Ctrl = Robot_Ctrl()
|
||
Ctrl.run()
|
||
msg = robot_control_cmd_lcmt()
|
||
|
||
try:
|
||
|
||
#起立
|
||
msg.mode = 12
|
||
msg.gait_id = 0
|
||
msg.life_count += 1 # Command will take effect when life_count update
|
||
Ctrl.Send_cmd(msg)
|
||
Ctrl.Wait_finish(12, 0)
|
||
|
||
|
||
|
||
# #向左转
|
||
msg.mode = 11 # 旋转1
|
||
msg.gait_id = 3 # 自变频Trot步态
|
||
msg.vel_des = [0.0, 0.0, 0.4] # 顺时针旋转（负Z轴速度）
|
||
msg.duration = 4200 # 执行9.5秒后自动停止
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
Ctrl.Send_cmd(msg)
|
||
time.sleep(10)
|
||
|
||
|
||
|
||
#直走
|
||
msg.mode = 11
|
||
msg.gait_id = 3
|
||
msg.vel_des = [0.3,0.0,0.0]
|
||
msg.duration = 2800
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
Ctrl.Send_cmd(msg)
|
||
time.sleep(10)
|
||
|
||
# #向左转
|
||
msg.mode = 11 # 旋转1
|
||
msg.gait_id = 3 # 自变频Trot步态
|
||
msg.vel_des = [0.0, 0.0, 0.4] # 顺时针旋转（负Z轴速度）
|
||
msg.duration = 4200 # 执行9.5秒后自动停止
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
Ctrl.Send_cmd(msg)
|
||
time.sleep(10)
|
||
|
||
|
||
# 倒车过石板路
|
||
msg.mode = 11
|
||
msg.gait_id = 3
|
||
msg.vel_des = [-0.3,0.0,0.0]
|
||
msg.duration = 20000
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
Ctrl.Send_cmd(msg)
|
||
time.sleep(22)
|
||
|
||
#旋转至正面
|
||
msg.mode = 11 # 旋转1
|
||
msg.gait_id = 3 # 自变频Trot步态
|
||
msg.vel_des = [0.0, 0.0, 0.4] # 顺时针旋转（负Z轴速度）
|
||
msg.duration = 8400 # 执行9.5秒后自动停止
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
Ctrl.Send_cmd(msg)
|
||
time.sleep(10)
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
# while Ctrl.mode_ok != 21: # 等待模式21（抬头）被确认完成
|
||
# time.sleep(0.01)
|
||
# camera_thread1 = MiroCameraThread()
|
||
# camera_thread1.start()
|
||
# time.sleep(10)
|
||
|
||
# #等待动作和拍照完成
|
||
# try:
|
||
# # 等待抬头动作完成（根据实际情况调整超时时间）
|
||
# # 等待相机线程完成
|
||
# camera_thread1.join(timeout=10)
|
||
# # 处理拍摄结果
|
||
# if camera_thread1.ros_initialized and camera_thread1.image is not None:
|
||
# # 保存或显示图像
|
||
# camera_thread1.camera.save_image(camera_thread1.image, "camera_image1.png")
|
||
# else:
|
||
# print("Camera Failed")
|
||
|
||
# except Exception as e:
|
||
# print(f"Fail: {e}")
|
||
|
||
|
||
|
||
except KeyboardInterrupt:
|
||
pass
|
||
Ctrl.quit()
|
||
sys.exit()
|
||
|
||
|
||
class Robot_Ctrl(object):
|
||
def __init__(self):
|
||
self.rec_thread = Thread(target=self.rec_responce)
|
||
self.send_thread = Thread(target=self.send_publish)
|
||
self.lc_r = lcm.LCM("udpm://239.255.76.67:7670?ttl=255")
|
||
self.lc_s = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
|
||
self.cmd_msg = robot_control_cmd_lcmt()
|
||
self.rec_msg = robot_control_response_lcmt()
|
||
self.send_lock = Lock()
|
||
self.delay_cnt = 0
|
||
self.mode_ok = 0
|
||
self.gait_ok = 0
|
||
self.runing = 1
|
||
|
||
def run(self):
|
||
self.lc_r.subscribe("robot_control_response", self.msg_handler)
|
||
self.send_thread.start()
|
||
self.rec_thread.start()
|
||
|
||
def msg_handler(self, channel, data):
|
||
self.rec_msg = robot_control_response_lcmt().decode(data)
|
||
if(self.rec_msg.order_process_bar >= 95):
|
||
self.mode_ok = self.rec_msg.mode
|
||
else:
|
||
self.mode_ok = 0
|
||
|
||
def rec_responce(self):
|
||
while self.runing:
|
||
self.lc_r.handle()
|
||
time.sleep( 0.002 )
|
||
|
||
def Wait_finish(self, mode, gait_id):
|
||
count = 0
|
||
while self.runing and count < 2000: #10s
|
||
if self.mode_ok == mode and self.gait_ok == gait_id:
|
||
return True
|
||
else:
|
||
time.sleep(0.005)
|
||
count += 1
|
||
|
||
def send_publish(self):
|
||
while self.runing:
|
||
self.send_lock.acquire()
|
||
if self.delay_cnt > 20: # Heartbeat signal 10HZ, It is used to maintain the heartbeat when life count is not updated
|
||
self.lc_s.publish("robot_control_cmd",self.cmd_msg.encode())
|
||
self.delay_cnt = 0
|
||
self.delay_cnt += 1
|
||
self.send_lock.release()
|
||
time.sleep( 0.005 )
|
||
|
||
def Send_cmd(self, msg):
|
||
self.send_lock.acquire()
|
||
self.delay_cnt = 50
|
||
self.cmd_msg = msg
|
||
self.send_lock.release()
|
||
|
||
def quit(self):
|
||
self.runing = 0
|
||
self.rec_thread.join()
|
||
self.send_thread.join()
|
||
|
||
class MiroCameraThread(threading.Thread):
|
||
def __init__(self):
|
||
super().__init__(daemon=True)
|
||
self.ros_initialized = False
|
||
self.camera = None
|
||
self.image = None
|
||
|
||
def run(self):
|
||
rclpy.init()
|
||
self.ros_initialized = True
|
||
self.camera = MiroCameraROS2()
|
||
self.image = self.camera.get_latest_image()
|
||
rclpy.shutdown()
|
||
|
||
|
||
# Main function
|
||
if __name__ == '__main__':
|
||
main()
|
||
|
||
|
||
#python3 src/cyberdog_simulator/cyberdog_gazebo/script/launchsim.py
|
||
|
||
#python3 src/cyberdog_simulator/cyberdog_gazebo/script/launchsim.py |