综述

本文总共有三部分，第一部分是URG-04的python驱动调试，第二部分是python串口读取旋转编码器数据.第三部分是将URG-04数据和串口数据合并调试。

第1部分：URG-04的python驱动调试

1.1 serial_port.py编写

# -*- coding: utf-8 -*-
# E:\Anaconda3\python.exe			 #可以根据实际情况改或删除import struct
import sysclass SerialPort(object):def __init__(self, serial_port):self.__port = serial_portself.__checksum = 0def close(self):self.__port.close()def get_checksum(self):return self.__checksumdef read(self, size):char = self.__port.read(size)if sys.version_info >= (3,0,0):char = str(char, 'UTF-8')return chardef 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 = addressself.__port.write(chr(address))self.__checksum += commandself.__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] & 0xFFreturn val[0]return Nonedef read_sbyte(self):res = self.__port.read(1)if len(res) > 0:val = struct.unpack('>b', res)self.__checksum += val[0] & 0xFFreturn val[0]return Nonedef read_word(self):res = self.__port.read(2)if len(res) > 0:val = struct.unpack('>H', res)self.__checksum += val[0] & 0xFFself.__checksum += (val[0] >> 8) & 0xFFreturn val[0]return Nonedef read_sword(self):res = self.__port.read(2)if len(res) > 0:val = struct.unpack('>h', res)self.__checksum += val[0] & 0xFFself.__checksum += (val[0] >> 8) & 0xFFreturn val[0]return Nonedef read_long(self):res = self.__port.read(4)if len(res) > 0:val = struct.unpack('>L', res)self.__checksum += val[0] & 0xFFself.__checksum += (val[0] >> 8) & 0xFFself.__checksum += (val[0] >> 16) & 0xFFself.__checksum += (val[0] >> 24) & 0xFFreturn val[0]return Nonedef read_slong(self):res = self.__port.read(4)if len(res) > 0:val = struct.unpack('>l', res)self.__checksum += val[0] & 0xFFself.__checksum += (val[0] >> 8) & 0xFFself.__checksum += (val[0] >> 16) & 0xFFself.__checksum += (val[0] >> 24) & 0xFFreturn val[0]return Nonedef write_byte(self, val):self.__checksum += val & 0xFFreturn self.__port.write(struct.pack('>B', val))def write_sbyte(self, val):self.__checksum += val & 0xFFreturn self.__port.write(struct.pack('>b', val))def write_word(self, val):self.__checksum += val & 0xFFself.__checksum += (val >> 8) & 0xFFreturn self.__port.write(struct.pack('>H', val))def write_sword(self, val):self.__checksum += val & 0xFFself.__checksum += (val >> 8) & 0xFFreturn self.__port.write(struct.pack('>h', val))def write_long(self, val):self.__checksum += val & 0xFFself.__checksum += (val >> 8) & 0xFFself.__checksum += (val >> 16) & 0xFFself.__checksum += (val >> 24) & 0xFFreturn self.__port.write(struct.pack('>L', val))def write_slong(self, val):self.__checksum += val & 0xFFself.__checksum += (val >> 8) & 0xFFself.__checksum += (val >> 16) & 0xFFself.__checksum += (val >> 24) & 0xFFreturn self.__port.write(struct.pack('>l', val))

1.2kokuyo.py编写

