Prepare to test lidar tracker.

tracking/lidar_cache.py

@@ -20,7 +20,6 @@ class LidarCache():
         self.run = True
         self.tracking_group_number = -1
         self.currentGroups = None
-        self.groupsChanged = []
 
     def start_cache(self, sender):
         self.thread = Thread(target=self.do_scanning, args=[sender])
@@ -42,7 +41,11 @@ class LidarCache():
         # TODO: Implement custom batching, as iter_scans can be unreliable
         for scan in self.lidar.iter_scans(min_len=self.measurements):
             print('Got %d measurments' % (len(scan)))
-            if(not self.run):
+            if len(scan) < self.measurements:
+                # Poor scan, likely since it was the first scan.
+                continue
+
+            if not self.run:
                 break
 
             # Now process the groups.
@@ -52,11 +55,19 @@ class LidarCache():
             else:
                 self.currentGroups = algorithms.calc_groups(scan)
 
+            self.fireGroupsChanged()
+
     def fireGroupsChanged(self):
         # Send the updated groups to 0MQ socket.
         # Rename this to be a generic listener method, rather than an explicit 'send' (even though it can be treated as such already)
-        self._mFactory.send_message_topic("lidar_map", messages.ProtoMessage(
-            message=tracker_pb.PointScan(points=[]).SerializeToString()))
+        pointScan = tracker_pb.PointScan()
+        for group in self.currentGroups:
+            for point in group.get_points():
+                pointScan.points.append(tracker_pb.Point(
+                    angle=point[1], distance=point[2], group_number=group.number))
+
+        self._mFactory.send_message_topic(
+            "lidar_map", messages.ProtoMessage(message=pointScan.SerializeToString()))
 
     def stop_scanning(self):
         self.run = False