''' 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 # For keyboard input import tty import termios import sys from robot_control_cmd_lcmt import robot_control_cmd_lcmt from robot_control_response_lcmt import robot_control_response_lcmt def getch(): fd = sys.stdin.fileno() old_settings = termios.tcgetattr(fd) try: tty.setraw(sys.stdin.fileno()) ch = sys.stdin.read(1) finally: termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) return ch def send_movement_command(ctrl, msg_template, direction): msg = robot_control_cmd_lcmt() msg.mode = 11 # Walk mode msg.gait_id = 26 # Fast trot msg.duration = 1000 # ms msg.step_height = [0.06, 0.06] msg.life_count = msg_template.life_count + 1 if direction == 'forward': msg.vel_des = [1.0, 0.0, 0.0] elif direction == 'backward': msg.vel_des = [-1.0, 0.0, 0.0] elif direction == 'left': msg.vel_des = [0.0, 1.0, 0.0] elif direction == 'right': msg.vel_des = [0.0, -1.0, 0.0] ctrl.Send_cmd(msg) print(f"Sent command: {direction.capitalize()}") ctrl.Wait_finish(11, 26) def main(): Ctrl = Robot_Ctrl() Ctrl.run() msg = robot_control_cmd_lcmt() try: print("Recovery stand") msg.mode = 12 # Recovery stand msg.gait_id = 0 msg.life_count += 1 Ctrl.Send_cmd(msg) Ctrl.Wait_finish(12, 0) print("\nReady to accept commands:") print("Use W (Forward), S (Backward), A (Left), D (Right), Q (Quit)\n") while True: char = getch().lower() # char = 'w' if char == 'w': send_movement_command(Ctrl, msg, 'forward') elif char == 's': send_movement_command(Ctrl, msg, 'backward') elif char == 'a': send_movement_command(Ctrl, msg, 'left') elif char == 'd': send_movement_command(Ctrl, msg, 'right') elif char == 'q': print("\nExiting...") break else: print("Invalid key pressed:", char) 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 print("Wait_finish timeout.") return False def send_publish(self): while self.runing: self.send_lock.acquire() if self.delay_cnt > 20: # Heartbeat signal 10HZ 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() # Main function if __name__ == '__main__': main()