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.
116 lines
3.5 KiB
116 lines
3.5 KiB
6 years ago
|
#!/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()
|