Start fixing algorithms

pycar/src/car/tracking/lidar_cache.py

@@ -5,6 +5,7 @@ import zmq
 from car.tracking.devices.mock_lidar import MockLidar
 import car.tracking.lidar_loader as lidar_loader
 import time
+import timeit
 
 
 class LidarCache():
@@ -49,6 +50,7 @@ class LidarCache():
             if not self.run:
                 break
 
+            start_time = time.time()
             # Now process the groups.
             if self.currentGroups is not None:
                 self.currentGroups = algorithms.assign_groups(
@@ -56,6 +58,7 @@ class LidarCache():
             else:
                 self.currentGroups = algorithms.calc_groups(scan)
 
+            print("total time: " + (str)(time.time() - start_time))
             self._fireGroupsChanged()
 
     def _fireGroupsChanged(self):
@@ -99,9 +102,7 @@ class LidarCache():
 
 if __name__ == '__main__':
     lidar = MockLidar(iter(lidar_loader.load_scans_bytes_file(
-        'car/src/car/tracking/out.pickle')))
+        'pycar/src/car/tracking/out.pickle')))
     cache = LidarCache(lidar)
-    cache.add_groups_changed_listener(lambda a: print(a))
+    # cache.add_groups_changed_listener(lambda a: print(a))
     cache.start_cache()
-    time.sleep(1)
-    cache.stop_scanning()