This commit is contained in:
simondlevy
2018-07-04 20:46:18 -04:00
parent b579f2f537
commit 1fb0162305

View File

@@ -28,7 +28,6 @@ from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import RPLidarA1 as LaserModel from breezyslam.sensors import RPLidarA1 as LaserModel
from rplidar import RPLidar as Lidar from rplidar import RPLidar as Lidar
from pltslamshow import SlamShow from pltslamshow import SlamShow
from logging import Logger
if __name__ == '__main__': if __name__ == '__main__':
@@ -50,6 +49,7 @@ if __name__ == '__main__':
# Create an iterator to collect scan data from the RPLidar # Create an iterator to collect scan data from the RPLidar
iterator = lidar.iter_scans() iterator = lidar.iter_scans()
# We will use these to store previous scan in case current scan is inadequate
previous_distances = None previous_distances = None
previous_angles = None previous_angles = None
@@ -65,13 +65,13 @@ if __name__ == '__main__':
distances = [item[2] for item in items] distances = [item[2] for item in items]
angles = [item[1] for item in items] angles = [item[1] for item in items]
# Update SLAM with current Lidar scan and scan angles if sufficient # Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES: if len(distances) > MIN_SAMPLES:
slam.update(distances, scan_angles_degrees=angles) slam.update(distances, scan_angles_degrees=angles)
previous_distances = distances.copy() previous_distances = distances.copy()
previous_angles = angles.copy() previous_angles = angles.copy()
# If not sufficient, use previous # If not adequate, use previous
elif previous_distances is not None: elif previous_distances is not None:
slam.update(previous_distances, scan_angles_degrees=previous_angles) slam.update(previous_distances, scan_angles_degrees=previous_angles)