108 lines
3.2 KiB
Python
Executable File
108 lines
3.2 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
|
|
'''
|
|
rpslam.py : BreezySLAM Python with SLAMTECH RP A1 Lidar
|
|
|
|
Copyright (C) 2018 Simon D. Levy
|
|
|
|
This code is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU Lesser General Public License as
|
|
published by the Free Software Foundation, either version 3 of the
|
|
License, or (at your option) any later version.
|
|
|
|
This code is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU Lesser General Public License
|
|
along with this code. If not, see <http://www.gnu.org/licenses/>.
|
|
'''
|
|
|
|
MAP_SIZE_PIXELS = 500
|
|
MAP_SIZE_METERS = 10
|
|
LIDAR_DEVICE = '/dev/ttyUSB0'
|
|
MIN_SAMPLES = 180
|
|
|
|
from breezyslam.algorithms import RMHC_SLAM
|
|
from breezyslam.sensors import RPLidarA1 as LaserModel
|
|
|
|
from rplidar import RPLidar as Lidar
|
|
|
|
from pltslamshow import SlamShow
|
|
|
|
from scipy.interpolate import interp1d
|
|
import numpy as np
|
|
|
|
if __name__ == '__main__':
|
|
|
|
# Connect to Lidar unit
|
|
lidar = Lidar(LIDAR_DEVICE)
|
|
|
|
# Create an RMHC SLAM object with a laser model and optional robot model
|
|
slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
|
|
|
|
# Set up a SLAM display
|
|
display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
|
|
|
|
# Initialize an empty trajectory
|
|
trajectory = []
|
|
|
|
# Initialize empty map
|
|
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
|
|
|
|
# Create an iterator to collect scan data from the RPLidar
|
|
iterator = lidar.iter_scans()
|
|
|
|
# We will use this to store previous scan in case current scan is inadequate
|
|
previous_distances = None
|
|
|
|
# First scan is crap, so ignore it
|
|
next(iterator)
|
|
|
|
while True:
|
|
|
|
# Extrat (quality, angle, distance) triples from current scan
|
|
items = [item for item in next(iterator)]
|
|
|
|
# Extract distances and angles from triples
|
|
distances = [item[2] for item in items]
|
|
angles = [item[1] for item in items]
|
|
|
|
print(len(distances))
|
|
|
|
# Update SLAM with current Lidar scan and scan angles if adequate
|
|
if len(distances) > MIN_SAMPLES:
|
|
|
|
# First interpolate to get 360 angles from 0 through 359, and corresponding distances
|
|
f = interp1d(angles, distances, fill_value='extrapolate')
|
|
distances = list(f(np.arange(360))) # slam.update wants a list
|
|
|
|
# Then update with interpolated distances
|
|
slam.update(distances)
|
|
|
|
# Store interplated distances for next time
|
|
previous_distances = distances.copy()
|
|
|
|
# If not adequate, use previous
|
|
elif previous_distances is not None:
|
|
slam.update(previous_distances)
|
|
|
|
# Get current robot position
|
|
x, y, theta = slam.getpos()
|
|
|
|
# Get current map bytes as grayscale
|
|
slam.getmap(mapbytes)
|
|
|
|
display.displayMap(mapbytes)
|
|
|
|
display.setPose(x, y, theta)
|
|
|
|
# Break on window close
|
|
if not display.refresh():
|
|
break
|
|
|
|
# Shut down the lidar connection
|
|
lidar.stop()
|
|
lidar.disconnect()
|