From 771b98a667b42bba2985576efa73c5559f3330dc Mon Sep 17 00:00:00 2001
From: Alex Mikhalev <alexmikhalevalex@gmail.com>
Date: Thu, 9 May 2019 10:22:04 -0700
Subject: [PATCH] use base64 encoding in python script

---
 e32_client/ugv.py | 35 +++++++++++++++++++----------------
 1 file changed, 19 insertions(+), 16 deletions(-)

diff --git a/e32_client/ugv.py b/e32_client/ugv.py
index 97f67dc..39f153e 100755
--- a/e32_client/ugv.py
+++ b/e32_client/ugv.py
@@ -5,6 +5,8 @@ import serial
 import threading
 from threading import Thread
 import time
+import binascii
+from base64 import b64decode, b64encode
 
 from e32 import E32
 import messages_pb2 as messages
@@ -12,8 +14,8 @@ from google.protobuf.message import Message
 
 
 class UGVComms(E32):
-    MAX_WRITE_RETRY=5
-    RETRY_TIME=1.0
+    MAX_WRITE_RETRY = 5
+    RETRY_TIME = 1.0
 
     def __init__(self, serial_port: serial.Serial):
         E32.__init__(self, serial_port)
@@ -23,16 +25,15 @@ class UGVComms(E32):
         self.last_status = None
         self.rx_thread = None
 
-    def write_len_delimited(self, data: bytes):
-        len_data = (len(data)).to_bytes(
-            1, byteorder='big')  # TODO: check byte order
-        self.ser.write(len_data)
-        self.ser.write(data)
+    def write_base64(self, data: bytes):
+        encoded = b64encode(data)
+        self.ser.write(encoded)
+        self.ser.write(b'\n')
 
     def write_message(self, msg: Message):
         print("writing message: ", msg)
         data = msg.SerializeToString()
-        self.write_len_delimited(data)
+        self.write_base64(data)
 
     def write_command(self, cmd_type: messages.GroundCommandType, retry=True):
         cmdid = self.next_command_id
@@ -59,17 +60,17 @@ class UGVComms(E32):
                 self.ack_cv.wait(timeout=time_left)
 
     def read_message(self):
-        len_data = self.ser.read(size=1)
-        if len(len_data) != 1:
+        data = self.ser.read_until(terminator=b'\n')
+        if len(data) is 0:
             return None
-        msg_len = int.from_bytes(len_data, byteorder='big')
-        data = self.ser.read(size=msg_len)
-        if len(data) != msg_len:
+        try:
+            decoded = b64decode(data, validate=True)
+        except binascii.Error:
             print("read bad data: ", data)
             self.ser.flush()
             return None
         msg = messages.UGV_Message()
-        msg.ParseFromString(data)
+        msg.ParseFromString(decoded)
         return msg
 
     def process_message(self, msg: messages.UGV_Message):
@@ -82,7 +83,7 @@ class UGVComms(E32):
                 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()
@@ -102,6 +103,7 @@ class UGVComms(E32):
                 print("error reading message: ", e, file=sys.stderr)
                 continue
 
+
 def main():
     if len(sys.argv) >= 2:
         ser_url = sys.argv[1]
@@ -109,7 +111,7 @@ def main():
         ser_url = "hwgrep://"
     ser = serial.serial_for_url(ser_url, baudrate=9600, parity=serial.PARITY_NONE,
                                 stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS,
-                                timeout=1.0)
+                                timeout=0.5)
     ugv = UGVComms(ser)
     ugv.start()
     time.sleep(0.2)
@@ -126,5 +128,6 @@ def main():
         ugv.ser.close()
     ugv.stop()
 
+
 if __name__ == "__main__":
     main()