222 lines
7.9 KiB
Python
222 lines
7.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
|
|
from threading import Thread, Lock
|
|
|
|
from robot_control_cmd_lcmt import robot_control_cmd_lcmt
|
|
from robot_control_response_lcmt import robot_control_response_lcmt
|
|
|
|
def main():
|
|
Ctrl = Robot_Ctrl()
|
|
Ctrl.run()
|
|
|
|
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.msg = robot_control_cmd_lcmt()
|
|
self.send_lock = Lock()
|
|
self.delay_cnt = 0
|
|
self.mode_ok = 0
|
|
self.gait_ok = 0
|
|
self.runing = 1
|
|
|
|
self.buffer = 0
|
|
self.status = 0
|
|
self.is_lifted = 0
|
|
|
|
self.stage = 0
|
|
|
|
self.motion_thread = Thread(target=self.motion,args=(self.stage,))
|
|
self.update_status_thread = Thread(target=self.update_status,args = ())
|
|
self.guard_thread = Thread(target=self.guard,args = ())
|
|
|
|
def motion(self,stage):
|
|
try:
|
|
test_speed = 0.1
|
|
test_duration = 5000
|
|
while stage == 0:
|
|
self.msg.mode = 12 # Recovery stand
|
|
self.msg.gait_id = 0
|
|
self.msg.life_count += 1 # Command will take effect when life_count update
|
|
self.Send_cmd(self.msg)
|
|
self.Wait_finish(12, 0)
|
|
stage += 1
|
|
self.stage += 1
|
|
print("up to", stage)
|
|
|
|
while stage == 1:###前
|
|
self.msg.mode = 11 # Recovery stand
|
|
self.msg.gait_id = 27
|
|
self.msg.vel_des = [test_speed, 0, 0] # forward left/rightmove rotate
|
|
self.msg.duration = test_duration
|
|
self.msg.life_count += 1 # Command will take effect when life_count update
|
|
self.Send_cmd(self.msg)
|
|
time.sleep(12)
|
|
stage += 1
|
|
self.stage += 1
|
|
print("up to", stage)
|
|
|
|
|
|
while stage == 2:###左
|
|
self.msg.mode = 11 # Recovery stand
|
|
self.msg.gait_id = 27
|
|
self.msg.vel_des = [0, test_speed, 0] # forward left/rightmove rotate
|
|
self.msg.duration = test_duration
|
|
self.msg.life_count += 1 # Command will take effect when life_count update
|
|
self.Send_cmd(self.msg)
|
|
time.sleep(12)
|
|
stage += 1
|
|
self.stage += 1
|
|
print("up to", stage)
|
|
|
|
while stage == 3: ####后
|
|
self.msg.mode = 11 # Recovery stand
|
|
self.msg.gait_id = 27
|
|
self.msg.vel_des = [-test_speed, 0, 0] # forward left/rightmove rotate
|
|
self.msg.duration = test_duration
|
|
self.msg.life_count += 1 # Command will take effect when life_count update
|
|
self.Send_cmd(self.msg)
|
|
time.sleep(12)
|
|
stage += 1
|
|
self.stage += 1
|
|
print("up to", stage)
|
|
|
|
while stage == 4: ###右
|
|
self.msg.mode = 11 # Recovery stand
|
|
self.msg.gait_id = 27
|
|
self.msg.vel_des = [0, -test_speed, 0] # forward left/rightmove rotate
|
|
self.msg.duration = test_duration
|
|
self.msg.life_count += 1 # Command will take effect when life_count update
|
|
self.Send_cmd(self.msg)
|
|
time.sleep(12)
|
|
stage += 1
|
|
self.stage += 1
|
|
print("up to", stage)
|
|
|
|
except KeyboardInterrupt:
|
|
pass
|
|
self.quit()
|
|
sys.exit()
|
|
|
|
def motion_quit(self):
|
|
# self.runing = 0 ###要不要锁死？
|
|
self.motion_thread.join()
|
|
|
|
def motion_restart(self,stage):
|
|
self.motion_thread = Thread(target=self.motion,args=(stage,))
|
|
# self.runing = 1 ###要不要锁死？
|
|
self.motion_thread.start()
|
|
print("restart", stage)
|
|
|
|
def update_status(self):
|
|
while True:
|
|
print(self.is_lifted)
|
|
self.buffer = self.is_lifted ###保存之前的值
|
|
if self.rec_msg.switch_status == 3: ##摔倒
|
|
self.is_lifted = 3
|
|
elif self.rec_msg.switch_status == 4: ###提起
|
|
self.is_lifted = 1 ###更新新值
|
|
else: #####正常运动0 或者 运动过程1
|
|
self.is_lifted = 0 ###更新新值
|
|
|
|
# print('status? ',self.rec_msg.switch_status)
|
|
time.sleep(0.08)
|
|
|
|
def guard(self):
|
|
while True:
|
|
# 含义 0 是正常， 1 被抬起 3 是高阻尼态
|
|
if self.buffer == 0 and (self.is_lifted == 3 or self.is_lifted == 1): ###正常运动到挂掉
|
|
self.motion_quit()
|
|
print("killed", stage)
|
|
time.sleep(0.1)
|
|
|
|
while self.is_lifted == 3:
|
|
self.msg.mode = 11 # 随便发个指令更新状态
|
|
self.msg.gait_id = 27
|
|
self.msg.vel_des = [0, 0, 0] # forward left/rightmove rotate
|
|
self.msg.duration = 100
|
|
self.msg.life_count += 1
|
|
self.Send_cmd(self.msg)
|
|
time.sleep(1)
|
|
|
|
while self.is_lifted == 1: ###提起之后随时监测是否落地
|
|
self.msg.mode = 12 # Recovery stand
|
|
self.msg.gait_id = 0
|
|
self.msg.life_count += 1 # Command will take effect when life_count update
|
|
self.Send_cmd(self.msg)
|
|
time.sleep(1)
|
|
print("monitor", stage)
|
|
|
|
self.Wait_finish(12,0)
|
|
self.motion_restart(self.stage)
|
|
time.sleep(0.1)
|
|
time.sleep(0.03)
|
|
|
|
def run(self):
|
|
self.lc_r.subscribe("robot_control_response", self.msg_handler)
|
|
self.send_thread.start()
|
|
self.rec_thread.start()
|
|
self.update_status_thread.start()
|
|
self.guard_thread.start()
|
|
self.motion_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()
|
|
self.update_status_thread.join()
|
|
self.guard_thread.join()
|
|
self.motion_thread.join()
|
|
|
|
# Main function
|
|
if __name__ == '__main__':
|
|
main() |