diff --git a/e32_client/ugv.py b/e32_client/ugv.py index 8a2df2d..b695762 100755 --- a/e32_client/ugv.py +++ b/e32_client/ugv.py @@ -25,8 +25,13 @@ class UGVComms(E32): def read_message(self): len_data = self.ser.read(size=1) + if len(len_data) != 1: + return None msg_len = int.from_bytes(len_data, byteorder='big') data = self.ser.read(size=msg_len) + if len(data) != msg_len: + self.ser.flush() + return None msg = messages.UGV_Message() msg.ParseFromString(data) return msg @@ -36,7 +41,8 @@ def __rx_thread_entry(ugv: UGVComms): while True: try: msg = ugv.read_message() - print("received UGV message: ", msg) + if msg is not None: + print("received UGV message: ", msg) except Exception as e: print("error reading message: ", e) continue @@ -47,7 +53,7 @@ if __name__ == "__main__": stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS, timeout=2.0) ugv = UGVComms(ser) - rx_thread = Thread(target=__rx_thread_entry, args=(ugv, )) + rx_thread = Thread(target=__rx_thread_entry, args=(ugv, ), daemon=True) rx_thread.start() # print("resetting") # ugv.reset() @@ -62,5 +68,6 @@ if __name__ == "__main__": ugv.write_message(gmsg) time.sleep(2.) + rx_thread.In rx_thread.join()