From 9429576aff9d0cb95fc5cfcf3cfccdde013289ea Mon Sep 17 00:00:00 2001
From: simondlevy <simon.d.levy@gmail.com>
Date: Thu, 5 Jul 2018 13:21:11 -0400
Subject: [PATCH] Interpolation via scipy

---
 examples/rpslam_scipy.py | 107 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 107 insertions(+)
 create mode 100755 examples/rpslam_scipy.py

diff --git a/examples/rpslam_scipy.py b/examples/rpslam_scipy.py
new file mode 100755
index 0000000..de65187
--- /dev/null
+++ b/examples/rpslam_scipy.py
@@ -0,0 +1,107 @@
+#!/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()