1 复杂版驱动
编写 serial_port.py
import struct
import sys
__author__ = 'paoolo'
class SerialPort(object):
def __init__(self, serial_port):
self.__port = serial_port
self.__checksum = 0
def close(self):
self.__port.close()
def get_checksum(self):
return self.__checksum
def read(self, size):
char = self.__port.read(size)
if sys.version_info >= (3,0,0):
char = str(char, 'UTF-8')
return char
def write(self, char):
if sys.version_info >= (3,0,0):
char = bytes(char, 'UTF-8')
self.__port.write(char)
def send_command(self, address, command):
self.__checksum = address
self.__port.write(chr(address))
self.__checksum += command
self.__port.write(chr(command))
def read_byte(self):
res = self.__port.read(1)
if len(res) > 0:
val = struct.unpack('>B', res)
self.__checksum += val[0] & 0xFF
return val[0]
return None
def read_sbyte(self):
res = self.__port.read(1)
if len(res) > 0:
val = struct.unpack('>b', res)
self.__checksum += val[0] & 0xFF
return val[0]
return None
def read_word(self):
res = self.__port.read(2)
if len(res) > 0:
val = struct.unpack('>H', res)
self.__checksum += val[0] & 0xFF
self.__checksum += (val[0] >> 8) & 0xFF
return val[0]
return None
def read_sword(self):
res = self.__port.read(2)
if len(res) > 0:
val = struct.unpack('>h', res)
self.__checksum += val[0] & 0xFF
self.__checksum += (val[0] >> 8) & 0xFF
return val[0]
return None
def read_long(self):
res = self.__port.read(4)
if len(res) > 0:
val = struct.unpack('>L', res)
self.__checksum += val[0] & 0xFF
self.__checksum += (val[0] >> 8) & 0xFF
self.__checksum += (val[0] >> 16) & 0xFF
self.__checksum += (val[0] >> 24) & 0xFF
return val[0]
return None
def read_slong(self):
res = self.__port.read(4)
if len(res) > 0:
val = struct.unpack('>l', res)
self.__checksum += val[0] & 0xFF
self.__checksum += (val[0] >> 8) & 0xFF
self.__checksum += (val[0] >> 16) & 0xFF
self.__checksum += (val[0] >> 24) & 0xFF
return val[0]
return None
def write_byte(self, val):
self.__checksum += val & 0xFF
return self.__port.write(struct.pack('>B', val))
def write_sbyte(self, val):
self.__checksum += val & 0xFF
return self.__port.write(struct.pack('>b', val))
def write_word(self, val):
self.__checksum += val & 0xFF
self.__checksum += (val >> 8) & 0xFF
return self.__port.write(struct.pack('>H', val))
def write_sword(self, val):
self.__checksum += val & 0xFF
self.__checksum += (val >> 8) & 0xFF
return self.__port.write(struct.pack('>h', val))
def write_long(self, val):
self.__checksum += val & 0xFF
self.__checksum += (val >> 8) & 0xFF
self.__checksum += (val >> 16) & 0xFF
self.__checksum += (val >> 24) & 0xFF
return self.__port.write(struct.pack('>L', val))
def write_slong(self, val):
self.__checksum += val & 0xFF
self.__checksum += (val >> 8) & 0xFF
self.__checksum += (val >> 16) & 0xFF
self.__checksum += (val >> 24) & 0xFF
return self.__port.write(struct.pack('>l', val))
import threading
import traceback
import sys
import time
__author__ = 'paoolo'
def chunks(l, n):
for i in range(0, len(l), n):
yield l[i:i + n]
def decode(val):
bin_str = '0b'
for char in val:
val = ord(char) - 0x30
bin_str += '%06d' % int(bin(val)[2:])
return int(bin_str, 2)
class Hokuyo(object):
SHORT_COMMAND_LEN = 5
MD_COMMAND_REPLY_LEN = 20
LASER_ON = 'BM\n'
LASER_OFF = 'QT\n'
RESET = 'RS\n'
VERSION_INFO = 'VV\n'
SENSOR_STATE = 'II\n'
SENSOR_SPECS = 'PP\n'
SET_SCIP2 = 'SCIP2.0\n'
CHARS_PER_VALUE = 3.0
CHARS_PER_LINE = 66.0
CHARS_PER_BLOCK = 64.0
START_DEG = 119.885
STEP_DEG = 0.35208516886930985
START_STEP = 44
STOP_STEP = 725
VERSION_INFO_LINES = 6
SENSOR_STATE_LINES = 8
SENSOR_SPECS_LINES = 9
def __init__(self, port):
self.__port = port
self.__port_lock = threading.RLock()
self.__timestamp, self.__angles, self.__distances = 0, [], []
self.__scan_lock = threading.Lock()
self.__is_active = True
self.__scanning_allowed = False
def __offset(self):
count = 2
result = ''
self.__port_lock.acquire()
try:
a = self.__port.read(1)
b = self.__port.read(1)
while not ((a == '\n' and b == '\n') or (a == '' and b == '')):
result += a
a = b
b = self.__port.read(1)
count += 1
finally:
self.__port_lock.release()
result += a
result += b
sys.stderr.write('READ %d EXTRA BYTES: "%s"\n' % (count, str(result)))
def __execute_command(self, command):
self.__port_lock.acquire()
try:
self.__port.write(command)
result = self.__port.read(len(command))
assert result == command
finally:
self.__port_lock.release()
return result
def __short_command(self, command, check_response=True):
result = ''
self.__port_lock.acquire()
try:
try:
result += self.__execute_command(command)
result += self.__port.read(Hokuyo.SHORT_COMMAND_LEN)
if check_response:
assert result[-5:-2] == '00P'
assert result[-2:] == '\n\n'
return result
except BaseException as e:
sys.stderr.write('RESULT: "%s"' % result)
traceback.print_exc()
self.__offset()
finally:
self.__port_lock.release()
def __long_command(self, cmd, lines, check_response=True):
result = ''
self.__port_lock.acquire()
try:
try:
result += self.__execute_command(cmd)
result += self.__port.read(4)
if check_response:
assert result[-4:-1] == '00P'
assert result[-1:] == '\n'
line = 0
while line < lines:
char = self.__port.read_byte()
if not char is None:
char = chr(char)
result += char
if char == '\n':
line += 1
else: # char is None
line += 1
assert result[-2:] == '\n\n'
return result
except BaseException as e:
sys.stderr.write('RESULT: "%s"' % result)
traceback.print_exc()
self.__offset()
finally:
self.__port_lock.release()
def terminate(self):
self.reset()
self.__is_active = False
self.__port_lock.acquire()
try:
self.__port.close()
finally:
self.__port_lock.release()
def laser_on(self):
return self.__short_command(Hokuyo.LASER_ON, check_response=True)
def laser_off(self):
return self.__short_command(Hokuyo.LASER_OFF)
def reset(self):
return self.__short_command(Hokuyo.RESET)
def set_scip2(self):
"for URG-04LX"
return self.__short_command(Hokuyo.SET_SCIP2, check_response=False)
def set_motor_speed(self, motor_speed=99):
return self.__short_command('CR' + '%02d' % motor_speed + '\n', check_response=False)
def set_high_sensitive(self, enable=True):
return self.__short_command('HS' + ('1\n' if enable else '0\n'), check_response=False)
def get_version_info(self):
return self.__long_command(Hokuyo.VERSION_INFO, Hokuyo.VERSION_INFO_LINES)
def get_sensor_state(self):
return self.__long_command(Hokuyo.SENSOR_STATE, Hokuyo.SENSOR_STATE_LINES)
def get_sensor_specs(self):
return self.__long_command(Hokuyo.SENSOR_SPECS, Hokuyo.SENSOR_SPECS_LINES)
def __get_and_parse_scan(self, cluster_count, start_step, stop_step):
distances = {}
result = ''
count = ((stop_step - start_step) * Hokuyo.CHARS_PER_VALUE * Hokuyo.CHARS_PER_LINE)
count /= (Hokuyo.CHARS_PER_BLOCK * cluster_count)
count += 1.0 + 4.0 # paoolo(FIXME): why +4.0?
count = int(count)
self.__port_lock.acquire()
try:
result += self.__port.read(count)
finally:
self.__port_lock.release()
assert result[-2:] == '\n\n'
result = result.split('\n')
result = [line[:-1] for line in result]
result = ''.join(result)
i = 0
start = (-Hokuyo.START_DEG + Hokuyo.STEP_DEG * cluster_count * (start_step - Hokuyo.START_STEP))
for chunk in chunks(result, 3):
distances[- ((Hokuyo.STEP_DEG * cluster_count * i) + start)] = decode(chunk)
i += 1
return distances
def get_single_scan(self, start_step=START_STEP, stop_step=STOP_STEP, cluster_count=1): # 返回scan字典
self.__port_lock.acquire()
try:
cmd = 'GD%04d%04d%02d\n' % (start_step, stop_step, cluster_count)
self.__port.write(cmd)
result = self.__port.read(len(cmd))
assert result == cmd
result += self.__port.read(4)
assert result[-4:-1] == '00P'
assert result[-1] == '\n'
result = self.__port.read(6)
assert result[-1] == '\n'
scan = self.__get_and_parse_scan(cluster_count, start_step, stop_step)
return scan
except BaseException as e:
traceback.print_exc()
self.__offset()
finally:
self.__port_lock.release()
def __get_multiple_scans(self, start_step=START_STEP, stop_step=STOP_STEP, cluster_count=1, # 返回 yield scan字典
scan_interval=0, number_of_scans=0):
self.__port_lock.acquire()
try:
cmd = 'MD%04d%04d%02d%01d%02d\n' % (start_step, stop_step, cluster_count, scan_interval, number_of_scans)
self.__port.write(cmd)
result = self.__port.read(len(cmd))
assert result == cmd
result += self.__port.read(Hokuyo.SHORT_COMMAND_LEN)
assert result[-2:] == '\n\n'
index = 0
while number_of_scans == 0 or index > 0:
index -= 1
result = self.__port.read(Hokuyo.MD_COMMAND_REPLY_LEN)
assert result[:13] == cmd[:13]
result = self.__port.read(6)
assert result[-1] == '\n'
scan = self.__get_and_parse_scan(cluster_count, start_step, stop_step)
yield scan
except BaseException as e:
traceback.print_exc()
self.__offset()
finally:
self.__port_lock.release()
def enable_scanning(self, _enable_scanning):
self.__scanning_allowed = _enable_scanning
def __set_scan(self, scan):
if scan is not None:
timestamp = int(time.time() * 1000.0)
angles, distances = Hokuyo.__parse_scan(scan)
self.__scan_lock.acquire()
try:
self.__angles, self.__distances, self.__timestamp = angles, distances, timestamp
finally:
self.__scan_lock.release()
def get_scan(self): # 单次扫描,返回self.__angles, self.__distances, self.__timestamp列表
if not self.__scanning_allowed:
scan = self.get_single_scan()
self.__set_scan(scan)
self.__scan_lock.acquire()
try:
return self.__angles, self.__distances, self.__timestamp
finally:
self.__scan_lock.release()
def scanning_loop(self): # 多次扫描,返回self.__angles, self.__distances, self.__timestamp列表
while self.__is_active:
if self.__scanning_allowed:
self.__port_lock.acquire()
for scan in self.__get_multiple_scans():
self.__set_scan(scan)
if not self.__scanning_allowed or not self.__is_active:
self.laser_off()
self.laser_on()
self.__port_lock.release()
break
time.sleep(0.1)
@staticmethod
def __parse_scan(scan):
angles = sorted(scan.keys())
distances = list(map(scan.get, angles))
return angles, distances
编写测试例程
import serial
import hokuyo
import serial_port
uart_port = 'COM11'
uart_speed = 115200
__author__ = 'paoolo'
if __name__ == '__main__':
laser_serial = serial.Serial(port=uart_port, baudrate=uart_speed, timeout=0.5)
port = serial_port.SerialPort(laser_serial)
laser = hokuyo.Hokuyo(port)
print('laser_on(): ')
print(laser.laser_on())
print('---')
print('laser.get_single_scan():')
print(laser.get_single_scan())
print('---')
print('laser.get_version_info(): ')
print(laser.get_version_info())
print('---')
print('laser.get_sensor_specs(): ')
print(laser.get_sensor_specs())
print('---')
print('laser.get_sensor_state(): ')
print(laser.get_sensor_state())
print('---')
print('laser.set_high_sensitive()')
print(laser.set_high_sensitive())
print('---')
print('laser.set_high_sensitive(False)')
print(laser.set_high_sensitive(False))
print('---')
print('laser.set_motor_speed(10)')
print(laser.set_motor_speed(10))
print('---')
print('laser.set_motor_speed()')
print(laser.set_motor_speed())
print('---')
print('laser.reset()')
print(laser.reset())
print('---')
print('laser.laser_off()')
print(laser.laser_off())
测试结果
C:/Users/scaux/PycharmProjects/test_urg_04/test_driver_hokuyo.py
pydev debugger: process 15416 is connecting
Connected to pydev debugger (build 183.5153.39)
laser_on():
BM
00P
---
laser.get_single_scan():
{119.885: 206, 119.5329148311307: 208, 119.18082966226139: 212, 118.82874449339208: 212, 118.47665932452277: 212, 118.12457415565346: 215, 117.77248898678414: 222, 117.42040381791483: 0, 117.06831864904552: 0, 116.71623348017621: 0, 116.3641483113069: 0, 116.0120631424376: 0, 115.65997797356829: 0, 115.30789280469898: 0, 114.95580763582967: 0, 114.60372246696036: 0, 114.25163729809105: 0, 113.89955212922173: 476, 113.54746696035242: 465, 113.19538179148311: 463, 112.8432966226138: 460, 112.4912114537445: 440, 112.13912628487519: 439, 111.78704111600588: 439, 111.43495594713657: 431, 111.08287077826726: 420, 110.73078560939794: 368, 110.37870044052863: 208, 110.02661527165932: 133, 109.67453010279002: 133, 109.3224449339207: 133, 108.9703597650514: 128, 108.61827459618209: 131, 108.26618942731278: 131, 107.91410425844347: 131, 107.56201908957416: 128, 107.20993392070486: 127, 106.85784875183555: 127, 106.50576358296624: 132, 106.15367841409692: 149, 105.80159324522761: 186, 105.4495080763583: 232, 105.09742290748899: 232, 104.74533773861968: 233, 104.39325256975037: 233, 104.04116740088106: 233, 103.68908223201176: 262, 103.33699706314243: 287, 102.98491189427313: 349, 102.63282672540382: 384, 102.28074155653451: 0, 101.9286563876652: 1426, 101.57657121879589: 0, 101.22448604992658: 7, 100.87240088105727: 0, 100.52031571218797: 0, 100.16823054331866: 0, 99.81614537444935: 0, 99.46406020558004: 629, 99.11197503671073: 629, 98.75988986784142: 7, 98.4078046989721: 0, 98.05571953010279: 0, 97.70363436123348: 0, 97.35154919236417: 0, 96.99946402349487: 0, 96.64737885462556: 0, 96.29529368575625: 640, 95.94320851688693: 627, 95.59112334801762: 627, 95.23903817914831: 640, 94.886953010279: 642, 94.53486784140969: 681, 94.18278267254038: 722, 93.83069750367108: 845, 93.47861233480177: 960, 93.12652716593246: 980, 92.77444199706315: 1038, 92.42235682819384: 1040, 92.07027165932453: 1046, 91.71818649045522: 1046, 91.36610132158592: 1046, 91.0140161527166: 1047, 90.66193098384728: 1056, 90.30984581497798: 0, 89.95776064610867: 0, 89.60567547723936: 0, 89.25359030837005: 0, 88.90150513950074: 0, 88.54941997063143: 1155, 88.19733480176211: 1157, 87.8452496328928: 1157, 87.4931644640235: 1155, 87.14107929515419: 1149, 86.78899412628488: 1149, 86.43690895741557: 1145, 86.08482378854626: 1149, 85.73273861967695: 7, 85.38065345080764: 1412, 85.02856828193833: 1384, 84.67648311306903: 1384, 84.32439794419972: 1365, 83.97231277533041: 1275, 83.6202276064611: 1273, 83.26814243759178: 1263, 82.91605726872247: 1251, 82.56397209985316: 1251, 82.21188693098385: 1251, 81.85980176211454: 1251, 81.50771659324523: 1264, 81.15563142437591: 1303, 80.8035462555066: 1344, 80.4514610866373: 1344, 80.09937591776799: 1344, 79.74729074889868: 1331, 79.39520558002937: 1331, 79.04312041116006: 1331, 78.69103524229075: 1376, 78.33895007342144: 0, 77.98686490455214: 0, 77.63477973568283: 0, 77.28269456681352: 3622, 76.93060939794421: 3622, 76.5785242290749: 3622, 76.22643906020559: 0, 75.87435389133628: 0, 75.52226872246696: 0, 75.17018355359765: 0, 74.81809838472834: 0, 74.46601321585904: 0, 74.11392804698973: 0, 73.7618428781204: 0, 73.4097577092511: 0, 73.05767254038179: 0, 72.70558737151248: 0, 72.35350220264317: 0, 72.00141703377386: 0, 71.64933186490455: 0, 71.29724669603524: 0, 70.94516152716594: 0, 70.59307635829663: 0, 70.24099118942732: 0, 69.88890602055801: 0, 69.5368208516887: 0, 69.1847356828194: 0, 68.83265051395009: 0, 68.48056534508078: 0, 68.12848017621145: 0, 67.77639500734215: 0, 67.42430983847284: 0, 67.07222466960353: 0, 66.72013950073422: 0, 66.36805433186491: 0, 66.01596916299559: 0, 65.66388399412628: 0, 65.31179882525697: 270, 64.95971365638766: 236, 64.60762848751835: 234, 64.25554331864905: 234, 63.90345814977974: 235, 63.55137298091043: 236, 63.19928781204112: 235, 62.84720264317181: 235, 62.495117474302496: 235, 62.14303230543319: 234, 61.79094713656388: 234, 61.43886196769457: 234, 61.08677679882526: 234, 60.73469162995595: 234, 60.38260646108664: 234, 60.03052129221733: 234, 59.67843612334802: 231, 59.32635095447871: 231, 58.974265785609404: 231, 58.62218061674009: 231, 58.27009544787078: 227, 57.91801027900147: 231, 57.56592511013216: 237, 57.213839941262854: 233, 56.86175477239354: 233, 56.50966960352423: 237, 56.15758443465492: 233, 55.80549926578561: 229, 55.453414096916305: 229, 55.101328928046996: 232, 54.74924375917769: 229, 54.39715859030838: 229, 54.04507342143906: 232, 53.69298825256975: 232, 53.34090308370044: 229, 52.98881791483113: 228, 52.63673274596182: 233, 52.284647577092514: 233, 51.932562408223205: 228, 51.5804772393539: 231, 51.22839207048459: 231, 50.87630690161528: 225, 50.52422173274597: 223, 50.17213656387665: 223, 49.82005139500734: 217, 49.46796622613803: 217, 49.11588105726872: 217, 48.763795888399414: 229, 48.411710719530106: 229, 48.0596255506608: 229, 47.70754038179149: 225, 47.35545521292218: 222, 47.00337004405287: 222, 46.65128487518355: 222, 46.29919970631424: 223, 45.94711453744493: 223, 45.595029368575624: 223, 45.242944199706315: 223, 44.89085903083701: 223, 44.5387738619677: 223, 44.18668869309839: 221, 43.83460352422908: 223, 43.48251835535977: 221, 43.130433186490464: 221, 42.77834801762114: 221, 42.42626284875183: 220, 42.074177679882524: 220, 41.722092511013216: 239, 41.37000734214391: 241, 41.0179221732746: 0, 40.66583700440529: 0, 40.31375183553598: 0, 39.96166666666667: 0, 39.609581497797365: 0, 39.257496328928056: 0, 38.90541116005873: 0, 38.553325991189425: 0, 38.201240822320116: 0, 37.84915565345081: 0, 37.4970704845815: 0, 37.14498531571219: 3233, 36.79290014684288: 3233, 36.440814977973574: 3212, 36.088729809104265: 3212, 35.73664464023496: 3272, 35.38455947136565: 3272, 35.032474302496325: 3272, 34.68038913362702: 7, 34.32830396475771: 3305, 33.9762187958884: 3305, 33.62413362701909: 3307, 33.27204845814978: 3307, 32.919963289280474: 3309, 32.567878120411166: 6, 32.21579295154186: 6, 31.86370778267255: 6, 31.511622613803226: 2468, 31.159537444933918: 2443, 30.80745227606461: 2420, 30.4553671071953: 2389, 30.103281938325992: 2372, 29.751196769456683: 2325, 29.399111600587375: 2308, 29.047026431718066: 2274, 28.694941262848758: 2248, 28.34285609397945: 2221, 27.99077092511014: 2184, 27.638685756240818: 2168, 27.28660058737151: 2168, 26.9345154185022: 2124, 26.582430249632893: 2088, 26.230345080763584: 724, 25.878259911894276: 691, 25.526174743024967: 665, 25.17408957415566: 656, 24.82200440528635: 656, 24.46991923641704: 650, 24.11783406754772: 649, 23.76574889867841: 619, 23.4136637298091: 611, 23.061578560939793: 607, 22.709493392070485: 594, 22.357408223201176: 592, 22.005323054331868: 587, 21.65323788546256: 587, 21.30115271659325: 575, 20.949067547723942: 563, 20.596982378854634: 529, 20.24489720998531: 525, 19.892812041116002: 522, 19.540726872246694: 513, 19.188641703377385: 512, 18.836556534508077: 511, 18.48447136563877: 510, 18.13238619676946: 490, 17.78030102790015: 480, 17.428215859030843: 472, 17.076130690161534: 469, 16.724045521292226: 469, 16.371960352422903: 469, 16.019875183553594: 469, 15.667790014684286: 464, 15.315704845814977: 464, 14.963619676945669: 462, 14.61153450807636: 455, 14.259449339207052: 455, 13.907364170337743: 447, 13.555279001468435: 446, 13.203193832599126: 440, 12.851108663729818: 435, 12.499023494860495: 434, 12.146938325991187: 429, 11.794853157121878: 428, 11.44276798825257: 429, 11.090682819383261: 429, 10.738597650513952: 429, 10.386512481644644: 428, 10.034427312775335: 425, 9.682342143906027: 425, 9.330256975036718: 425, 8.978171806167396: 426, 8.626086637298087: 426, 8.274001468428779: 426, 7.92191629955947: 424, 7.569831130690162: 421, 7.217745961820853: 421, 6.8656607929515445: 424, 6.513575624082236: 425, 6.1614904552129275: 425, 5.809405286343619: 428, 5.4573201174743105: 425, 5.105234948604988: 425, 4.753149779735679: 428, 4.401064610866371: 431, 4.048979441997062: 431, 3.6968942731277536: 431, 3.344809104258445: 421, 2.9927239353891366: 395, 2.640638766519828: 382, 2.2885535976505196: 373, 1.936468428781211: 368, 1.5843832599118883: 367, 1.2322980910425798: 355, 0.8802129221732713: 355, 0.5281277533039628: 355, 0.17604258443465426: 354, -0.17604258443465426: 354, -0.5281277533039628: 357, -0.8802129221732713: 351, -1.2322980910425798: 342, -1.5843832599118883: 335, -1.9364684287811968: 333, -2.2885535976505196: 327, -2.640638766519828: 333, -2.9927239353891366: 329, -3.344809104258445: 329, -3.6968942731277536: 330, -4.048979441997062: 330, -4.401064610866371: 329, -4.753149779735679: 326, -5.105234948604988: 326, -5.457320117474296: 326, -5.809405286343605: 336, -6.1614904552129275: 342, -6.513575624082236: 342, -6.8656607929515445: 345, -7.217745961820853: 345, -7.569831130690162: 345, -7.92191629955947: 347, -8.274001468428779: 347, -8.626086637298087: 348, -8.978171806167396: 351, -9.330256975036704: 348, -9.682342143906013: 346, -10.034427312775321: 348, -10.38651248164463: 350, -10.738597650513938: 350, -11.090682819383247: 350, -11.442767988252555: 350, -11.794853157121892: 350, -12.1469383259912: 354, -12.49902349486051: 354, -12.851108663729818: 354, -13.203193832599126: 359, -13.555279001468435: 360, -13.907364170337743: 364, -14.259449339207052: 1299, -14.61153450807636: 1325, -14.963619676945669: 1330, -15.315704845814977: 1330, -15.667790014684286: 1330, -16.019875183553594: 1329, -16.371960352422903: 1329, -16.72404552129221: 1329, -17.07613069016152: 1326, -17.42821585903083: 1326, -17.780301027900137: 1325, -18.132386196769446: 1322, -18.484471365638754: 1319, -18.836556534508063: 1319, -19.1886417033774: 1316, -19.540726872246708: 1315, -19.892812041116017: 1311, -20.244897209985325: 1309, -20.596982378854634: 1305, -20.949067547723942: 1302, -21.30115271659325: 1302, -21.65323788546256: 1302, -22.005323054331868: 1302, -22.357408223201176: 1297, -22.709493392070485: 1296, -23.061578560939793: 1294, -23.4136637298091: 1293, -23.76574889867841: 1294, -24.11783406754772: 1292, -24.469919236417027: 1287, -24.822004405286336: 1287, -25.174089574155644: 1286, -25.526174743024953: 1283, -25.87825991189426: 1282, -26.23034508076357: 1282, -26.582430249632907: 1283, -26.934515418502215: 1283, -27.286600587371524: 1280, -27.638685756240832: 1281, -27.99077092511014: 1281, -28.34285609397945: 1281, -28.694941262848758: 1284, -29.047026431718066: 1284, -29.399111600587375: 1286, -29.751196769456683: 1286, -30.103281938325992: 1286, -30.4553671071953: 1286, -30.80745227606461: 1286, -31.159537444933918: 1287, -31.511622613803226: 1286, -31.863707782672535: 1286, -32.21579295154184: 1286, -32.56787812041115: 1291, -32.91996328928046: 1283, -33.27204845814977: 1291, -33.62413362701908: 1291, -33.976218795888386: 1291, -34.32830396475772: 1287, -34.68038913362703: 1287, -35.03247430249634: 1287, -35.38455947136565: 1287, -35.73664464023496: 1280, -36.088729809104265: 1274, -36.440814977973574: 1194, -36.79290014684288: 1088, -37.14498531571219: 1085, -37.4970704845815: 1075, -37.84915565345081: 1075, -38.201240822320116: 1069, -38.553325991189425: 1069, -38.90541116005873: 1068, -39.25749632892804: 1068, -39.60958149779735: 1068, -39.96166666666666: 1073, -40.31375183553597: 1078, -40.665837004405276: 1079, -41.017922173274584: 1090, -41.37000734214389: 1090, -41.72209251101323: 1079, -42.07417767988254: 974, -42.42626284875185: 953, -42.778348017621155: 938, -43.130433186490464: 937, -43.48251835535977: 937, -43.83460352422908: 935, -44.18668869309839: 930, -44.5387738619677: 929, -44.89085903083701: 928, -45.242944199706315: 928, -45.595029368575624: 926, -45.94711453744493: 928, -46.29919970631424: 934, -46.65128487518355: 934, -47.00337004405286: 934, -47.355455212922166: 934, -47.707540381791475: 934, -48.05962555066078: 945, -48.41171071953009: 1029, -48.7637958883994: 1105, -49.11588105726871: 1115, -49.467966226138046: 1115, -49.820051395007354: 1107, -50.17213656387666: 1107, -50.52422173274597: 1096, -50.87630690161528: 1078, -51.22839207048459: 1062, -51.5804772393539: 1048, -51.932562408223205: 1023, -52.284647577092514: 1000, -52.63673274596182: 985, -52.98881791483113: 971, -53.34090308370044: 955, -53.69298825256975: 953, -54.04507342143906: 929, -54.397158590308365: 915, -54.74924375917767: 896, -55.10132892804698: 886, -55.45341409691629: 874, -55.8054992657856: 859, -56.15758443465491: 838, -56.509669603524216: 835, -56.86175477239355: 825, -57.21383994126286: 819, -57.56592511013217: 802, -57.91801027900148: 795, -58.27009544787079: 791, -58.622180616740096: 774, -58.974265785609404: 759, -59.32635095447871: 741, -59.67843612334802: 741, -60.03052129221733: 738, -60.38260646108664: 737, -60.73469162995595: 714, -61.086776798825255: 712, -61.438861967694564: 711, -61.79094713656387: 694, -62.14303230543318: 686, -62.49511747430249: 683, -62.8472026431718: 675, -63.199287812041106: 665, -63.551372980910415: 664, -63.90345814977972: 643, -64.25554331864906: 640, -64.60762848751837: 640, -64.95971365638768: 639, -65.31179882525699: 636, -65.6638839941263: 636, -66.0159691629956: 610, -66.36805433186491: 517, -66.72013950073422: 463, -67.07222466960353: 456, -67.42430983847284: 444, -67.77639500734215: 438, -68.12848017621145: 428, -68.48056534508076: 424, -68.83265051395007: 420, -69.18473568281938: 412, -69.53682085168869: 412, -69.888906020558: 411, -70.2409911894273: 410, -70.59307635829661: 401, -70.94516152716592: 401, -71.29724669603523: 399, -71.64933186490457: 399, -72.00141703377388: 398, -72.35350220264318: 393, -72.7055873715125: 383, -73.0576725403818: 382, -73.40975770925111: 378, -73.76184287812042: 378, -74.11392804698973: 378, -74.46601321585904: 378, -74.81809838472834: 378, -75.17018355359765: 383, -75.52226872246696: 383, -75.87435389133627: 385, -76.22643906020558: 386, -76.57852422907489: 414, -76.9306093979442: 476, -77.2826945668135: 476, -77.63477973568281: 476, -77.98686490455212: 478, -78.33895007342143: 475, -78.69103524229074: 474, -79.04312041116005: 472, -79.39520558002938: 464, -79.74729074889869: 464, -80.099375917768: 460, -80.45146108663731: 460, -80.80354625550662: 455, -81.15563142437593: 455, -81.50771659324523: 455, -81.85980176211454: 446, -82.21188693098385: 446, -82.56397209985316: 435, -82.91605726872247: 435, -83.26814243759178: 406, -83.62022760646109: 347, -83.9723127753304: 347, -84.3243979441997: 347, -84.67648311306901: 425, -85.02856828193832: 427, -85.38065345080763: 427, -85.73273861967694: 425, -86.08482378854625: 419, -86.43690895741555: 419, -86.78899412628489: 419, -87.1410792951542: 419, -87.49316446402351: 419, -87.84524963289282: 419, -88.19733480176212: 414, -88.54941997063143: 412, -88.90150513950074: 407, -89.25359030837005: 407, -89.60567547723936: 404, -89.95776064610867: 404, -90.30984581497798: 394, -90.66193098384728: 394, -91.0140161527166: 398, -91.3661013215859: 398, -91.71818649045521: 393, -92.07027165932452: 392, -92.42235682819383: 387, -92.77444199706314: 387, -93.12652716593244: 387, -93.47861233480175: 389, -93.83069750367106: 387, -94.18278267254037: 386, -94.5348678414097: 379, -94.88695301027902: 376, -95.23903817914832: 373, -95.59112334801763: 373, -95.94320851688694: 372, -96.29529368575625: 372, -96.64737885462556: 372, -96.99946402349487: 364, -97.35154919236417: 364, -97.70363436123348: 364, -98.05571953010279: 364, -98.4078046989721: 364, -98.75988986784141: 359, -99.11197503671072: 359, -99.46406020558003: 359, -99.81614537444933: 360, -100.16823054331864: 360, -100.52031571218795: 360, -100.87240088105726: 363, -101.22448604992657: 363, -101.57657121879588: 359, -101.92865638766521: 359, -102.28074155653452: 359, -102.63282672540383: 359, -102.98491189427314: 359, -103.33699706314245: 352, -103.68908223201176: 352, -104.04116740088106: 350, -104.39325256975037: 349, -104.74533773861968: 349, -105.09742290748899: 349, -105.4495080763583: 349, -105.80159324522761: 349, -106.15367841409692: 348, -106.50576358296622: 348, -106.85784875183553: 347, -107.20993392070484: 345, -107.56201908957415: 345, -107.91410425844346: 347, -108.26618942731277: 347, -108.61827459618208: 346, -108.97035976505138: 346, -109.32244493392072: 346, -109.67453010279003: 342, -110.02661527165934: 342, -110.37870044052865: 341, -110.73078560939796: 341, -111.08287077826726: 338, -111.43495594713657: 341, -111.78704111600588: 341, -112.13912628487519: 337, -112.4912114537445: 342, -112.8432966226138: 342, -113.19538179148311: 337, -113.54746696035242: 337, -113.89955212922173: 334, -114.25163729809104: 329, -114.60372246696035: 334, -114.95580763582966: 334, -115.30789280469897: 334, -115.65997797356827: 335, -116.01206314243758: 335, -116.36414831130689: 335, -116.71623348017623: 335, -117.06831864904554: 333, -117.42040381791485: 329, -117.77248898678415: 329, -118.12457415565346: 329, -118.47665932452277: 329, -118.82874449339208: 331, -119.18082966226139: 331, -119.5329148311307: 328, -119.885: 328}
---
laser.get_version_info():
VV
00P
VEND:Hokuyo Automatic Co.,Ltd.;[
PROD:SOKUIKI Sensor URG-04LX-UG01(Simple-URG);[
FIRM:3.4.03(17/Dec./2012);T
PROT:SCIP 2.0;N
SERI:H1629885;\
---
laser.get_sensor_specs():
PP
00P
MODL:URG-04LX-UG01(Simple-URG)(Hokuyo Automatic Co.,Ltd.);N
DMIN:20;4
DMAX:5600;_
ARES:1024;\
AMIN:44;7
AMAX:725;o
AFRT:384;6
SCAN:600;e
---
laser.get_sensor_state():
II
00P
MODL:URG-04LX-UG01(Simple-URG)(Hokuyo Automatic Co.,Ltd.);N
LASR:ON;9
SCSP:Initial(600[rpm])<-Default setting by user;A
MESM:Measuring by Normal Mode;0
SBPS:19200[bps]<-Default setting by user;A
TIME:041597;S
STAT:Sensor works well.;8
---
laser.set_high_sensitive()
HS1
00P
---
laser.set_high_sensitive(False)
HS0
00P
---
laser.set_motor_speed(10)
CR10
00P
---
laser.set_motor_speed()
CR99
00P
---
laser.reset()
RS
00P
---
laser.laser_off()
QT
00P
Process finished with exit code 0
2 简化版驱动
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# The MIT License
#
# Copyright (c) 2010 Yota Ichino
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
##############################################################################
# revised by xux
# 2019-06-07
# 兼容 python3
# python3 compatiable
##############################################################################
import serial
import re
import math
import time
class UrgDevice(serial.Serial):
def __init__(self):
super(serial.Serial, self).__init__()
def __del__(self):
self.laser_off()
def connect(self, port='/dev/ttyACM0', baudrate=115200, timeout=0.1):
'''
Connect to URG device
port : Port or device name. ex:/dev/ttyACM0, COM1, etc...
baudrate : Set baudrate. ex: 9600, 38400, etc...
timeout : Set timeout[sec]
'''
self.port = port
self.baudrate = baudrate
self.timeout = timeout
try:
self.open()
except:
return False
if not self.isOpen():
return False
self.set_scip2()
self.get_parameter()
return True
def is_open(self):
'''If port is opening, return True.'''
#aa = self.isOpen()
return self.isOpen()
def flush_input_buf(self):
'''Clear input buffer.'''
self.flushInput()
def send_command(self, cmd):
'''Send command to device.'''
# REVISED ,COMPATIABLE PYTHON3
self.write(cmd.encode())
def __receive_data(self):
#return self.readlines()
#########################################
# REVISED COMPATIBALE WITH PYTHON3
aaa = self.readlines()
#print('type of aaa: ', type(aaa))
for i in range(len(aaa)):
aaa[i] = bytes.decode(aaa[i])
#print(' __receive_data, aaa = ',aaa)
return aaa
#############################################
def set_scip2(self):
'''Set SCIP2.0 protcol'''
self.flush_input_buf()
self.send_command('SCIP2.0\n')
return self.__receive_data()
def get_version(self):
'''Get version information.'''
if not self.isOpen():
return False
self.flush_input_buf()
self.send_command('VV\n')
get = self.__receive_data()
return get
def get_parameter(self):
'''Get device parameter'''
#if not self.is_open():
# revised: 'bool' object is not callable, there is not BOOL!
if not self.isOpen():
return False
self.send_command('PP\n')
get = self.__receive_data()
print('get_parameter, get :',get)
# check expected value
if not (get[:2] == ['PP\n', '00P\n']):
return False
# pick received data out of parameters
self.pp_params = {}
for item in get[2:10]:
tmp = re.split(r':|;', item)[:2]
self.pp_params[tmp[0]] = tmp[1]
return self.pp_params
def laser_on(self):
'''Turn on the laser.'''
#print('laser_on, isOpen :', self.isOpen())
if not self.isOpen():
return False
self.send_command('BM\n')
get = self.__receive_data()
#print('laser_on, get : ', get)
if not (get == ['BM\n', '00P\n', '\n']) and not (get == ['BM\n', '02R\n', '\n']):
return False
return True
def laser_off(self):
'''Turn off the laser.'''
print('laser_off : turn off the laser')
if not self.isOpen():
return False
self.flush_input_buf()
self.send_command('QT\n')
get = self.__receive_data()
if not (get == ['QT\n', '00P\n', '\n']):
return False
return True
def __decode(self, encode_str):
'''Return a numeric which converted encoded string from numeric'''
decode = 0
for c in encode_str:
decode <<= 6
decode &= ~0x3f
decode |= ord(c) - 0x30
return decode
def __decode_length(self, encode_str, byte):
'''Return leght data as list'''
data = []
for i in range(0, len(encode_str), byte):
split_str = encode_str[i: i + byte]
data.append(self.__decode(split_str))
return data
def index2rad(self, index):
'''Convert index to radian and reurun.'''
rad = (2.0 * math.pi) * (index - int(self.pp_params['AFRT'])) / int(self.pp_params['ARES'])
return rad
def create_capture_command(self):
'''create capture command.'''
cmd = 'GD' + self.pp_params['AMIN'].zfill(4) + self.pp_params['AMAX'].zfill(4) + '01\n'
return cmd
def create_scan_command(self, byte=3, interval=0, num_scan=1):
'''create scan command.'''
if byte == 3:
command = "MD"
elif byte == 2:
command = "MS"
else:
print("byte: 2 or 3")
raise ValueError
if interval > 9:
print("interval should be 0~9")
raise ValueError
str_interval = str(interval)
if num_scan < 100:
str_num_scan = str(num_scan).zfill(2)
else:
str_num_scans = "00"
cmd = command + self.pp_params['AMIN'].zfill(4) + self.pp_params['AMAX'].zfill(4) + '01'+ str_interval \
+ str_num_scan+ '\n'
return cmd
def scan_sec(self):
'''Return time of a cycle.'''
rpm = float(self.pp_params['SCAN'])
return (60.0 / rpm)
# 连续获取多圈数据 unfinished
def scan(self,byte=3, interval=0, num_scan=1):
cmd = self.create_scan_command()
self.flush_input_buf()
self.send_command(cmd)
get = self.__receive_data()
#if get[1] != "00P\n":
# print("Status: ", get[1][:-2])
# raise EnvironmentError
if not (get[:2] == [cmd, '00P\n']):
return [], -1
for n in range(num_scan):
pass
## 获取一圈数据
def capture(self):
if not self.laser_on():
print('laser off')
return [], -1
# Receive lenght data
cmd = self.create_capture_command()
self.flush_input_buf()
#print('capture, cmd: ', cmd)
self.send_command(cmd)
time.sleep(0.1)
get = self.__receive_data()
# print('capture, get: ', get)
# checking the answer
if not (get[:2] == [cmd, '00P\n']):
return [], -1
# decode the timestamp
tm_str = get[2][:-1] # timestamp
timestamp = self.__decode(tm_str)
# decode length data
length_byte = 0
line_decode_str = ''
if cmd[:2] == ('GS' or 'MS'):
length_byte = 2
elif cmd[:2] == ('GD' or 'MD'):
length_byte = 3
# Combine different lines which mean length data
NUM_OF_CHECKSUM = -2
for line in get[3:]:
line_decode_str += line[:NUM_OF_CHECKSUM]
# Set dummy data by begin index.
self.length_data = [-1 for i in range(int(self.pp_params['AMIN']))]
self.length_data += self.__decode_length(line_decode_str, length_byte)
return (self.length_data, timestamp)
def main():
urg = UrgDevice( )
if not urg.connect('COM11'):
print('Connect error')
exit()
else:
print(urg)
for i in range(10):
data, tm = urg.capture()
if data == 0:
continue
print(len(data),data, tm)
if __name__ == '__main__':
main()
```