Move root car to pycar, put other pycar back to car.

pycar/src/car/tracking/lidar_cache.py

@@ -0,0 +1,95 @@
+from threading import Thread
+from car.tracking import algorithms
+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
+
+
+class LidarCache():
+    """
+    A class that retrieves scans from the lidar,
+    runs grouping algorithms between scans and 
+    keeps a copy of the group data.
+    """
+
+    def __init__(self, lidar, measurements=100):
+        self.lidar = lidar
+        self.measurements = measurements
+        print('Info: ' + str(self.lidar.get_info()))
+        print('Health: ' + str(self.lidar.get_health()))
+        self.run = True
+        self.tracking_group_number = -1
+        self.currentGroups = None
+        self._group_listeners = []
+
+    def start_cache(self):
+        self.thread = Thread(target=self.do_scanning)
+        self.thread.start()
+
+    def do_scanning(self):
+        """Performs scans whilst cache is running, and will pass calculated groups data to the sender.
+
+        Parameters
+        ----------
+        listener: 
+            Any object that includes the onGroupsChanged method. 
+
+        """
+
+        # Batch over scans, so we don't need to do our own batching to determine groups
+        # 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 len(scan) < self.measurements:
+                # Poor scan, likely since it was the first scan.
+                continue
+
+            if not self.run:
+                break
+
+            # Now process the groups.
+            if self.currentGroups is not None:
+                self.currentGroups = algorithms.assign_groups(
+                    self.currentGroups, algorithms.calc_groups(scan))
+            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)
+        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))
+
+        for listener in self._group_listeners:
+            listener(pointScan)
+
+    def add_groups_changed_listener(self, listener):
+        """
+        Add a listener for a change in scans. THis will provide a tuple with the new group
+        scans, which can then be sent off to a network listener for display, or to update the 
+        vehicle with a new velocity.
+
+        Parameters
+        ----------
+        listener
+            An function that takes a PointScan proto object as its argument.
+        """
+        self._group_listeners.append(listener)
+
+    def stop_scanning(self):
+        self.run = False
+
+if __name__ == '__main__':
+    lidar = MockLidar(iter(lidar_loader.load_scans_bytes_file('car/src/car/tracking/out.pickle')))
+    cache = LidarCache(lidar)
+    cache.add_groups_changed_listener(lambda a : print(a))
+    cache.start_cache()
+    time.sleep(1)
+    cache.stop_scanning()