Restored Matlab, Java

examples/rpslam.py

@@ -30,6 +30,9 @@ 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
@@ -52,36 +55,33 @@ if __name__ == '__main__':
 
     while True:
 
-        try:
+        # Extrat (quality, angle, distance) triples from current scan
+        items = [item for item in next(iterator)]
 
-            # 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]
 
-            # Extract distances and angles from triples
-            distances = [item[2] for item in items]
-            angles    = [item[1] for item in items]
-            print(angles)
-
-        except KeyboardInterrupt:
-            break
+        # 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)))
 
         # Update SLAM with current Lidar scan, using third element of (quality, angle, distance) triples
-        #slam.update([item[2] for item in next(iterator)])
+        slam.update(distances)
 
         # Get current robot position
-        #x, y, theta = slam.getpos()
+        x, y, theta = slam.getpos()
 
         # Get current map bytes as grayscale
-        #slam.getmap(mapbytes)
+        slam.getmap(mapbytes)
 
-        #display.displayMap(mapbytes)
+        display.displayMap(mapbytes)
 
-        #display.setPose(x, y, theta)
+        display.setPose(x, y, theta)
 
-        # Exit on ESCape
-        #key = display.refresh()
-        #if key != None and (key&0x1A):
-        #    exit(0)
+        # Exit on window close
+        if not display.refresh():
+            exit(0)
 
     lidar.stop()
     lidar.disconnect()