From b10f7eaf55009eed4d8a5a82e82b0a6a4e53d329 Mon Sep 17 00:00:00 2001
From: Piv <18462828+Piv200@users.noreply.github.com>
Date: Thu, 19 Mar 2020 20:23:04 +1030
Subject: [PATCH] Prepare to test lidar tracker.

---
 control/MotorServer.py  |  7 +++++++
 tracking/lidar_cache.py | 19 +++++++++++++++----
 2 files changed, 22 insertions(+), 4 deletions(-)

diff --git a/control/MotorServer.py b/control/MotorServer.py
index a9e6977..8406e53 100755
--- a/control/MotorServer.py
+++ b/control/MotorServer.py
@@ -13,6 +13,8 @@ from control.gpiozero.vehicle import Vehicle
 from control.gpiozero.mockvehicle import MockVehicle
 from slam.slam_servicer import SlamServicer
 import slam.SlamController_pb2_grpc as SlamController_pb2_grpc
+import tracking.lidar_tracker_pb2_grpc as lidar_tracker_pb2_grpc
+from tracking.lidar_servicer import LidarServicer
 
 
 class MotorServicer(motorService_pb2_grpc.CarControlServicer):
@@ -54,6 +56,8 @@ class MotorServicer(motorService_pb2_grpc.CarControlServicer):
         motorService_pb2_grpc.add_CarControlServicer_to_server(self, server)
         SlamController_pb2_grpc.add_SlamControlServicer_to_server(
             self.create_slam_servicer(), server)
+        lidar_tracker_pb2_grpc.add_PersonTrackingServicer_to_server(
+            self.create_lidar_servicer(), server)
         # Disable tls for local testing.
         # server.add_secure_port('[::]:50051', self.create_credentials())
         server.add_insecure_port('[::]:50051')
@@ -64,6 +68,9 @@ class MotorServicer(motorService_pb2_grpc.CarControlServicer):
     def create_slam_servicer(self):
         return SlamServicer('/dev/ttyUSB0')
 
+    def create_lidar_servicer(self):
+        return LidarServicer()
+
     def create_credentials(self):
         # Relativise this stuff.
         pvtKeyPath = '/home/pi/tls/device.key'
diff --git a/tracking/lidar_cache.py b/tracking/lidar_cache.py
index 800ba11..441f3be 100644
--- a/tracking/lidar_cache.py
+++ b/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