You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
115 lines
3.5 KiB
115 lines
3.5 KiB
#!/usr/bin/env python3 |
|
|
|
import sys |
|
import serial |
|
import time |
|
import logging |
|
import readline |
|
|
|
from ugv import UGVComms |
|
import messages_pb2 as messages |
|
|
|
log = logging.getLogger("ugv_cmd") |
|
|
|
class UGV_CLI: |
|
def __init__(self): |
|
self.is_running = False |
|
self.last_state = messages.STATE_IDLE |
|
self.commands = { |
|
'help': self.help_msg, |
|
'h': self.help_msg, |
|
'?': self.help_msg, |
|
'exit': self.exit, |
|
'q': self.exit |
|
} |
|
pass |
|
|
|
|
|
def help_msg(self): |
|
print("""Commands: |
|
help, h, ?: Print this help message |
|
exit, q, C-c, C-d: Quit the program |
|
""") |
|
|
|
def exit(self): |
|
self.is_running = False |
|
|
|
def disable(self): |
|
self.ugv.write_command(messages.CMD_DISABLE) |
|
|
|
def set_target(self): |
|
# TODO: parse arguments somehow |
|
cmd = messages.GroundCommand() |
|
cmd.type = messages.CMD_SET_TARGET |
|
cmd.target_location.latitude = 34.068415 |
|
cmd.target_location.longitude = -118.443217 |
|
self.ugv.write_command(cmd) |
|
|
|
def set_config(self): |
|
# TODO: read from config.yml |
|
cmd = messages.GroundCommand() |
|
cmd.type = messages.CMD_SET_CONFIG |
|
cmd.config.angle_pid.kp = 0.10 |
|
cmd.config.angle_pid.ki = 0 # .00005 |
|
cmd.config.angle_pid.kd = 0.4 |
|
cmd.config.angle_pid.max_output = 0.5 |
|
cmd.config.angle_pid.max_i_error = 15.0 |
|
cmd.config.min_target_dist = 10.0 |
|
cmd.config.min_flip_pitch = 90.0 |
|
self.ugv.write_command(cmd) |
|
|
|
def drive_heading(self): |
|
# TODO: parse arguments somehow |
|
cmd = messages.GroundCommand() |
|
cmd.type = messages.CMD_DRIVE_HEADING |
|
cmd.drive_heading.heading = -115.0 - 180 |
|
cmd.drive_heading.power = 0.3 |
|
self.ugv.write_command(cmd) |
|
time.sleep(2.0) |
|
|
|
def start(self): |
|
self.is_running = True |
|
readline.parse_and_bind('tab: complete') |
|
#readline.parse_and_bind('set editing-mode vi') |
|
|
|
if len(sys.argv) >= 2: |
|
ser_url = sys.argv[1] |
|
else: |
|
ser_url = "loop://" |
|
ser = serial.serial_for_url(ser_url, baudrate=9600, parity=serial.PARITY_NONE, |
|
stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS, |
|
timeout=0.5) |
|
self.ugv = UGVComms(ser) |
|
self.ugv.start() |
|
time.sleep(0.2) |
|
try: |
|
while self.is_running: |
|
line = input("UGV> ") |
|
line_parts = line.split(' ') |
|
if len(line_parts) is 0: |
|
continue |
|
try: |
|
cmd = self.commands[line_parts[0]] |
|
except KeyError: |
|
print("Unknown command: '%s'" % line_parts[0]) |
|
continue |
|
cmd() |
|
# TODO: continuously write state |
|
# while True: |
|
# if self.ugv.last_status is None or self.ugv.last_status.state is not messages.STATE_DRIVE_HEADING: |
|
except (KeyboardInterrupt, EOFError): |
|
self.exit() |
|
finally: |
|
log.info("disabling UGV...") |
|
try: |
|
self.ugv.write_command(messages.CMD_DISABLE) |
|
log.info("done. exiting") |
|
except KeyboardInterrupt: |
|
log.info("force exiting...") |
|
self.ugv.stop() |
|
|
|
|
|
if __name__ == "__main__": |
|
logging.basicConfig(format='%(asctime)s [%(name)s] %(levelname)s: %(message)s', datefmt='%Y-%b-%d %H:%M:%S') |
|
logging.getLogger().setLevel(logging.DEBUG) |
|
UGV_CLI().start()
|
|
|