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 serial
|
||||||
import struct
|
import struct
|
||||||
|
|
||||||
@ -6,25 +8,25 @@ PARITY_ODD = 1
|
|||||||
PARITY_EVEN = 2
|
PARITY_EVEN = 2
|
||||||
|
|
||||||
BAUD_TABLE = {
|
BAUD_TABLE = {
|
||||||
0: 1200,
|
0: 1200,
|
||||||
1: 2400,
|
1: 2400,
|
||||||
2: 4800,
|
2: 4800,
|
||||||
3: 9600,
|
3: 9600,
|
||||||
4: 19200,
|
4: 19200,
|
||||||
5: 38400,
|
5: 38400,
|
||||||
6: 57600,
|
6: 57600,
|
||||||
7: 115200,
|
7: 115200,
|
||||||
}
|
}
|
||||||
|
|
||||||
AIR_DATA_RATE_TABLE = {
|
AIR_DATA_RATE_TABLE = {
|
||||||
0: 300,
|
0: 300,
|
||||||
1: 1200,
|
1: 1200,
|
||||||
2: 2400,
|
2: 2400,
|
||||||
3: 4800,
|
3: 4800,
|
||||||
4: 9600,
|
4: 9600,
|
||||||
5: 19200,
|
5: 19200,
|
||||||
6: 19200,
|
6: 19200,
|
||||||
7: 19200,
|
7: 19200,
|
||||||
}
|
}
|
||||||
|
|
||||||
TX_MODE_TRANSPARENT = 0
|
TX_MODE_TRANSPARENT = 0
|
||||||
@ -34,103 +36,132 @@ IO_MODE_OPEN_COLLECTOR = 0
|
|||||||
IO_MODE_PUSH_PULL = 1
|
IO_MODE_PUSH_PULL = 1
|
||||||
|
|
||||||
TX_POWER_TABLE = {
|
TX_POWER_TABLE = {
|
||||||
0: 30,
|
0: 30,
|
||||||
1: 27,
|
1: 27,
|
||||||
2: 24,
|
2: 24,
|
||||||
3: 21,
|
3: 21,
|
||||||
}
|
}
|
||||||
|
|
||||||
def least_gte(value, dic):
|
|
||||||
items = sorted(dic.items(), key = lambda item: item[1])
|
def __least_gte(value, dic):
|
||||||
last_k = None
|
items = sorted(dic.items(), key=lambda item: item[1])
|
||||||
for k, v in items:
|
last_k = None
|
||||||
last_k = k
|
for k, v in items:
|
||||||
if v >= value:
|
last_k = k
|
||||||
break
|
if v >= value:
|
||||||
return last_k
|
break
|
||||||
|
return last_k
|
||||||
|
|
||||||
|
|
||||||
class E32_Params:
|
class E32_Params:
|
||||||
save: bool
|
save: bool
|
||||||
address: int
|
address: int
|
||||||
parity: int
|
parity: int
|
||||||
baud: int
|
baud: int
|
||||||
air_data_rate: int
|
air_data_rate: int
|
||||||
channel: int
|
channel: int
|
||||||
tx_mode: int
|
tx_mode: int
|
||||||
io_mode: int
|
io_mode: int
|
||||||
wake_up_time: int
|
wake_up_time: int
|
||||||
fec_enabled: bool
|
fec_enabled: bool
|
||||||
tx_power: int
|
tx_power: int
|
||||||
|
|
||||||
def default():
|
@staticmethod
|
||||||
p = E32_Params()
|
def default():
|
||||||
p.save = True
|
p = E32_Params()
|
||||||
p.address = 0
|
p.save = True
|
||||||
p.parity = PARITY_NONE
|
p.address = 0
|
||||||
p.baud = 9600
|
p.parity = PARITY_NONE
|
||||||
p.air_data_rate = 2400
|
p.baud = 9600
|
||||||
p.channel = 0x17
|
p.air_data_rate = 2400
|
||||||
p.tx_mode = TX_MODE_TRANSPARENT
|
p.channel = 0x17
|
||||||
p.io_mode = IO_MODE_PUSH_PULL
|
p.tx_mode = TX_MODE_TRANSPARENT
|
||||||
p.wake_up_time = 250
|
p.io_mode = IO_MODE_PUSH_PULL
|
||||||
p.fec_enabled = True
|
p.wake_up_time = 250
|
||||||
p.tx_power = 30
|
p.fec_enabled = True
|
||||||
return p
|
p.tx_power = 30
|
||||||
|
return p
|
||||||
|
|
||||||
def unpack(data):
|
@staticmethod
|
||||||
p = E32_Params()
|
def unpack(data):
|
||||||
datab = bytes(data)
|
p = E32_Params()
|
||||||
if datab[0] == 0xC0:
|
datab = bytes(data)
|
||||||
p.save = True
|
if len(datab) != 6:
|
||||||
elif datab[0] == 0xC2:
|
raise Exception('invalid E32_Params data length')
|
||||||
p.save = False
|
if datab[0] == 0xC0:
|
||||||
else:
|
p.save = True
|
||||||
raise Exception('invalid E32_Params data')
|
elif datab[0] == 0xC2:
|
||||||
p.address = (datab[1] << 8) | datab[2]
|
p.save = False
|
||||||
p.parity = (datab[3] >> 6) & 0b11
|
else:
|
||||||
p.baud = BAUD_TABLE[(datab[3] >> 3) & 0b111]
|
raise Exception('invalid E32_Params data header')
|
||||||
p.air_data_rate = AIR_DATA_RATE_TABLE[datab[3] & 0b111]
|
p.address = (datab[1] << 8) | datab[2]
|
||||||
p.channel = datab[4]
|
p.parity = (datab[3] >> 6) & 0b11
|
||||||
p.tx_mode = (datab[5] >> 7) & 1
|
p.baud = BAUD_TABLE[(datab[3] >> 3) & 0b111]
|
||||||
p.io_mode = (datab[5] >> 6) & 1
|
p.air_data_rate = AIR_DATA_RATE_TABLE[datab[3] & 0b111]
|
||||||
p.wake_up_time = 250 * (((datab[5] >> 3) & 0b111) + 1)
|
p.channel = datab[4]
|
||||||
p.fec_enabled = (datab[5] & (1 << 2)) != 0
|
p.tx_mode = (datab[5] >> 7) & 1
|
||||||
p.tx_power = TX_POWER_TABLE[datab[5] & 0b11]
|
p.io_mode = (datab[5] >> 6) & 1
|
||||||
return p
|
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):
|
def pack(self):
|
||||||
p = self
|
p = self
|
||||||
datab = bytearray(range(6))
|
datab = bytearray(range(6))
|
||||||
if p.save:
|
if p.save:
|
||||||
datab[0] = 0xC0
|
datab[0] = 0xC0
|
||||||
else:
|
else:
|
||||||
datab[0] = 0xC2
|
datab[0] = 0xC2
|
||||||
datab[1] = (p.address >> 8) & 0xFF
|
datab[1] = (p.address >> 8) & 0xFF
|
||||||
datab[2] = (p.address) & 0xFF
|
datab[2] = (p.address) & 0xFF
|
||||||
datab[3] = 0
|
datab[3] = 0
|
||||||
datab[3] |= p.parity << 6
|
datab[3] |= p.parity << 6
|
||||||
datab[3] |= (least_gte(p.baud, BAUD_TABLE) << 3)
|
datab[3] |= (__least_gte(p.baud, BAUD_TABLE) << 3)
|
||||||
datab[3] |= (least_gte(p.air_data_rate, AIR_DATA_RATE_TABLE))
|
datab[3] |= (__least_gte(p.air_data_rate, AIR_DATA_RATE_TABLE))
|
||||||
datab[4] = p.channel
|
datab[4] = p.channel
|
||||||
datab[5] = 0
|
datab[5] = 0
|
||||||
datab[5] |= p.tx_mode << 7
|
datab[5] |= p.tx_mode << 7
|
||||||
datab[5] |= p.io_mode << 6
|
datab[5] |= p.io_mode << 6
|
||||||
datab[5] |= (int((p.wake_up_time / 250) - 1) & 0b111) << 3
|
datab[5] |= (int((p.wake_up_time / 250) - 1) & 0b111) << 3
|
||||||
datab[5] |= p.fec_enabled << 2
|
datab[5] |= p.fec_enabled << 2
|
||||||
datab[5] |= least_gte(p.tx_power, TX_POWER_TABLE)
|
datab[5] |= __least_gte(p.tx_power, TX_POWER_TABLE)
|
||||||
return datab
|
return datab
|
||||||
|
|
||||||
|
|
||||||
class E32:
|
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__":
|
if __name__ == "__main__":
|
||||||
p = E32_Params.default()
|
p = E32_Params.default()
|
||||||
print("params: ", p.__dict__)
|
print("params: ", p.__dict__)
|
||||||
data = p.pack()
|
data = p.pack()
|
||||||
print("packed data: ", ', '.join(format(x, '02x') for x in data))
|
print("packed data: ", ', '.join(format(x, '02x') for x in data))
|
||||||
p2 = E32_Params.unpack(data)
|
p2 = E32_Params.unpack(data)
|
||||||
print("unpacked params: ", p2.__dict__)
|
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