161 lines
4.4 KiB
Python
161 lines
4.4 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
|
|
|
|
# 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() |