Start doing test.

tracking/animate_alg.py

@@ -15,16 +15,22 @@ DMAX = 4000
 IMIN = 0
 IMAX = 50
 
-
-def update_line(num, iterator, line):
+def update_line(num, iterator, line, prev_groups):
     scan = next(iterator)
     # Now update the groups, and then update the maps with different colours for different groups.
+    if(prev_groups.groups is None):
+        prev_groups = alg.calc_groups(scan)
+    groups = alg.assign_groups(prev_groups, alg.calc_groups(scan))
     offsets = np.array([(np.radians(meas[1]), meas[2]) for meas in scan])
     line.set_offsets(offsets)
     intens = np.array([meas[0] for meas in scan])
     line.set_array(intens)
     return line,
 
+class Bunch:
+    def __init__(self, **kwds):
+        self.__dict__.update(kwds)
+
 
 def run():
     lidar = MockLidar(loader.load_scans_bytes_file("tracking/out.pickle"))
@@ -34,10 +40,10 @@ def run():
                       cmap=plt.cm.Greys_r, lw=0)
     ax.set_rmax(DMAX)
     ax.grid(True)
-
+    prev_groups = Bunch(groups=None)
     iterator = lidar.iter_scans()
     ani = animation.FuncAnimation(fig, update_line,
-                                  fargs=(iterator, line), interval=50)
+                                  fargs=(iterator, line, prev_groups), interval=50)
     plt.show()
     lidar.stop()
     lidar.disconnect()