Add 'car/' from commit 'eee0e8dc445691e600680f4abc77f2814b20b054'

git-subtree-dir: car
git-subtree-mainline: 1d29a5526c
git-subtree-split: eee0e8dc44

car/tracking/lidar_servicer.py

@@ -0,0 +1,44 @@
+import tracking.lidar_tracker_pb2 as lidar_tracker_pb2
+from tracking.lidar_tracker_pb2_grpc import PersonTrackingServicer
+from tracking.lidar_cache import LidarCache
+from multiprocessing import Process
+import messaging.message_factory as mf
+import tracking.devices.factory as lidar_factory
+
+from messaging import messages
+import tracking.algorithms as alg
+
+class LidarServicer(PersonTrackingServicer):
+
+    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(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):
+        self._tracked_group = request.value
+
+    def stop_tracking(self, request, context):
+        self.cache.stop_scanning()
+
+    def start_tracking(self, request, context):
+        """Starts the lidar cache, streaming on the provided port."""
+        self._port = request.value
+        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