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