# -*- coding: utf-8 -*-
import threading
import traceback
import sys
import timedef 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) - 0x30bin_str += '%06d' % int(bin(val)[2:])return int(bin_str, 2)class Hokuyo(object):SHORT_COMMAND_LEN = 5MD_COMMAND_REPLY_LEN = 20LASER_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.0CHARS_PER_LINE = 66.0CHARS_PER_BLOCK = 64.0START_DEG = 119.885STEP_DEG = 0.35208516886930985START_STEP = 44STOP_STEP = 725VERSION_INFO_LINES = 6SENSOR_STATE_LINES = 8SENSOR_SPECS_LINES = 9def __init__(self, port):self.__port = portself.__port_lock = threading.RLock()self.__timestamp, self.__angles, self.__distances = 0, [], []self.__scan_lock = threading.Lock()self.__is_active = Trueself.__scanning_allowed = Falsedef __offset(self):count = 2result = ''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 += aa = bb = self.__port.read(1)count += 1finally:self.__port_lock.release()result += aresult += bsys.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 == commandfinally:self.__port_lock.release()return resultdef __short_command(self, command, check_response=True):result = ''self.__port_lock.acquire()try:# noinspection PyBroadExceptiontry: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 resultexcept 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:# noinspection PyBroadExceptiontry:result += self.__execute_command(cmd)result += self.__port.read(4)if check_response:assert result[-4:-1] == '00P'assert result[-1:] == '\n'line = 0while line < lines:char = self.__port.read_byte()if not char is None:char = chr(char)result += charif char == '\n':line += 1else:  # char is Noneline += 1assert result[-2:] == '\n\n'return resultexcept 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 = Falseself.__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 = 0start = (-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 += 1return distancesdef get_single_scan(self, start_step=START_STEP, stop_step=STOP_STEP, cluster_count=1):self.__port_lock.acquire()# noinspection PyBroadExceptiontry:cmd = 'GD%04d%04d%02d\n' % (start_step, stop_step, cluster_count)self.__port.write(cmd)result = self.__port.read(len(cmd))assert result == cmdresult += 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 scanexcept 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,scan_interval=0, number_of_scans=0):self.__port_lock.acquire()# noinspection PyBroadExceptiontry: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 == cmdresult += self.__port.read(Hokuyo.SHORT_COMMAND_LEN)assert result[-2:] == '\n\n'index = 0while number_of_scans == 0 or index > 0:index -= 1result = 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 scanexcept BaseException as e:traceback.print_exc()self.__offset()finally:self.__port_lock.release()def enable_scanning(self, _enable_scanning):self.__scanning_allowed = _enable_scanningdef __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, timestampfinally:self.__scan_lock.release()def get_scan(self):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.__timestampfinally:self.__scan_lock.release()def scanning_loop(self):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()breaktime.sleep(0.1)@staticmethoddef __parse_scan(scan):angles = sorted(scan.keys())distances = list(map(scan.get, angles))return angles, distances

1.3 test_drive_hokuyo.py

#!\Anaconda3\python.exe  #可以根据实际情况改或删除
# -*- coding: utf-8 -*-
import serialimport hokuyo
import serial_portuart_port = 'COM14'  	#根据实际情况改端口
uart_speed = 115200if __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())

将三个文件放在一个文件夹中，测试结果：

第2部分：python串口读取旋转编码器数据

2.1 下位机Arduino读取旋转编码器的数据并发送

本节概述：首先使Arduino读取旋转编码器的数据，其次调通Arduino的串口通信，最后调试：将旋转编码器的数据通过串口传输到上位机上。

Arduino的程序代码：

