improve ugv script
This commit is contained in:
parent
7f6b699d46
commit
5175649281
@ -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='ENABLE', index=1, number=1,
|
||||
name='CMD_DRIVE_TO_TARGET', index=1, number=1,
|
||||
serialized_options=None,
|
||||
type=None),
|
||||
_descriptor.EnumValueDescriptor(
|
||||
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
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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()
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user