From b3e9dd75ca94a3c3897cbc86025d1568d91ef534 Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Thu, 16 May 2019 19:01:03 -0700 Subject: [PATCH] lots of work on tools --- tools/ugv.py | 5 +++ tools/ugv_cmd.py | 82 ++++++++++++++++++++++++++++++++++-------------- 2 files changed, 64 insertions(+), 23 deletions(-) diff --git a/tools/ugv.py b/tools/ugv.py index 5a73c83..43b3b92 100755 --- a/tools/ugv.py +++ b/tools/ugv.py @@ -25,6 +25,7 @@ class UGVComms(E32): self.ack_cv = threading.Condition() self.next_command_id = 1 self.last_status = None + self.last_status_time = None self.rx_thread = None self.is_running = False @@ -89,11 +90,15 @@ class UGVComms(E32): self.ack_cv.notify() elif msg.HasField("status"): self.last_status = msg.status + self.last_status_time = time.time() + else: + log.warn("unknown UGV message: %s", msg) def start(self): if self.is_running: log.warning("RX thread already running") return False + self.is_running = True self.rx_thread = Thread(target=self.__rx_thread_entry, daemon=True) self.rx_thread.start() log.debug("started RX thread") diff --git a/tools/ugv_cmd.py b/tools/ugv_cmd.py index becf95f..19c7144 100755 --- a/tools/ugv_cmd.py +++ b/tools/ugv_cmd.py @@ -5,12 +5,23 @@ import serial import time import logging import readline +import yaml from ugv import UGVComms import messages_pb2 as messages +import config_pb2 log = logging.getLogger("ugv_cmd") +def dict2pb(d, pb): + for key in d: + val = d[key] + if isinstance(val, dict): + dict2pb(val, getattr(pb, key)) + else: + setattr(pb, key, val) + + class UGV_CLI: def __init__(self): self.is_running = False @@ -20,7 +31,16 @@ class UGV_CLI: 'h': self.help_msg, '?': self.help_msg, 'exit': self.exit, - 'q': self.exit + 'q': self.exit, + 'disable': self.disable, + 'd': self.disable, + 'target': self.set_target, + 'config': self.set_config, + 'c': self.set_config, + 'drive_heading': self.drive_heading, + 'drive_to_target': self.drive_to_target, + 'get_status': self.get_status, + 's': self.get_status, } pass @@ -37,45 +57,58 @@ exit, q, C-c, C-d: Quit the program def disable(self): self.ugv.write_command(messages.CMD_DISABLE) - def set_target(self): - # TODO: parse arguments somehow + def set_target(self, lat=34.068415, long=-118.443217): cmd = messages.GroundCommand() cmd.type = messages.CMD_SET_TARGET - cmd.target_location.latitude = 34.068415 - cmd.target_location.longitude = -118.443217 + cmd.target_location.latitude = lat + cmd.target_location.longitude = long self.ugv.write_command(cmd) + log.info("set target to (%d, %d)", lat, long) def set_config(self): - # TODO: read from config.yml + with open('./tools/config.yml', 'r') as configfile: + config = yaml.load(configfile) + + if 'REVISION' in config: + config_rev = config['REVISION'] + del config['REVISION'] + else: + config_rev = 1 + 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 + dict2pb(config, cmd.config) self.ugv.write_command(cmd) + log.info("updated config") - def drive_heading(self): - # TODO: parse arguments somehow + def drive_heading(self, heading=65, power=0.0): cmd = messages.GroundCommand() cmd.type = messages.CMD_DRIVE_HEADING - cmd.drive_heading.heading = -115.0 - 180 - cmd.drive_heading.power = 0.3 + cmd.drive_heading.heading = float(heading) + cmd.drive_heading.power = float(power) self.ugv.write_command(cmd) - time.sleep(2.0) + log.info("driving heading %d at power %d", heading, power) + + def drive_to_target(self): + cmd = messages.GroundCommand() + cmd.type = messages.CMD_DRIVE_TO_TARGET + self.ugv.write_command(cmd) + log.info("driving to target") + + def get_status(self): + if self.ugv.last_status_time is None: + log.info("no status received") + else: + last_status_delay = time.time() - self.ugv.last_status_time + log.info("last status (%.4f seconds ago): %s", last_status_delay, self.ugv.last_status) 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_url = "hwgrep://USB1" ser = serial.serial_for_url(ser_url, baudrate=9600, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS, timeout=0.5) @@ -93,7 +126,10 @@ exit, q, C-c, C-d: Quit the program except KeyError: print("Unknown command: '%s'" % line_parts[0]) continue - cmd() + try: + cmd(*line_parts[1:]) + except Exception as e: + print("Error executing command: ", e) # 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: @@ -111,5 +147,5 @@ exit, q, C-c, C-d: Quit the program 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) + logging.getLogger().setLevel(logging.INFO) UGV_CLI().start()