improve ugv script

e32_client/messages_pb2.py

@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 # Generated by the protocol buffer compiler.  DO NOT EDIT!
 # source: messages.proto
 
@@ -20,7 +21,7 @@ DESCRIPTOR = _descriptor.FileDescriptor(
   package='uas.ugv.messages',
   syntax='proto3',
   serialized_options=_b('H\003'),
-  serialized_pb=_b('\n\x0emessages.proto\x12\x10uas.ugv.messages\"V\n\x08Location\x12\x13\n\x0b\x66ix_quality\x18\x01 \x01(\r\x12\x10\n\x08latitude\x18\x02 \x01(\x02\x12\x11\n\tlongitude\x18\x03 \x01(\x02\x12\x10\n\x08\x61ltitude\x18\x04 \x01(\x02\"f\n\nUGV_Status\x12*\n\x05state\x18\x01 \x01(\x0e\x32\x1b.uas.ugv.messages.UGV_State\x12,\n\x08location\x18\x02 \x01(\x0b\x32\x1a.uas.ugv.messages.Location\"c\n\x0bUGV_Message\x12.\n\x06status\x18\x01 \x01(\x0b\x32\x1c.uas.ugv.messages.UGV_StatusH\x00\x12\x15\n\x0b\x63ommand_ack\x18\x02 \x01(\rH\x00\x42\r\n\x0bugv_message\"N\n\rGroundCommand\x12\n\n\x02id\x18\x01 \x01(\r\x12\x31\n\x04type\x18\x02 \x01(\x0e\x32#.uas.ugv.messages.GroundCommandType\"U\n\rGroundMessage\x12\x32\n\x07\x63ommand\x18\x01 \x01(\x0b\x32\x1f.uas.ugv.messages.GroundCommandH\x00\x42\x10\n\x0eground_message*>\n\tUGV_State\x12\x08\n\x04IDLE\x10\x00\x12\x0c\n\x08\x41QUIRING\x10\x01\x12\x0b\n\x07\x44RIVING\x10\x02\x12\x0c\n\x08\x46INISHED\x10\x03*,\n\x11GroundCommandType\x12\x0b\n\x07\x44ISABLE\x10\x00\x12\n\n\x06\x45NABLE\x10\x01\x42\x02H\x03\x62\x06proto3')
+  serialized_pb=_b('\n\x0emessages.proto\x12\x10uas.ugv.messages\"V\n\x08Location\x12\x13\n\x0b\x66ix_quality\x18\x01 \x01(\r\x12\x10\n\x08latitude\x18\x02 \x01(\x02\x12\x11\n\tlongitude\x18\x03 \x01(\x02\x12\x10\n\x08\x61ltitude\x18\x04 \x01(\x02\"f\n\nUGV_Status\x12*\n\x05state\x18\x01 \x01(\x0e\x32\x1b.uas.ugv.messages.UGV_State\x12,\n\x08location\x18\x02 \x01(\x0b\x32\x1a.uas.ugv.messages.Location\"c\n\x0bUGV_Message\x12.\n\x06status\x18\x01 \x01(\x0b\x32\x1c.uas.ugv.messages.UGV_StatusH\x00\x12\x15\n\x0b\x63ommand_ack\x18\x02 \x01(\rH\x00\x42\r\n\x0bugv_message\"N\n\rGroundCommand\x12\n\n\x02id\x18\x01 \x01(\r\x12\x31\n\x04type\x18\x02 \x01(\x0e\x32#.uas.ugv.messages.GroundCommandType\"U\n\rGroundMessage\x12\x32\n\x07\x63ommand\x18\x01 \x01(\x0b\x32\x1f.uas.ugv.messages.GroundCommandH\x00\x42\x10\n\x0eground_message*f\n\tUGV_State\x12\x0e\n\nSTATE_IDLE\x10\x00\x12\x12\n\x0eSTATE_AQUIRING\x10\x01\x12\x11\n\rSTATE_DRIVING\x10\x02\x12\x12\n\x0eSTATE_FINISHED\x10\x03\x12\x0e\n\nSTATE_TEST\x10\x04*K\n\x11GroundCommandType\x12\x0f\n\x0b\x43MD_DISABLE\x10\x00\x12\x17\n\x13\x43MD_DRIVE_TO_TARGET\x10\x01\x12\x0c\n\x08\x43MD_TEST\x10\x02\x42\x02H\x03\x62\x06proto3')
 )
 
 _UGV_STATE = _descriptor.EnumDescriptor(
@@ -30,26 +31,30 @@ _UGV_STATE = _descriptor.EnumDescriptor(
   file=DESCRIPTOR,
   values=[
     _descriptor.EnumValueDescriptor(
-      name='IDLE', index=0, number=0,
+      name='STATE_IDLE', index=0, number=0,
       serialized_options=None,
       type=None),
     _descriptor.EnumValueDescriptor(
-      name='AQUIRING', index=1, number=1,
+      name='STATE_AQUIRING', index=1, number=1,
       serialized_options=None,
       type=None),
     _descriptor.EnumValueDescriptor(
-      name='DRIVING', index=2, number=2,
+      name='STATE_DRIVING', index=2, number=2,
       serialized_options=None,
       type=None),
     _descriptor.EnumValueDescriptor(
-      name='FINISHED', index=3, number=3,
+      name='STATE_FINISHED', index=3, number=3,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='STATE_TEST', index=4, number=4,
       serialized_options=None,
       type=None),
   ],
   containing_type=None,
   serialized_options=None,
   serialized_start=496,
-  serialized_end=558,
+  serialized_end=598,
 )
 _sym_db.RegisterEnumDescriptor(_UGV_STATE)
 
@@ -61,28 +66,34 @@ _GROUNDCOMMANDTYPE = _descriptor.EnumDescriptor(
   file=DESCRIPTOR,
   values=[
     _descriptor.EnumValueDescriptor(
-      name='DISABLE', index=0, number=0,
+      name='CMD_DISABLE', index=0, number=0,
       serialized_options=None,
       type=None),
     _descriptor.EnumValueDescriptor(
-      name='ENABLE', index=1, number=1,
+      name='CMD_DRIVE_TO_TARGET', index=1, number=1,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='CMD_TEST', index=2, number=2,
       serialized_options=None,
       type=None),
   ],
   containing_type=None,
   serialized_options=None,
-  serialized_start=560,
+  serialized_start=600,
-  serialized_end=604,
+  serialized_end=675,
 )
 _sym_db.RegisterEnumDescriptor(_GROUNDCOMMANDTYPE)
 
 GroundCommandType = enum_type_wrapper.EnumTypeWrapper(_GROUNDCOMMANDTYPE)
-IDLE = 0
+STATE_IDLE = 0
-AQUIRING = 1
+STATE_AQUIRING = 1
-DRIVING = 2
+STATE_DRIVING = 2
-FINISHED = 3
+STATE_FINISHED = 3
-DISABLE = 0
+STATE_TEST = 4
-ENABLE = 1
+CMD_DISABLE = 0
+CMD_DRIVE_TO_TARGET = 1
+CMD_TEST = 2
 
 
 

e32_client/ugv.py

@@ -1,6 +1,8 @@
 #!/usr/bin/env python3
 
+import sys
 import serial
+import threading
 from threading import Thread
 import time
 
@@ -10,8 +12,16 @@ from google.protobuf.message import Message
 
 
 class UGVComms(E32):
+    MAX_WRITE_RETRY=5
+    RETRY_TIME=1.0
+
     def __init__(self, serial_port: serial.Serial):
         E32.__init__(self, serial_port)
+        self.msg_acks = []
+        self.ack_cv = threading.Condition()
+        self.next_command_id = 1
+        self.last_status = None
+        self.rx_thread = None
 
     def write_len_delimited(self, data: bytes):
         len_data = (len(data)).to_bytes(
@@ -20,9 +30,34 @@ class UGVComms(E32):
         self.ser.write(data)
 
     def write_message(self, msg: Message):
+        print("writing message: ", msg)
         data = msg.SerializeToString()
         self.write_len_delimited(data)
 
+    def write_command(self, cmd_type: messages.GroundCommandType, retry=True):
+        cmdid = self.next_command_id
+        self.next_command_id += 1
+
+        gmsg = messages.GroundMessage()
+        gmsg.command.id = cmdid
+        gmsg.command.type = cmd_type
+        self.write_message(gmsg)
+        last_write_time = time.time()
+        if not retry:
+            return
+        with self.ack_cv:
+            while True:
+                if cmdid in self.msg_acks:
+                    self.msg_acks.remove(cmdid)
+                    print("received ack for command")
+                    return
+                time_left = time.time() - last_write_time
+                if time_left >= self.RETRY_TIME:
+                    print("retry writing command")
+                    self.write_message(gmsg)
+                    last_write_time = time.time()
+                self.ack_cv.wait(timeout=time_left)
+
     def read_message(self):
         len_data = self.ser.read(size=1)
         if len(len_data) != 1:
@@ -37,42 +72,55 @@ class UGVComms(E32):
         msg.ParseFromString(data)
         return msg
 
+    def process_message(self, msg: messages.UGV_Message):
+        if msg is None:
+            return
+        print("received UGV message: ", msg)
+        if msg.HasField("command_ack"):
+            with self.ack_cv:
+                self.msg_acks.append(msg.command_ack)
+                self.ack_cv.notify()
+        elif msg.HasField("status"):
+            self.last_status = msg.status
+    
+    def start(self):
+        self.rx_thread = Thread(target=self.__rx_thread_entry, daemon=True)
+        self.rx_thread.start()
 
-def __rx_thread_entry(ugv: UGVComms):
+    def stop(self):
-    while ugv.ser.is_open:
+        self.rx_thread.join()
-        try:
-            msg = ugv.read_message()
-            if msg is not None:
-                print("received UGV message: ", msg)
-        except Exception as e:
-            print("error reading message: ", e)
-            continue
 
+    def __rx_thread_entry(self):
+        while self.ser.is_open:
+            try:
+                msg = self.read_message()
+                self.process_message(msg)
+            except serial.SerialException as e:
+                print("serial error: ", e, file=sys.stderr)
+                return
+            except Exception as e:
+                print("error reading message: ", e, file=sys.stderr)
+                continue
 
-if __name__ == "__main__":
+def main():
-    ser = serial.serial_for_url("/dev/ttyUSB1", baudrate=9600, parity=serial.PARITY_NONE,
+    ser = serial.serial_for_url("/dev/ttyUSB2", baudrate=9600, parity=serial.PARITY_NONE,
                                 stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS,
                                 timeout=2.0)
     ugv = UGVComms(ser)
-    rx_thread = Thread(target=__rx_thread_entry, args=(ugv, ), daemon=True)
+    ugv.start()
-    rx_thread.start()
-    # print("resetting")
-    # ugv.reset()
-    cmd_id = 1
     time.sleep(0.2)
     try:
         while True:
-            gmsg = messages.GroundMessage()
+            if ugv.last_status is None or ugv.last_status.state is not messages.STATE_TEST:
-            gmsg.command.id = cmd_id
+                ugv.write_command(messages.CMD_TEST)
-            gmsg.command.type = messages.DISABLE
-            cmd_id += 1
-            print("writing message: ", gmsg)
-            ugv.write_message(gmsg)
             time.sleep(2.)
     except KeyboardInterrupt:
+        ugv.write_command(messages.CMD_DISABLE)
         print("exiting...")
     finally:
+        ugv.ser.flush()
         ugv.ser.close()
+    ugv.stop()
 
-    rx_thread.join()
+if __name__ == "__main__":
-
+    main()