激光雷达RPLidar的配置（arduino和rasberrypi）
1.arduino配置
arduino文件
#include 头文件RPlidar.h
#pragma once
#include "Arduino.h"
#include "inc/rptypes.h"
#include "inc/rplidar_cmd.h"
struct RPLidarMeasurement
{
float distance;
float angle;
uint8_t quality;
bool startBit;
};
class RPLidar
{
public:
enum {
RPLIDAR_SERIAL_BAUDRATE = 115200,
RPLIDAR_DEFAULT_TIMEOUT = 500,
};
RPLidar();
~RPLidar();
// open the given serial interface and try to connect to the RPLIDAR
bool begin(HardwareSerial &serialobj);
// close the currently opened serial interface
void end();
// check whether the serial interface is opened
bool isOpen();
// ask the RPLIDAR for its health info
u_result getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout = RPLIDAR_DEFAULT_TIMEOUT);
// ask the RPLIDAR for its device info like the serial number
u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = RPLIDAR_DEFAULT_TIMEOUT);
// stop the measurement operation
u_result stop();
// start the measurement operation
u_result startScan(bool force = false, _u32 timeout = RPLIDAR_DEFAULT_TIMEOUT*2);
// wait for one sample point to arrive
u_result waitPoint(_u32 timeout = RPLIDAR_DEFAULT_TIMEOUT);
// retrieve currently received sample point
const RPLidarMeasurement & getCurrentPoint()
{
return _currentMeasurement;
}
protected:
u_result _sendCommand(_u8 cmd, const void * payload, size_t payloadsize);
u_result _waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout);
protected:
HardwareSerial * _bined_serialdev;
RPLidarMeasurement _currentMeasurement;
};
2.raspberryPi配置
'''test example:
from rplidar import RPLidar
lidar = RPLidar('/dev/ttyUSB0')
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
for i, scan in enumerate(lidar.iter_scans()):
print('%d: Got %d measurments' % (i, len(scan)))
if i > 10:
break
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
'''
import logging
import sys
import time
import codecs
import serial
import struct
import matplotlib.pyplot as plt
import numpy as np
import matplotlib.animation as animation
SYNC_BYTE = b'\xA5'
SYNC_BYTE2 = b'\x5A'
GET_INFO_BYTE = b'\x50'
GET_HEALTH_BYTE = b'\x52'
STOP_BYTE = b'\x25'
RESET_BYTE = b'\x40'
SCAN_BYTE = b'\x20'
FORCE_SCAN_BYTE = b'\x21'
DESCRIPTOR_LEN = 7
INFO_LEN = 20
HEALTH_LEN = 3
INFO_TYPE = 4
HEALTH_TYPE = 6
SCAN_TYPE = 129
#Constants & Command to start A2 motor
MAX_MOTOR_PWM = 1023
DEFAULT_MOTOR_PWM = 660
SET_PWM_BYTE = b'\xF0'
_HEALTH_STATUSES = {
0: 'Good',
1: 'Warning',
2: 'Error',
}
class RPLidarException(Exception):
'''Basic exception class for RPLidar'''
def _b2i(byte):
'''Converts byte to integer (for Python 2 compatability)'''
return byte if int(sys.version[0]) == 3 else ord(byte)
def _process_scan(raw):
'''Processes input raw data and returns measurment data'''
new_scan = bool(_b2i(raw[0]) & 0b1)
inversed_new_scan = bool((_b2i(raw[0]) >> 1) & 0b1)
quality = _b2i(raw[0]) >> 2
if new_scan == inversed_new_scan:
raise RPLidarException('New scan flags mismatch')
check_bit = _b2i(raw[1]) & 0b1
if check_bit != 1:
raise RPLidarException('Check bit not equal to 1')
angle = ((_b2i(raw[1]) >> 1) + (_b2i(raw[2]) << 7)) / 64.
distance = (_b2i(raw[3]) + (_b2i(raw[4]) << 8)) / 4.
return new_scan, quality, angle, distance
class RPLidar(object):
'''Class for communicating with RPLidar rangefinder scanners'''
_serial_port = None #: serial port connection
port = '' #: Serial port name, e.g. /dev/ttyUSB0
timeout = 1 #: Serial port timeout
motor = False #: Is motor running?
baudrate = 115200 #: Baudrate for serial port
def __init__(self, port, baudrate=115200, timeout=1, logger=None):
'''Initilize RPLidar object for communicating with the sensor.
Parameters
----------
port : str
Serial port name to which sensor is connected
baudrate : int, optional
Baudrate for serial connection (the default is 115200)
timeout : float, optional
Serial port connection timeout in seconds (the default is 1)
logger : logging.Logger instance, optional
Logger instance, if none is provided new instance is created
'''
self._serial_port = None
self.port = port
self.baudrate = baudrate
self.timeout = timeout
self.motor_running = None
if logger is None:
logger = logging.getLogger('rplidar')
self.logger = logger
self.connect()
self.start_motor()
def connect(self):
'''Connects to the serial port with the name `self.port`. If it was
connected to another serial port disconnects from it first.'''
if self._serial_port is not None:
self.disconnect()
try:
self._serial_port = serial.Serial(
self.port, self.baudrate,
parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE,
timeout=self.timeout, dsrdtr=True)
except serial.SerialException as err:
raise RPLidarException('Failed to connect to the sensor '
'due to: %s' % err)
def disconnect(self):
'''Disconnects from the serial port'''
if self._serial_port is None:
return
self._serial_port.close()
def set_pwm(self, pwm):
assert(0 <= pwm <= MAX_MOTOR_PWM)
payload = struct.pack(", pwm)
self._send_payload_cmd(SET_PWM_BYTE, payload)
def start_motor(self):
'''Starts sensor motor'''
self.logger.info('Starting motor')
# For A1
self._serial_port.dtr = False
# For A2
self.set_pwm(DEFAULT_MOTOR_PWM)
self.motor_running = True
def stop_motor(self):
'''Stops sensor motor'''
self.logger.info('Stoping motor')
# For A2
self.set_pwm(0)
time.sleep(.001)
# For A1
self._serial_port.dtr = True
self.motor_running = False
def _send_payload_cmd(self, cmd, payload):
'''Sends `cmd` command with `payload` to the sensor'''
size = struct.pack('B', len(payload))
req = SYNC_BYTE + cmd + size + payload
checksum = 0
for v in struct.unpack('B'*len(req), req):
checksum ^= v
req += struct.pack('B', checksum)
self._serial_port.write(req)
self.logger.debug('Command sent: %s' % req)
def _send_cmd(self, cmd):
'''Sends `cmd` command to the sensor'''
req = SYNC_BYTE + cmd
self._serial_port.write(req)
self.logger.debug('Command sent: %s' % req)
def _read_descriptor(self):
'''Reads descriptor packet'''
descriptor = self._serial_port.read(DESCRIPTOR_LEN)
self.logger.debug('Recieved descriptor: %s', descriptor)
if len(descriptor) != DESCRIPTOR_LEN:
raise RPLidarException('Descriptor length mismatch')
elif not descriptor.startswith(SYNC_BYTE + SYNC_BYTE2):
raise RPLidarException('Incorrect descriptor starting bytes')
is_single = _b2i(descriptor[-2]) == 0
return _b2i(descriptor[2]), is_single, _b2i(descriptor[-1])
def _read_response(self, dsize):
'''Reads response packet with length of `dsize` bytes'''
self.logger.debug('Trying to read response: %d bytes', dsize)
data = self._serial_port.read(dsize)
self.logger.debug('Recieved data: %s', data)
if len(data) != dsize:
raise RPLidarException('Wrong body size')
return data
def get_info(self):
'''Get device information
Returns
-------
dict
Dictionary with the sensor information
'''
self._send_cmd(GET_INFO_BYTE)
dsize, is_single, dtype = self._read_descriptor()
if dsize != INFO_LEN:
raise RPLidarException('Wrong get_info reply length')
if not is_single:
raise RPLidarException('Not a single response mode')
if dtype != INFO_TYPE:
raise RPLidarException('Wrong response data type')
raw = self._read_response(dsize)
serialnumber = codecs.encode(raw[4:], 'hex').upper()
serialnumber = codecs.decode(serialnumber, 'ascii')
data = {
'model': _b2i(raw[0]),
'firmware': (_b2i(raw[2]), _b2i(raw[1])),
'hardware': _b2i(raw[3]),
'serialnumber': serialnumber,
}
return data
def get_health(self):
'''Get device health state. When the core system detects some
potential risk that may cause hardware failure in the future,
the returned status value will be 'Warning'. But sensor can still work
as normal. When sensor is in the Protection Stop state, the returned
status value will be 'Error'. In case of warning or error statuses
non-zero error code will be returned.
Returns
-------
status : str
'Good', 'Warning' or 'Error' statuses
error_code : int
The related error code that caused a warning/error.
'''
self._send_cmd(GET_HEALTH_BYTE)
dsize, is_single, dtype = self._read_descriptor()
if dsize != HEALTH_LEN:
raise RPLidarException('Wrong get_info reply length')
if not is_single:
raise RPLidarException('Not a single response mode')
if dtype != HEALTH_TYPE:
raise RPLidarException('Wrong response data type')
raw = self._read_response(dsize)
status = _HEALTH_STATUSES[_b2i(raw[0])]
error_code = (_b2i(raw[1]) << 8) + _b2i(raw[2])
return status, error_code
def clear_input(self):
'''Clears input buffer by reading all available data'''
self._serial_port.read_all()
def stop(self):
'''Stops scanning process, disables laser diode and the measurment
system, moves sensor to the idle state.'''
self.logger.info('Stoping scanning')
self._send_cmd(STOP_BYTE)
time.sleep(.001)
self.clear_input()
def reset(self):
'''Resets sensor core, reverting it to a similar state as it has
just been powered up.'''
self.logger.info('Reseting the sensor')
self._send_cmd(RESET_BYTE)
time.sleep(.002)
def iter_measurments(self, max_buf_meas=500):
'''Iterate over measurments. Note that consumer must be fast enough,
otherwise data will be accumulated inside buffer and consumer will get
data with increaing lag.
Parameters
----------
max_buf_meas : int
Maximum number of measurments to be stored inside the buffer. Once
numbe exceeds this limit buffer will be emptied out.
Yields
------
new_scan : bool
True if measurment belongs to a new scan
quality : int
Reflected laser pulse strength
angle : float
The measurment heading angle in degree unit [0, 360)
distance : float
Measured object distance related to the sensor's rotation center.
In millimeter unit. Set to 0 when measurment is invalid.
'''
self.start_motor()
status, error_code = self.get_health()
self.logger.debug('Health status: %s [%d]', status, error_code)
if status == _HEALTH_STATUSES[2]:
self.logger.warning('Trying to reset sensor due to the error. '
'Error code: %d', error_code)
self.reset()
status, error_code = self.get_health()
if status == _HEALTH_STATUSES[2]:
raise RPLidarException('RPLidar hardware failure. '
'Error code: %d' % error_code)
elif status == _HEALTH_STATUSES[1]:
self.logger.warning('Warning sensor status detected! '
'Error code: %d', error_code)
cmd = SCAN_BYTE
self._send_cmd(cmd)
dsize, is_single, dtype = self._read_descriptor()
if dsize != 5:
raise RPLidarException('Wrong get_info reply length')
if is_single:
raise RPLidarException('Not a multiple response mode')
if dtype != SCAN_TYPE:
raise RPLidarException('Wrong response data type')
while True:
raw = self._read_response(dsize)
self.logger.debug('Recieved scan response: %s' % raw)
if max_buf_meas:
data_in_buf = self._serial_port.in_waiting
if data_in_buf > max_buf_meas*dsize:
self.logger.warning(
'Too many measurments in the input buffer: %d/%d. '
'Clearing buffer...',
data_in_buf//dsize, max_buf_meas)
self._serial_port.read(data_in_buf//dsize*dsize)
yield _process_scan(raw)
def iter_scans(self, max_buf_meas=500, min_len=5):
'''Iterate over scans. Note that consumer must be fast enough,
otherwise data will be accumulated inside buffer and consumer will get
data with increasing lag.
Parameters
----------
max_buf_meas : int
Maximum number of measurments to be stored inside the buffer. Once
numbe exceeds this limit buffer will be emptied out.
min_len : int
Minimum number of measurments in the scan for it to be yelded.
Yields
------
scan : list
List of the measurments. Each measurment is tuple with following
format: (quality, angle, distance). For values description please
refer to `iter_measurments` method's documentation.
'''
scan = []
iterator = self.iter_measurments(max_buf_meas)
for new_scan, quality, angle, distance in iterator:
if new_scan:
if len(scan) > min_len:
yield scan
scan = []
if quality > 0 and distance > 0:
scan.append((quality, angle, distance))
def record_measurment(path):
lidar = RPLidar('/dev/ttyUSB0')
outfile = open(path, 'w')
try:
print('Recording measurments... Press Crl+C to stop.')
for measurment in lidar.iter_measurments():
line = '\t'.join(str(v) for v in measurment)
outfile.write(line + '\n')
except KeyboardInterrupt:
print("stoping")
lidar.stop()
lidar.stop_motor()
outfile.close()
DMAX = 4000
IMIN = 0
IMAX = 50
def update_line(num, iterator, line):
scan = next(iterator)
offsets = np.array([(np.radians(meas[1]), meas[2]) for meas in scan])
line.set_offsets(offsets)
intens = np.array([meas[0] for meas in scan])
line.set_array(intens)
return line,
def draw_lines():
lidar = RPLidar(PORT_NAME)
fig = plt.figure()
ax = plt.subplot(111, projection='polar')
line = ax.scatter([0, 0], [0, 0], s=5, c=[IMIN, IMAX],
cmap=plt.cm.Greys_r, lw=0)
ax.set_rmax(DMAX)
ax.grid(True)
iterator = lidar.iter_scans()
ani = animation.FuncAnimation(fig, update_line,
fargs=(iterator, line), interval=50)
plt.show()
# lidar.stop()
# lidar.disconnect()
def measure_speed():
lidar = RPLidar(PORT_NAME)
old_t = None
data = []
try:
print('Press Ctrl+C to stop')
for _ in lidar.iter_scans():
now = time.time()
if old_t is None:
old_t = now
continue
delta = now - old_t
print('%.2f Hz, %.2f RPM' % (1/delta, 60/delta))
data.append(delta)
old_t = now
except KeyboardInterrupt:
print('Stoping. Computing mean...')
lidar.stop()
lidar.disconnect()
delta = sum(data)/len(data)
print('Mean: %.2f Hz, %.2f RPM' % (1/delta, 60/delta))
##########execute codes
from rplidar import RPLidar
PORT_NAME='/dev/ttyUSB0'
lidar = RPLidar(PORT_NAME)
record_measurment("/home/pi/Desktop/record1.txt")
#draw_lines()
#measure_speed()
##########
输出效果
