Remove debug stuff, lidar streaming working.

Tracking still not working correctly, due to assign groups not working propery.

pycar/src/car/tracking/algorithms.py

@@ -112,10 +112,8 @@ def calc_groups(scan):
     currentGroup = Group(0)
     allGroups = [currentGroup]
     currentGroupNumber = 0
-    num_iters = 0
     # assume the list is already sorted.
     for point in scan:
-        num_iters += 1
         if prevPoint is None:
             prevPoint = point
             currentGroup.add_point(point)
@@ -132,11 +130,6 @@ def calc_groups(scan):
             allGroups.append(currentGroup)
 
         prevPoint = point
-    print(num_iters)
-    print(len(allGroups))
-    for group in allGroups:
-        print(len(group.get_points()))
-
     return allGroups
 
 
@@ -182,7 +175,7 @@ def assign_groups(prev_groups, new_groups):
         for new_group in new_groups:
             new_centre = find_centre(new_group)
             # They are considered the same if the new group and old group centres are within 10cm.
-            if ((new_centre[0] - old_centre[0]) ** 2 + (new_centre[1] - old_centre[1]) ** 2) < 100 ** 2:
+            if ((new_centre[0] - old_centre[0]) ** 2 + (new_centre[1] - old_centre[1]) ** 2) < 50 ** 2:
                 new_group.number = group.number
                 if group.number > max_group_number:
                     max_group_number = group.number

pycar/src/car/tracking/animate_alg.py

@@ -2,7 +2,7 @@
 Animates distances and angle of lidar
 Uses model-free algorithms to track grouping of points (objects/groups)
 """
-from tracking.mock_lidar import MockLidar
+from car.tracking.devices.mock_lidar import MockLidar
 import matplotlib.pyplot as plt
 import numpy as np
 import matplotlib.animation as animation
@@ -35,7 +35,7 @@ class Bunch:
 
 
 def run():
-    lidar = MockLidar(loader.load_scans_bytes_file("tracking/out.pickle"))
+    lidar = MockLidar(loader.load_scans_bytes_file("pycar/src/car/tracking/out.pickle"))
     fig = plt.figure()
     ax = plt.subplot(111, projection='polar')
     line = ax.scatter([0, 0], [0, 0], s=5, c=[IMIN, IMAX],

pycar/src/car/tracking/lidar_cache.py

@@ -4,8 +4,6 @@ import car.tracking.lidar_tracker_pb2 as tracker_pb
 import zmq
 from car.tracking.devices.mock_lidar import MockLidar
 import car.tracking.lidar_loader as lidar_loader
-import time
-import timeit
 
 
 class LidarCache():
@@ -50,7 +48,6 @@ 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(
@@ -58,7 +55,6 @@ class LidarCache():
             else:
                 self.currentGroups = algorithms.calc_groups(scan)
 
-            print("total time: " + (str)(time.time() - start_time))
             self._fireGroupsChanged()
 
     def _fireGroupsChanged(self):