lots more work on python ugv client
This commit is contained in:
parent
24bce4311a
commit
f475a5b962
237
e32_client/e32.py
Normal file → Executable file
237
e32_client/e32.py
Normal file → Executable file
@ -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
|
||||
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
|
||||
@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
|
||||
|
||||
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
|
||||
@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
|
||||
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__)
|
||||
|
||||
351
e32_client/messages_pb2.py
Normal file
351
e32_client/messages_pb2.py
Normal file
@ -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)
|
||||
66
e32_client/ugv.py
Executable file
66
e32_client/ugv.py
Executable file
@ -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()
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user