综述
本文总共有三部分，第一部分是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 sys
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))
1.2kokuyo.py编写
# -*- coding: utf-8 -*-
import threading
import traceback
import sys
import time
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:
# noinspection PyBroadException
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:
# noinspection PyBroadException
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):
self.__port_lock.acquire()
# noinspection PyBroadException
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,
scan_interval=0, number_of_scans=0):
self.__port_lock.acquire()
# noinspection PyBroadException
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):
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):
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
1.3 test_drive_hokuyo.py
#!\Anaconda3\python.exe #可以根据实际情况改或删除
# -*- coding: utf-8 -*-
import serial
import hokuyo
import serial_port
uart_port = 'COM14' #根据实际情况改端口
uart_speed = 115200
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())
将三个文件放在一个文件夹中，测试结果：
第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. This
routine is run between each time loop() runs, so using delay inside loop can
delay 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 serial
serialPort = "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_port
uart_port = 'COM14' #激光雷达URG-40的读取数据的串口设置
uart_speed = 115200
serialPort = "COM7" # 串口旋转编码器
baudRate = 115200
if __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,
...
扫一扫
585
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
