Make the lidar streamer ready to work with main controller

tracking/lidar_servicer.py

@@ -3,21 +3,25 @@ from tracking.lidar_tracker_pb2_grpc import PersonTrackingServicer
 from tracking.lidar_cache import LidarCache
 from multiprocessing import Process
 import messaging.message_factory as mf
-from rplidar import RPLidar
-from messaging import messages
+import tracking.devices.factory as lidar_factory
 
+from messaging import messages
+import tracking.algorithms as alg
 
 class LidarServicer(PersonTrackingServicer):
 
-    def __init__(self):
+    def __init__(self, vehicle=None):
         # TODO: Put the rplidar creation in a factory or something, to make it possible to test this servicer.
         # Also, it would allow creating the service without the lidar being connected. 
-        self.cache = LidarCache(RPLidar('/dev/ttyUSB0'), measurements=100)
+        self.cache = LidarCache(lidar_factory.get_lidar(), measurements=100)
+        self.cache.add_groups_changed_listener(self)
         self._mFactory = None
         self._port = None
+        self._vehicle = vehicle
+        self._tracked_group = None
 
     def set_tracking_group(self, request, context):
-        pass
+        self._tracked_group = request.value
 
     def stop_tracking(self, request, context):
         self.cache.stop_scanning()
@@ -25,10 +29,16 @@ class LidarServicer(PersonTrackingServicer):
     def start_tracking(self, request, context):
         """Starts the lidar cache, streaming on the provided port."""
         self._port = request.value
-        self.cache.start_cache(self)
+        self.cache.start_cache()
 
     def onGroupsChanged(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)
         self._mFactory.send_message_topic("lidar_map", messages.ProtoMessage(message=message.SerializeToString()))
+
+        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())
+            pass