-
-
Save Fabien-B/87e57316e5b265ca42d179f3c860f879 to your computer and use it in GitHub Desktop.
from serial import Serial | |
import time | |
from collections import namedtuple | |
class UST: | |
Command = namedtuple('Command',['command', 'answer_expected', 'answer']) | |
START_RANGING = Command('#GT15466', False, '') | |
STOP_RANGING = Command('#ST5297', True, '#ST00A845') | |
ID = Command('#IN0D54', True, '') | |
ID2 = Command('#CLC2DD', True, '') | |
PLOP = Command('#GR0EEE1', True, '') | |
MESURE_LENGHT = 4359 | |
def __init__(self, port="/dev/ttyACM0", baudrate=115200): | |
self.ser = Serial(port, baudrate) | |
self.stop_ranging() | |
def send_command(self, command, timeout=2): | |
self.ser.write(command.command.encode()+b'\n') # writes command to LIDAR | |
self.ser.reset_input_buffer() | |
data = b'' | |
start_time = time.time() | |
if command.answer_expected and command.answer != '': #precise answer expected, search for it ! | |
while time.time() - start_time < timeout: | |
if self.ser.in_waiting: | |
data+=self.ser.read() | |
data = data.split(b'\n')[-1] | |
if command.answer.encode() in data: | |
break | |
return data | |
elif command.answer_expected: # answer expected but be don't known which : return the first one (until \n) | |
while time.time() - start_time < timeout: | |
if self.ser.in_waiting: | |
data+=self.ser.read() | |
if b'\n' in data: | |
data = data.split(b'\n')[0] | |
break | |
return data | |
else: | |
return b'' | |
def stop_ranging(self): | |
self.send_command(self.STOP_RANGING) | |
def start_ranging(self): | |
self.stop_ranging() | |
self.send_command(self.START_RANGING) | |
def stop(self): | |
self.stop_ranging() | |
def get_measures(self): | |
""" | |
returns measures under the form (timestamp, [(distance, quality), ...]) | |
timestamp : time in seconds since the LIDAR startup | |
distance range : 0 - 65635 | |
valur range : 0 - ??? (65635 max) | |
eg: (102.123456, [(552, 1244), (646, 1216), (676, 1270), ...]) | |
""" | |
raw_bytes=self.ser.read(ust.MESURE_LENGHT) | |
data = raw_bytes.split(b':') | |
timestamp = int(data[1], 16)/10**6 | |
measurement_bytes = data[3] | |
measurements = [(int(measurement_bytes[i:i+4],16), int(measurement_bytes[i+4:i+8],16)) for i in range(0, len(measurement_bytes)-8, 8)] | |
return (timestamp, measurements) | |
def get_data(self, nb_bytes): | |
data = self.ser.read(nb_bytes) | |
data = data.split(b':') | |
data = data[3] | |
aa = [(int(data[i:i+4],16), int(data[i+4:i+8],16)) for i in range(0, len(data)-8, 8)] | |
print('data received') | |
if __name__ == "__main__": | |
ust = UST() | |
try: | |
ret = ust.send_command(ust.ID) | |
print(ret) | |
ust.start_ranging() | |
while True: | |
data = ust.get_measures() | |
if len(data[1]) == 541 : | |
print("ok at {:.06f} s".format(data[0])) | |
#print(data) | |
finally: | |
ust.stop() | |
Thank you for this !
This code is very fragile to any communication error (not my best code).
I do not remember if each scan ends with \n, but if so, it would be preferable to read bytes until this character is hit.
Hi, the output of the lidar (at least the UST05LN, but probably others too) is ASCII, so you can take a look at it as a text.
For each scan, the lidar output some data separated by ":".
One of the field is the timestamp of the scan, an other field is the list of the distances and signal strength, on 2 bytes each.
For example, 01F404B0020803E8022605DC
means, in the (distance, quality) form: [(500, 1200), (520, 1000), (550, 1500)].
Based on this, you can look if the protocol is similar.
What I can see in your error message is that it tries to interpret as hexa a string containing L and N letters, which are not hexadecimals symbols. It is probably not measurement data, you can certainly figure that out by reading the ASCII output.
Also, this code id very fragile to communications errors, so you may want to write a better code, but at least you have the idea of the protocol!
Thanks a lot Fabien-B!
So that anyone looking here would know, for UST-20LN version of this code only changes needed are:
line 87: if len(data[1]) == 541
toline 87: if len(data[1]) == 1081
since for 20LN the resolution is higher, andline 16: MESURE_LENGHT = 4359
(= 541 steps * 8 bytes + 31 bytes ) toline 16: MESURE_LENGHT = 8679
(= 1081 steps * 8 bytes + 31 bytes )