Begin supporting scan angles argument to Scan.update()

python/breezyslam/algorithms.py

@@ -87,29 +87,30 @@ class CoreSLAM(object):
         # Initialize the map 
         self.map = pybreezyslam.Map(map_size_pixels, map_size_meters)
                 
-    def update(self, scans_mm, poseChange, should_update_map=True):
+    def update(self, scans_mm, pose_change, scan_angles_degrees=None, should_update_map=True):
         '''
         Updates the scan and odometry, and calls the the implementing class's _updateMapAndPointcloud method with
         the specified pose change.
          
         scan_mm is a list of Lidar scan values, whose count is specified in the scan_size 
         attribute of the Laser object passed to the CoreSlam constructor
-        poseChange is a tuple (dxy_mm, dtheta_degrees, dt_seconds) computed from odometry
+        pose_change is a tuple (dxy_mm, dtheta_degrees, dt_seconds) computed from odometry
+        scan_angles_degrees is an optional list of angles corresponding to the distances in scans_mm
         should_update_map flags for whether you want to update the map
         '''
 
         # Convert pose change (dxy,dtheta,dt) to velocities (dxy/dt, dtheta/dt) for scan update
-        velocity_factor = (1 / poseChange[2])  if (poseChange[2] > 0) else 0 # units => units/sec
+        velocity_factor = (1 / pose_change[2])  if (pose_change[2] > 0) else 0 # units => units/sec
-        dxy_mm_dt = poseChange[0] * velocity_factor  
+        dxy_mm_dt = pose_change[0] * velocity_factor  
-        dtheta_degrees_dt = poseChange[1] * velocity_factor
+        dtheta_degrees_dt = pose_change[1] * velocity_factor
         velocities = (dxy_mm_dt, dtheta_degrees_dt)
 
         # Build a scan for computing distance to map, and one for updating map 
-        self._scan_update(self.scan_for_mapbuild, scans_mm, velocities)
+        self._scan_update(self.scan_for_mapbuild, scans_mm, velocities, scan_angles_degrees)
-        self._scan_update(self.scan_for_distance, scans_mm, velocities)
+        self._scan_update(self.scan_for_distance, scans_mm, velocities, scan_angles_degrees)
 
         # Implementing class updates map and pointcloud
-        self._updateMapAndPointcloud(poseChange[0], poseChange[1], should_update_map)
+        self._updateMapAndPointcloud(pose_change[0], pose_change[1], should_update_map)
         
     def getmap(self, mapbytes):
         '''
@@ -136,9 +137,10 @@ class CoreSLAM(object):
          return self.__str__()
 
         
-    def _scan_update(self, scan, lidar, velocities):
+    def _scan_update(self, scan, lidar, velocities, scan_angles_degrees):
 
-        scan.update(scans_mm=lidar, hole_width_mm=self.hole_width_mm, velocities=velocities)
+        scan.update(scans_mm=lidar, hole_width_mm=self.hole_width_mm, 
+                velocities=velocities, scan_angles_degrees=scan_angles_degrees)
         
         
 # SinglePositionSLAM class ---------------------------------------------------------------------------------------------
@@ -251,13 +253,13 @@ class RMHC_SLAM(SinglePositionSLAM):
         self.sigma_theta_degrees = sigma_theta_degrees
         self.max_search_iter = max_search_iter
         
-    def update(self, scan_mm, angles_degrees=None, poseChange=None, should_update_map=True):
+    def update(self, scans_mm, pose_change=None, scan_angles_degrees=None, should_update_map=True):
 
-        if not poseChange:
+        if not pose_change:
         
-            poseChange = (0, 0, 0)
+            pose_change = (0, 0, 0)
     
-        CoreSLAM.update(self, scan_mm, poseChange, should_update_map)   
+        CoreSLAM.update(self, scans_mm, pose_change, scan_angles_degrees, should_update_map)   
     
     def _getNewPosition(self, start_position):
         '''

python/pybreezyslam.c

@@ -268,13 +268,15 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds)
     PyObject * py_lidar = NULL;
     double hole_width_mm = 0;
     PyObject * py_velocities = NULL;
+    PyObject * py_scan_angles_degrees = NULL;
 
-    static char* argnames[] = {"scans_mm", "hole_width_mm", "velocities", NULL};
+    static char* argnames[] = {"scans_mm", "hole_width_mm", "velocities", "scan_angles_degrees", NULL};
 
-    if (!PyArg_ParseTupleAndKeywords(args, kwds,"Od|O", argnames,
+    if (!PyArg_ParseTupleAndKeywords(args, kwds,"Od|OO", argnames,
         &py_lidar, 
         &hole_width_mm,
-        &py_velocities))
+        &py_velocities,
+        &py_scan_angles_degrees))
     {
         return null_on_raise_argument_exception("Scan", "update");
     }
@@ -286,13 +288,31 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds)
             "lidar must be a list");
     }
 
-    // Bozo filter on LIDAR argument list size
+    // Scan angles provided; run bozo-filter to match against lidar-list size
-    if (PyList_Size(py_lidar) != self->scan.size)
+    if (py_scan_angles_degrees != Py_None) 
+    {
+        // Bozo filter on SCAN_ANGLES_DEGREES argument
+        if (!PyList_Check(py_scan_angles_degrees))
+        {
+            return null_on_raise_argument_exception_with_details("Scan", "update", 
+                    "scan angles must be a list");
+        }
+
+        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");
+        }
+    }
+
+    // No scan angles provided; lidar-list size must match scan size
+    else if (PyList_Size(py_lidar) != self->scan.size)
     {        
         return null_on_raise_argument_exception_with_details("Scan", "update", 
                 "lidar size mismatch");
     }
 
+
     // Default to no velocities
     double dxy_mm = 0;
     double dtheta_degrees = 0;
@@ -315,7 +335,6 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds)
         }
     }
 
-
     // Extract LIDAR values from argument
     int k = 0;
     for (k=0; k<self->scan.size; ++k)