''' 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) self.guard_thread = Thread(target=self.guard) 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() def update_status(self): while True: print(self.buffer,'#####',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: ###is_lifted 含义 0 是正常, 1 被抬起 3 是高阻尼态 if self.buffer == 0 and (self.is_lifted == 3 or self.is_lifted == 1): ###正常运动到挂掉 self.motion_quit() print("killed", self.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',self.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.motion_thread.join() self.update_status_thread.join() self.guard_thread.join() # Main function if __name__ == '__main__': main()