From 51756492812102e67b58d0fb2c4e64a72a60d843 Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Thu, 25 Apr 2019 19:44:44 -0700 Subject: [PATCH] improve ugv script --- e32_client/messages_pb2.py | 43 ++++++++++------- e32_client/ugv.py | 96 ++++++++++++++++++++++++++++---------- 2 files changed, 99 insertions(+), 40 deletions(-) diff --git a/e32_client/messages_pb2.py b/e32_client/messages_pb2.py index 4c808a5..de942f6 100644 --- a/e32_client/messages_pb2.py +++ b/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='CMD_DRIVE_TO_TARGET', index=1, number=1, serialized_options=None, type=None), _descriptor.EnumValueDescriptor( - name='ENABLE', index=1, number=1, + name='CMD_TEST', index=2, number=2, serialized_options=None, type=None), ], containing_type=None, serialized_options=None, - serialized_start=560, - serialized_end=604, + serialized_start=600, + serialized_end=675, ) _sym_db.RegisterEnumDescriptor(_GROUNDCOMMANDTYPE) GroundCommandType = enum_type_wrapper.EnumTypeWrapper(_GROUNDCOMMANDTYPE) -IDLE = 0 -AQUIRING = 1 -DRIVING = 2 -FINISHED = 3 -DISABLE = 0 -ENABLE = 1 +STATE_IDLE = 0 +STATE_AQUIRING = 1 +STATE_DRIVING = 2 +STATE_FINISHED = 3 +STATE_TEST = 4 +CMD_DISABLE = 0 +CMD_DRIVE_TO_TARGET = 1 +CMD_TEST = 2 diff --git a/e32_client/ugv.py b/e32_client/ugv.py index 94e916f..7cd9907 100755 --- a/e32_client/ugv.py +++ b/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): - while ugv.ser.is_open: - 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 stop(self): + self.rx_thread.join() + 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__": - ser = serial.serial_for_url("/dev/ttyUSB1", baudrate=9600, parity=serial.PARITY_NONE, +def main(): + 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) - rx_thread.start() - # print("resetting") - # ugv.reset() - cmd_id = 1 + ugv.start() time.sleep(0.2) try: while True: - gmsg = messages.GroundMessage() - gmsg.command.id = cmd_id - gmsg.command.type = messages.DISABLE - cmd_id += 1 - print("writing message: ", gmsg) - ugv.write_message(gmsg) + if ugv.last_status is None or ugv.last_status.state is not messages.STATE_TEST: + ugv.write_command(messages.CMD_TEST) 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()