From f475a5b962c11715eac3395b2f07b4d0823e7ecb Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Sat, 9 Feb 2019 19:22:37 -0800 Subject: [PATCH] lots more work on python ugv client --- e32_client/e32.py | 243 ++++++++++++++----------- e32_client/messages_pb2.py | 351 +++++++++++++++++++++++++++++++++++++ e32_client/ugv.py | 66 +++++++ 3 files changed, 554 insertions(+), 106 deletions(-) mode change 100644 => 100755 e32_client/e32.py create mode 100644 e32_client/messages_pb2.py create mode 100755 e32_client/ugv.py diff --git a/e32_client/e32.py b/e32_client/e32.py old mode 100644 new mode 100755 index 6740079..bb90bbb --- a/e32_client/e32.py +++ b/e32_client/e32.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import serial import struct @@ -6,25 +8,25 @@ PARITY_ODD = 1 PARITY_EVEN = 2 BAUD_TABLE = { - 0: 1200, - 1: 2400, - 2: 4800, - 3: 9600, - 4: 19200, - 5: 38400, - 6: 57600, - 7: 115200, + 0: 1200, + 1: 2400, + 2: 4800, + 3: 9600, + 4: 19200, + 5: 38400, + 6: 57600, + 7: 115200, } AIR_DATA_RATE_TABLE = { - 0: 300, - 1: 1200, - 2: 2400, - 3: 4800, - 4: 9600, - 5: 19200, - 6: 19200, - 7: 19200, + 0: 300, + 1: 1200, + 2: 2400, + 3: 4800, + 4: 9600, + 5: 19200, + 6: 19200, + 7: 19200, } TX_MODE_TRANSPARENT = 0 @@ -34,103 +36,132 @@ IO_MODE_OPEN_COLLECTOR = 0 IO_MODE_PUSH_PULL = 1 TX_POWER_TABLE = { - 0: 30, - 1: 27, - 2: 24, - 3: 21, + 0: 30, + 1: 27, + 2: 24, + 3: 21, } -def least_gte(value, dic): - items = sorted(dic.items(), key = lambda item: item[1]) - last_k = None - for k, v in items: - last_k = k - if v >= value: - break - return last_k + +def __least_gte(value, dic): + items = sorted(dic.items(), key=lambda item: item[1]) + last_k = None + for k, v in items: + last_k = k + if v >= value: + break + return last_k + class E32_Params: - save: bool - address: int - parity: int - baud: int - air_data_rate: int - channel: int - tx_mode: int - io_mode: int - wake_up_time: int - fec_enabled: bool - tx_power: int - - def default(): - p = E32_Params() - p.save = True - p.address = 0 - p.parity = PARITY_NONE - p.baud = 9600 - p.air_data_rate = 2400 - p.channel = 0x17 - p.tx_mode = TX_MODE_TRANSPARENT - p.io_mode = IO_MODE_PUSH_PULL - p.wake_up_time = 250 - p.fec_enabled = True - p.tx_power = 30 - return p - - def unpack(data): - p = E32_Params() - datab = bytes(data) - if datab[0] == 0xC0: - p.save = True - elif datab[0] == 0xC2: - p.save = False - else: - raise Exception('invalid E32_Params data') - p.address = (datab[1] << 8) | datab[2] - p.parity = (datab[3] >> 6) & 0b11 - p.baud = BAUD_TABLE[(datab[3] >> 3) & 0b111] - p.air_data_rate = AIR_DATA_RATE_TABLE[datab[3] & 0b111] - p.channel = datab[4] - p.tx_mode = (datab[5] >> 7) & 1 - p.io_mode = (datab[5] >> 6) & 1 - p.wake_up_time = 250 * (((datab[5] >> 3) & 0b111) + 1) - p.fec_enabled = (datab[5] & (1 << 2)) != 0 - p.tx_power = TX_POWER_TABLE[datab[5] & 0b11] - return p - - def pack(self): - p = self - datab = bytearray(range(6)) - if p.save: - datab[0] = 0xC0 - else: - datab[0] = 0xC2 - datab[1] = (p.address >> 8) & 0xFF - datab[2] = (p.address) & 0xFF - datab[3] = 0 - datab[3] |= p.parity << 6 - datab[3] |= (least_gte(p.baud, BAUD_TABLE) << 3) - datab[3] |= (least_gte(p.air_data_rate, AIR_DATA_RATE_TABLE)) - datab[4] = p.channel - datab[5] = 0 - datab[5] |= p.tx_mode << 7 - datab[5] |= p.io_mode << 6 - datab[5] |= (int((p.wake_up_time / 250) - 1) & 0b111) << 3 - datab[5] |= p.fec_enabled << 2 - datab[5] |= least_gte(p.tx_power, TX_POWER_TABLE) - return datab + save: bool + address: int + parity: int + baud: int + air_data_rate: int + channel: int + tx_mode: int + io_mode: int + wake_up_time: int + fec_enabled: bool + tx_power: int + + @staticmethod + def default(): + p = E32_Params() + p.save = True + p.address = 0 + p.parity = PARITY_NONE + p.baud = 9600 + p.air_data_rate = 2400 + p.channel = 0x17 + p.tx_mode = TX_MODE_TRANSPARENT + p.io_mode = IO_MODE_PUSH_PULL + p.wake_up_time = 250 + p.fec_enabled = True + p.tx_power = 30 + return p + + @staticmethod + def unpack(data): + p = E32_Params() + datab = bytes(data) + if len(datab) != 6: + raise Exception('invalid E32_Params data length') + if datab[0] == 0xC0: + p.save = True + elif datab[0] == 0xC2: + p.save = False + else: + raise Exception('invalid E32_Params data header') + p.address = (datab[1] << 8) | datab[2] + p.parity = (datab[3] >> 6) & 0b11 + p.baud = BAUD_TABLE[(datab[3] >> 3) & 0b111] + p.air_data_rate = AIR_DATA_RATE_TABLE[datab[3] & 0b111] + p.channel = datab[4] + p.tx_mode = (datab[5] >> 7) & 1 + p.io_mode = (datab[5] >> 6) & 1 + p.wake_up_time = 250 * (((datab[5] >> 3) & 0b111) + 1) + p.fec_enabled = (datab[5] & (1 << 2)) != 0 + p.tx_power = TX_POWER_TABLE[datab[5] & 0b11] + return p + + def pack(self): + p = self + datab = bytearray(range(6)) + if p.save: + datab[0] = 0xC0 + else: + datab[0] = 0xC2 + datab[1] = (p.address >> 8) & 0xFF + datab[2] = (p.address) & 0xFF + datab[3] = 0 + datab[3] |= p.parity << 6 + datab[3] |= (__least_gte(p.baud, BAUD_TABLE) << 3) + datab[3] |= (__least_gte(p.air_data_rate, AIR_DATA_RATE_TABLE)) + datab[4] = p.channel + datab[5] = 0 + datab[5] |= p.tx_mode << 7 + datab[5] |= p.io_mode << 6 + datab[5] |= (int((p.wake_up_time / 250) - 1) & 0b111) << 3 + datab[5] |= p.fec_enabled << 2 + datab[5] |= __least_gte(p.tx_power, TX_POWER_TABLE) + return datab class E32: - ser: serial.Serial + ser: serial.Serial + + def __init__(self, serial_port: serial.Serial): + self.ser = serial_port + + def close(self): + self.ser.close() + + def read_version(self): + # self.ser.flush() + self.ser.write(b'\xC3\xC3\xC3') + version = self.ser.read(size=4) + print("version: ", version) + return version + + def reset(self): + # self.ser.flush() + print("writing: ", b'\xC4\xC4\xC4') + self.ser.write(b'0xC40xC40xC4') + + def read_params(self): + # self.ser.flush() + self.ser.write(b'\xC1\xC1\xC1') + param_bytes = self.ser.read(size=6) + print("param_bytes: ", param_bytes) + return E32_Params.unpack(param_bytes) - def __init__(self, serial_port: serial.Serial): - self.ser = serial_port if __name__ == "__main__": - p = E32_Params.default() - print("params: ", p.__dict__) - data = p.pack() - print("packed data: ", ', '.join(format(x, '02x') for x in data)) - p2 = E32_Params.unpack(data) - print("unpacked params: ", p2.__dict__) + p = E32_Params.default() + print("params: ", p.__dict__) + data = p.pack() + print("packed data: ", ', '.join(format(x, '02x') for x in data)) + p2 = E32_Params.unpack(data) + print("unpacked params: ", p2.__dict__) diff --git a/e32_client/messages_pb2.py b/e32_client/messages_pb2.py new file mode 100644 index 0000000..4c808a5 --- /dev/null +++ b/e32_client/messages_pb2.py @@ -0,0 +1,351 @@ +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: messages.proto + +import sys +_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) +from google.protobuf.internal import enum_type_wrapper +from google.protobuf import descriptor as _descriptor +from google.protobuf import message as _message +from google.protobuf import reflection as _reflection +from google.protobuf import symbol_database as _symbol_database +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + + + +DESCRIPTOR = _descriptor.FileDescriptor( + name='messages.proto', + 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') +) + +_UGV_STATE = _descriptor.EnumDescriptor( + name='UGV_State', + full_name='uas.ugv.messages.UGV_State', + filename=None, + file=DESCRIPTOR, + values=[ + _descriptor.EnumValueDescriptor( + name='IDLE', index=0, number=0, + serialized_options=None, + type=None), + _descriptor.EnumValueDescriptor( + name='AQUIRING', index=1, number=1, + serialized_options=None, + type=None), + _descriptor.EnumValueDescriptor( + name='DRIVING', index=2, number=2, + serialized_options=None, + type=None), + _descriptor.EnumValueDescriptor( + name='FINISHED', index=3, number=3, + serialized_options=None, + type=None), + ], + containing_type=None, + serialized_options=None, + serialized_start=496, + serialized_end=558, +) +_sym_db.RegisterEnumDescriptor(_UGV_STATE) + +UGV_State = enum_type_wrapper.EnumTypeWrapper(_UGV_STATE) +_GROUNDCOMMANDTYPE = _descriptor.EnumDescriptor( + name='GroundCommandType', + full_name='uas.ugv.messages.GroundCommandType', + filename=None, + file=DESCRIPTOR, + values=[ + _descriptor.EnumValueDescriptor( + name='DISABLE', index=0, number=0, + serialized_options=None, + type=None), + _descriptor.EnumValueDescriptor( + name='ENABLE', index=1, number=1, + serialized_options=None, + type=None), + ], + containing_type=None, + serialized_options=None, + serialized_start=560, + serialized_end=604, +) +_sym_db.RegisterEnumDescriptor(_GROUNDCOMMANDTYPE) + +GroundCommandType = enum_type_wrapper.EnumTypeWrapper(_GROUNDCOMMANDTYPE) +IDLE = 0 +AQUIRING = 1 +DRIVING = 2 +FINISHED = 3 +DISABLE = 0 +ENABLE = 1 + + + +_LOCATION = _descriptor.Descriptor( + name='Location', + full_name='uas.ugv.messages.Location', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='fix_quality', full_name='uas.ugv.messages.Location.fix_quality', index=0, + number=1, type=13, cpp_type=3, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='latitude', full_name='uas.ugv.messages.Location.latitude', index=1, + number=2, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='longitude', full_name='uas.ugv.messages.Location.longitude', index=2, + number=3, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='altitude', full_name='uas.ugv.messages.Location.altitude', index=3, + number=4, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=36, + serialized_end=122, +) + + +_UGV_STATUS = _descriptor.Descriptor( + name='UGV_Status', + full_name='uas.ugv.messages.UGV_Status', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='state', full_name='uas.ugv.messages.UGV_Status.state', index=0, + number=1, type=14, cpp_type=8, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='location', full_name='uas.ugv.messages.UGV_Status.location', index=1, + number=2, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=124, + serialized_end=226, +) + + +_UGV_MESSAGE = _descriptor.Descriptor( + name='UGV_Message', + full_name='uas.ugv.messages.UGV_Message', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='status', full_name='uas.ugv.messages.UGV_Message.status', index=0, + number=1, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='command_ack', full_name='uas.ugv.messages.UGV_Message.command_ack', index=1, + number=2, type=13, cpp_type=3, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + _descriptor.OneofDescriptor( + name='ugv_message', full_name='uas.ugv.messages.UGV_Message.ugv_message', + index=0, containing_type=None, fields=[]), + ], + serialized_start=228, + serialized_end=327, +) + + +_GROUNDCOMMAND = _descriptor.Descriptor( + name='GroundCommand', + full_name='uas.ugv.messages.GroundCommand', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='id', full_name='uas.ugv.messages.GroundCommand.id', index=0, + number=1, type=13, cpp_type=3, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='type', full_name='uas.ugv.messages.GroundCommand.type', index=1, + number=2, type=14, cpp_type=8, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=329, + serialized_end=407, +) + + +_GROUNDMESSAGE = _descriptor.Descriptor( + name='GroundMessage', + full_name='uas.ugv.messages.GroundMessage', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='command', full_name='uas.ugv.messages.GroundMessage.command', index=0, + number=1, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + _descriptor.OneofDescriptor( + name='ground_message', full_name='uas.ugv.messages.GroundMessage.ground_message', + index=0, containing_type=None, fields=[]), + ], + serialized_start=409, + serialized_end=494, +) + +_UGV_STATUS.fields_by_name['state'].enum_type = _UGV_STATE +_UGV_STATUS.fields_by_name['location'].message_type = _LOCATION +_UGV_MESSAGE.fields_by_name['status'].message_type = _UGV_STATUS +_UGV_MESSAGE.oneofs_by_name['ugv_message'].fields.append( + _UGV_MESSAGE.fields_by_name['status']) +_UGV_MESSAGE.fields_by_name['status'].containing_oneof = _UGV_MESSAGE.oneofs_by_name['ugv_message'] +_UGV_MESSAGE.oneofs_by_name['ugv_message'].fields.append( + _UGV_MESSAGE.fields_by_name['command_ack']) +_UGV_MESSAGE.fields_by_name['command_ack'].containing_oneof = _UGV_MESSAGE.oneofs_by_name['ugv_message'] +_GROUNDCOMMAND.fields_by_name['type'].enum_type = _GROUNDCOMMANDTYPE +_GROUNDMESSAGE.fields_by_name['command'].message_type = _GROUNDCOMMAND +_GROUNDMESSAGE.oneofs_by_name['ground_message'].fields.append( + _GROUNDMESSAGE.fields_by_name['command']) +_GROUNDMESSAGE.fields_by_name['command'].containing_oneof = _GROUNDMESSAGE.oneofs_by_name['ground_message'] +DESCRIPTOR.message_types_by_name['Location'] = _LOCATION +DESCRIPTOR.message_types_by_name['UGV_Status'] = _UGV_STATUS +DESCRIPTOR.message_types_by_name['UGV_Message'] = _UGV_MESSAGE +DESCRIPTOR.message_types_by_name['GroundCommand'] = _GROUNDCOMMAND +DESCRIPTOR.message_types_by_name['GroundMessage'] = _GROUNDMESSAGE +DESCRIPTOR.enum_types_by_name['UGV_State'] = _UGV_STATE +DESCRIPTOR.enum_types_by_name['GroundCommandType'] = _GROUNDCOMMANDTYPE +_sym_db.RegisterFileDescriptor(DESCRIPTOR) + +Location = _reflection.GeneratedProtocolMessageType('Location', (_message.Message,), dict( + DESCRIPTOR = _LOCATION, + __module__ = 'messages_pb2' + # @@protoc_insertion_point(class_scope:uas.ugv.messages.Location) + )) +_sym_db.RegisterMessage(Location) + +UGV_Status = _reflection.GeneratedProtocolMessageType('UGV_Status', (_message.Message,), dict( + DESCRIPTOR = _UGV_STATUS, + __module__ = 'messages_pb2' + # @@protoc_insertion_point(class_scope:uas.ugv.messages.UGV_Status) + )) +_sym_db.RegisterMessage(UGV_Status) + +UGV_Message = _reflection.GeneratedProtocolMessageType('UGV_Message', (_message.Message,), dict( + DESCRIPTOR = _UGV_MESSAGE, + __module__ = 'messages_pb2' + # @@protoc_insertion_point(class_scope:uas.ugv.messages.UGV_Message) + )) +_sym_db.RegisterMessage(UGV_Message) + +GroundCommand = _reflection.GeneratedProtocolMessageType('GroundCommand', (_message.Message,), dict( + DESCRIPTOR = _GROUNDCOMMAND, + __module__ = 'messages_pb2' + # @@protoc_insertion_point(class_scope:uas.ugv.messages.GroundCommand) + )) +_sym_db.RegisterMessage(GroundCommand) + +GroundMessage = _reflection.GeneratedProtocolMessageType('GroundMessage', (_message.Message,), dict( + DESCRIPTOR = _GROUNDMESSAGE, + __module__ = 'messages_pb2' + # @@protoc_insertion_point(class_scope:uas.ugv.messages.GroundMessage) + )) +_sym_db.RegisterMessage(GroundMessage) + + +DESCRIPTOR._options = None +# @@protoc_insertion_point(module_scope) diff --git a/e32_client/ugv.py b/e32_client/ugv.py new file mode 100755 index 0000000..8a2df2d --- /dev/null +++ b/e32_client/ugv.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 + +import serial +from threading import Thread +import time + +from e32 import E32 +import messages_pb2 as messages +from google.protobuf.message import Message + + +class UGVComms(E32): + def __init__(self, serial_port: serial.Serial): + E32.__init__(self, serial_port) + + 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_message(self, msg: Message): + data = msg.SerializeToString() + self.write_len_delimited(data) + + def read_message(self): + len_data = self.ser.read(size=1) + msg_len = int.from_bytes(len_data, byteorder='big') + data = self.ser.read(size=msg_len) + msg = messages.UGV_Message() + msg.ParseFromString(data) + return msg + + +def __rx_thread_entry(ugv: UGVComms): + while True: + try: + msg = ugv.read_message() + print("received UGV message: ", msg) + except Exception as e: + print("error reading message: ", e) + continue + + +if __name__ == "__main__": + ser = serial.serial_for_url("loop://", 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, )) + rx_thread.start() + # print("resetting") + # ugv.reset() + cmd_id = 1 + time.sleep(0.2) + 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) + time.sleep(2.) + + rx_thread.join() +