amikhalev
/
uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Browse Source

lots more work on python ugv client

try-fix-comms-errors
Alex Mikhalev 6 years ago
parent
24bce4311a
commit
f475a5b962
3 changed files with 554 additions and 106 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 43
      e32_client/e32.py
  2. 351
      e32_client/messages_pb2.py
  3. 66
      e32_client/ugv.py

43
e32_client/e32.py
Unescape View File

@ -1,3 +1,5 @@ @@ -1,3 +1,5 @@
#!/usr/bin/env python3
import serial
import struct
@ -40,8 +42,9 @@ TX_POWER_TABLE = { @@ -40,8 +42,9 @@ TX_POWER_TABLE = {
3: 21,
}
def least_gte(value, dic):
items = sorted(dic.items(), key = lambda item: item[1])
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
@ -49,6 +52,7 @@ def least_gte(value, dic): @@ -49,6 +52,7 @@ def least_gte(value, dic):
break
return last_k
class E32_Params:
save: bool
address: int
@ -62,6 +66,7 @@ class E32_Params: @@ -62,6 +66,7 @@ class E32_Params:
fec_enabled: bool
tx_power: int
@staticmethod
def default():
p = E32_Params()
p.save = True
@ -77,15 +82,18 @@ class E32_Params: @@ -77,15 +82,18 @@ class E32_Params:
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')
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]
@ -109,15 +117,15 @@ class E32_Params: @@ -109,15 +117,15 @@ class E32_Params:
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[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)
datab[5] |= __least_gte(p.tx_power, TX_POWER_TABLE)
return datab
@ -127,6 +135,29 @@ class E32: @@ -127,6 +135,29 @@ class E32:
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)
if __name__ == "__main__":
p = E32_Params.default()
print("params: ", p.__dict__)

351
e32_client/messages_pb2.py
Unescape View File

@ -0,0 +1,351 @@ @@ -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
Unescape View File

@ -0,0 +1,66 @@ @@ -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()
Write Preview
Loading…
Cancel
Save