Rework lidar cache to support grpc streaming

pycar/src/car/tracking/lidar_cache.py

@@ -10,7 +10,7 @@ import time
 class LidarCache():
     """
     A class that retrieves scans from the lidar,
-    runs grouping algorithms between scans and 
+    runs grouping algorithms on the scans and 
     keeps a copy of the group data.
     """
 
@@ -56,16 +56,10 @@ class LidarCache():
             else:
                 self.currentGroups = algorithms.calc_groups(scan)
 
-            self.fireGroupsChanged()
+            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))
+    def _fireGroupsChanged(self):
+        pointScan = self.current_scan
 
         for listener in self._group_listeners:
             listener(pointScan)
@@ -81,15 +75,33 @@ class LidarCache():
         listener
             An function that takes a PointScan proto object as its argument.
         """
-        self._group_listeners.append(listener)
+        if(listener not in self._group_listeners):
+            self._group_listeners.append(listener)
+
+    def clear_listeners(self):
+        """
+        Clear all group change listeners.
+        """
+        self._group_listeners = []
+
+    @property
+    def current_scan(self):
+        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))
+        return pointScan
 
     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')))
+    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.add_groups_changed_listener(lambda a: print(a))
     cache.start_cache()
     time.sleep(1)
     cache.stop_scanning()

pycar/src/car/tracking/lidar_servicer.py

@@ -10,6 +10,7 @@ from car.messaging import messages
 import car.tracking.algorithms as alg
 import os
 import google.protobuf.empty_pb2 as empty
+import time
 
 
 class LidarServicer(PersonTrackingServicer):
@@ -18,7 +19,6 @@ class LidarServicer(PersonTrackingServicer):
         self._lidar = RecordingLidarDecorator(
             lidar_factory.get_lidar())
         self.cache = LidarCache(self._lidar, measurements=100)
-        self.cache.add_groups_changed_listener(self.onGroupsChanged)
         self._mFactory = None
         self._port = 50052 if 'CAR_ZMQ_PORT' not in os.environ else os.environ[
             'CAR_ZMQ_PORT']
@@ -32,18 +32,22 @@ class LidarServicer(PersonTrackingServicer):
         return empty.Empty()
 
     def stop_tracking(self, request, context):
+        print('Stopping tracking')
         self._should_stream = False
         self.cache.stop_scanning()
+        self.cache.clear_listeners()
         return empty.Empty()
 
     def start_tracking(self, request, context):
         """Starts the lidar cache, streaming on the provided port."""
         self._should_stream = True
+        # Want to rework this, like the sleam streamer/processer rework.
+        self.cache.add_groups_changed_listener(self.zmq_stream_listener)
         self.cache.start_cache()
         return empty.Empty()
 
     def record(self, request, context):
-        # TODO: Fix this to not require 
+        # TODO: Fix this to not require actually running the cache, just recording the lidar.
         if request.value:
             self.cache.start_cache()
         else:
@@ -55,7 +59,17 @@ class LidarServicer(PersonTrackingServicer):
         self._lidar.save_data(request.file)
         return empty.Empty()
 
-    def onGroupsChanged(self, message):
+    def lidar_stream(self, request, context):
+        # Streams the state of the lidar cache.
+        print('Received Lidar gRPC Stream Start Request')
+        self._should_stream = True
+        self.cache.start_cache()
+        sleep_time = request.time_between_messages if request.time_between_messages > 0.01 else 0.01
+        while self._should_stream:
+            time.sleep(sleep_time)
+            yield self.cache.current_scan
+
+    def zmq_stream_listener(self, message):
         if self._mFactory is None:
             # Create the zmq socket in the thread that it will be used, just to be safe.
             self._mFactory = mf.getZmqPubSubStreamer(self._port)
@@ -64,8 +78,10 @@ class LidarServicer(PersonTrackingServicer):
             self._mFactory.send_message_topic(
                 "lidar_map", messages.ProtoMessage(message=message.SerializeToString()))
 
+    def on_groups_changed(self, message):
         if self._tracked_group is not None and self._vehicle is not None:
             # Update vehicle to correctly follow the tracked group.
             # Leave for now, need to work out exactly how this will change.
             # alg.dualServoChange(alg.find_centre())
+            # Put this in a separate module I think, shouldn't control the can autonomously from the servicer.
             pass