lots of work on tools

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")

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):
+    def set_target(self, lat=34.068415, long=-118.443217):
-        # TODO: parse arguments somehow
         cmd = messages.GroundCommand()
         cmd.type = messages.CMD_SET_TARGET
-        cmd.target_location.latitude = 34.068415
+        cmd.target_location.latitude = lat
-        cmd.target_location.longitude = -118.443217
+        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
+        dict2pb(config, cmd.config)
-        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)
+        log.info("updated config")
 
-    def drive_heading(self):
+    def drive_heading(self, heading=65, power=0.0):
-        # TODO: parse arguments somehow
         cmd = messages.GroundCommand()
         cmd.type = messages.CMD_DRIVE_HEADING
-        cmd.drive_heading.heading = -115.0 - 180
+        cmd.drive_heading.heading = float(heading)
-        cmd.drive_heading.power = 0.3
+        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()