Begin C interpolation code for distances,angles

examples/rpslam.py

@@ -30,9 +30,6 @@ 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
@@ -55,34 +52,41 @@ if __name__ == '__main__':
 
     while True:
 
-        # Extrat (quality, angle, distance) triples from current scan
+        try:
+
+            # Extract (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]
 
-        # 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
+            print(len(distances), len(angles))
 
-        # Update SLAM with current Lidar scan, using third element of (quality, angle, distance) triples
-        slam.update(distances)
+            # Update SLAM with current Lidar scan and scan angles
+            slam.update(distances, scan_angles_degrees=angles)
 
-        # Get current robot position
-        x, y, theta = slam.getpos()
+        except KeyboardInterrupt:
 
-        # 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
 
+        # Get current robot position
+        #x, y, theta = slam.getpos()
+
+        # Get current map bytes as grayscale
+        #slam.getmap(mapbytes)
+
+        # Display the map
+        #display.displayMap(mapbytes)
+
+        # Display the robot's pose in the map
+        #display.setPose(x, y, theta)
+
+        # Break on window close
+        #if not display.refresh():
+        #    break
+
     # Shut down the lidar connection
+    from time import sleep
     lidar.stop()
     lidar.disconnect()

python/pybreezyslam.c

@@ -288,21 +288,25 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds)
             "lidar must be a list");
     }
 
-    // Scan angles provided; run bozo-filter to match against lidar-list size
+    // Scan angles provided
     if (py_scan_angles_degrees != Py_None) 
     {
-        // Bozo filter on SCAN_ANGLES_DEGREES argument
+        // Bozo filter #1: SCAN_ANGLES_DEGREES  must be a list
         if (!PyList_Check(py_scan_angles_degrees))
         {
             return null_on_raise_argument_exception_with_details("Scan", "update", 
                     "scan angles must be a list");
         }
 
+        // Bozo filter #2: must have same number of scan angles as scan distances
         if (PyList_Size(py_lidar) != PyList_Size(py_scan_angles_degrees))
         {
             return null_on_raise_argument_exception_with_details("Scan", "update", 
                     "number of scan angles must equal number of scan distances");
         }
+
+        printf("Interpolate!\n"); 
+        Py_RETURN_NONE;
     }
 
     // No scan angles provided; lidar-list size must match scan size
@@ -312,13 +316,12 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds)
                 "lidar size mismatch");
     }
 
-
     // Default to no velocities
     double dxy_mm = 0;
     double dtheta_degrees = 0;
 
     // Bozo filter on velocities tuple
-    if (py_velocities)
+    if (py_velocities != Py_None)
     {
         if (!PyTuple_Check(py_velocities))
         {