HOKUYO LIDAR URG-04 之 PYTHON驱动

最新推荐文章于 2021-12-29 19:50:22 发布

戈扬

最新推荐文章于 2021-12-29 19:50:22 发布

阅读量3k

点赞数 1

分类专栏： PYTHON

本文链接：https://blog.csdn.net/xuzhexing/article/details/91127965

版权

PYTHON 专栏收录该内容

67 篇文章 20 订阅

订阅专栏

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))

编写hokuyo.py

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()
    ```

戈扬

关注

1
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
HOKUYO LIDAR URG-04 之 PYTHON驱动

编写 serial_port.pyimport structimport sys__author__ = 'paoolo'class SerialPort(object): def __init__(self, serial_port): self.__port = serial_port self.__checksum = 0 de...
复制链接

扫一扫

专栏目录