/** File Name: uart_test.c* Description: Encode and Serial Event example*              When new serial data arrives, this sketch adds it to a String.*               When a newline is received, the loop prints the string and clears it.* Author: yuyu* Create Date: 2019-06-17*/String inputString = "";         // a String to hold incoming data
bool stringComplete = false;  // whether the string is complete//int val; 
int encoder0PinA = 6;   //Rotary encoder phase A pin
int encoder0PinB = 7;   //Rotary encider phase B pin
long int encoder0Pos = 0; //count the namber of encoder
int encoder0PinALast = LOW;
int n = LOW; void setup() { pinMode (encoder0PinA,INPUT);pinMode (encoder0PinB,INPUT);Serial.begin (115200);// reserve 200 bytes for the inputString:inputString.reserve(200);} void loop() { n = digitalRead(encoder0PinA);if ((encoder0PinALast == LOW) && (n == HIGH)) {//上升沿if (digitalRead(encoder0PinB) == LOW) {encoder0Pos--;} else {encoder0Pos++;}Serial.println (encoder0Pos);
//     Serial.print ("mm\n");} encoder0PinALast = n;// print the string when a newline arrives:if (stringComplete) {Serial.println(inputString);Serial.println (encoder0Pos);// clear the string:inputString = "";stringComplete = false; } }/*SerialEvent occurs whenever a new data comes in the hardware serial RX. Thisroutine is run between each time loop() runs, so using delay inside loop candelay response. Multiple bytes of data may be available.
*/
void serialEvent() {while (Serial.available()) {// get the new byte:char inChar = (char)Serial.read();// add it to the inputString:inputString += inChar;// if the incoming character is a newline, set a flag so the main loop can// do something about it:if (inChar == '\n') {stringComplete = true;}}
} 

2.2 上位机的测试简易版python代码

serial_test.py:

# E:\Anaconda3\python.exe
# -*- coding: utf-8 -*-
import serialserialPort = "COM7"  # 串口
baudRate = 115200  # 波特率
ser = serial.Serial(serialPort, baudRate, timeout=0.5)
print("参数设置：串口=%s ，波特率=%d" % (serialPort, baudRate))while 1:str = ser.readline()print(str)ser.close()

2.3 测试结果

第3部分：将URG-04数据和串口数据合并调试

3.1 liar_serial_encode.py的编写

import serial
import hokuyo
import serial_portuart_port = 'COM14'		#激光雷达URG-40的读取数据的串口设置
uart_speed = 115200serialPort = "COM7"  # 串口旋转编码器
baudRate = 115200if __name__ == '__main__':laser_serial = serial.Serial(port=uart_port, baudrate=uart_speed, timeout=0.5)port = serial_port.SerialPort(laser_serial)ser = serial.Serial(serialPort, baudRate, timeout=0.5)while 1:laser = hokuyo.Hokuyo(port)print('laser_on(): ')print(laser.laser_on())print('laser.get_single_scan():')print(laser.get_single_scan())print(laser.get_single_scan())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('---')str = ser.readline()print(str)ser.close()

3.1 测试结果

总结

本次写的博客就是：调试各个模块然后，然后简单地将旋转编码器数据加入激光雷达的数据中，后期还要将整个数据其进一步优化。

激光雷达采集的部分数据如下：

119.885: 0,119.5329148311307: 0,119.18082966226139: 0,118.82874449339208: 0,118.47665932452277: 0,118.12457415565346: 0,117.77248898678414: 0,117.42040381791483: 0,117.06831864904552: 0,116.71623348017621: 0,116.3641483113069: 0,116.0120631424376: 0,115.65997797356829: 7,115.30789280469898: 0,114.95580763582967: 0,114.60372246696036: 0,114.25163729809105: 0,113.89955212922173: 507,113.54746696035242: 480,113.19538179148311: 473,112.8432966226138: 473,112.4912114537445: 469,112.13912628487519: 445,111.78704111600588: 394,111.43495594713657: 347,111.08287077826726: 347,110.73078560939794: 347,110.37870044052863: 350,110.02661527165932: 7,109.67453010279002: 0,109.3224449339207: 0,108.9703597650514: 0,108.61827459618209: 0,108.26618942731278: 0,107.91410425844347: 0,107.56201908957416: 0,107.20993392070486: 0,106.85784875183555: 0,106.50576358296624: 0,106.15367841409692: 0,105.80159324522761: 0,105.4495080763583: 0,105.09742290748899: 0,104.74533773861968: 0,104.39325256975037: 0,104.04116740088106: 0,103.68908223201176: 0,103.33699706314243: 0,102.98491189427313: 0,102.63282672540382: 0,102.28074155653451: 0,101.9286563876652: 0,101.57657121879589: 0,101.22448604992658: 0,100.87240088105727: 0,100.52031571218797: 0,100.16823054331866: 0,99.81614537444935: 0,99.46406020558004: 0,99.11197503671073: 0,98.75988986784142: 0,98.4078046989721: 0,98.05571953010279: 0,97.70363436123348: 0,97.35154919236417: 0,96.99946402349487: 0,96.64737885462556: 0,96.29529368575625: 0,95.94320851688693: 0,95.59112334801762: 0,95.23903817914831: 0,94.886953010279: 0,94.53486784140969: 0,94.18278267254038: 0,93.83069750367108: 0,93.47861233480177: 1102,93.12652716593246: 1102,92.77444199706315: 7,92.42235682819384: 0,92.07027165932453: 0,91.71818649045522: 0,91.36610132158592: 0,91.0140161527166: 1278,90.66193098384728: 1278,90.30984581497798: 1278,89.95776064610867: 1278,89.60567547723936: 1278,89.25359030837005: 1227,88.90150513950074: 1171,88.54941997063143: 1171,88.19733480176211: 1171,87.8452496328928: 1171,...

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！