From 4d511ca91b5c7fe5ae3b83496150de20350ac73a Mon Sep 17 00:00:00 2001
From: Piv <18462828+Piv200@users.noreply.github.com>
Date: Sun, 24 May 2020 22:48:13 +0930
Subject: [PATCH] Add some running code for lidar cache, test stubs, fix proto.

---
 car/src/car/tracking/algorithms.py            |  2 +-
 car/src/car/tracking/lidar_cache.py           | 15 +++++++--
 car/src/car/tracking/lidar_servicer.py        |  2 +-
 car/tests/test_algorithms.py                  | 33 +++++++++++++++++++
 car/tests/test_lidar_cache.py                 | 10 ++++++
 .../proto/car/tracking/lidar_tracker.proto    |  2 +-
 6 files changed, 59 insertions(+), 5 deletions(-)
 create mode 100644 car/tests/test_algorithms.py
 create mode 100644 car/tests/test_lidar_cache.py

diff --git a/car/src/car/tracking/algorithms.py b/car/src/car/tracking/algorithms.py
index f61741c..8f4df7a 100644
--- a/car/src/car/tracking/algorithms.py
+++ b/car/src/car/tracking/algorithms.py
@@ -28,7 +28,7 @@ class Group:
 
     def _update_min_max(self, new_point):
         """
-        Updates the in and max points for this group.
+        Updates the min and max points for this group.
         This is to determine when assigning groups whether the
         same group is selected.
         """
diff --git a/car/src/car/tracking/lidar_cache.py b/car/src/car/tracking/lidar_cache.py
index 40eb789..f7551b3 100644
--- a/car/src/car/tracking/lidar_cache.py
+++ b/car/src/car/tracking/lidar_cache.py
@@ -2,6 +2,9 @@ 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():
@@ -65,7 +68,7 @@ class LidarCache():
                     angle=point[1], distance=point[2], group_number=group.number))
 
         for listener in self._group_listeners:
-            listener.onGroupsChanged(pointScan)
+            listener(pointScan)
 
     def add_groups_changed_listener(self, listener):
         """
@@ -76,9 +79,17 @@ class LidarCache():
         Parameters
         ----------
         listener
-            An object that implements the onGroupsChanged(message) method. 
+            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()
diff --git a/car/src/car/tracking/lidar_servicer.py b/car/src/car/tracking/lidar_servicer.py
index 9038428..bb41106 100644
--- a/car/src/car/tracking/lidar_servicer.py
+++ b/car/src/car/tracking/lidar_servicer.py
@@ -18,7 +18,7 @@ class LidarServicer(PersonTrackingServicer):
         self._lidar = RecordingLidarDecorator(
             lidar_factory.get_lidar())
         self.cache = LidarCache(self._lidar, measurements=100)
-        self.cache.add_groups_changed_listener(self)
+        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']
diff --git a/car/tests/test_algorithms.py b/car/tests/test_algorithms.py
new file mode 100644
index 0000000..0cf697a
--- /dev/null
+++ b/car/tests/test_algorithms.py
@@ -0,0 +1,33 @@
+import unittest
+import car.tracking as tracking
+from car.tracking.devices.mock_lidar import MockLidar
+import car.tracking.lidar_loader as loader
+
+class TestAlgorithms(unittest.TestCase):
+
+    def setUp(self):
+        self.lidar = MockLidar(iter(loader.get_scans('../src/car/tracking/out.pickle')))
+
+    def test_find_centre(self):
+        # e.g.
+        #self.assertEqual
+        pass
+
+    def test_convert_lidar_cartesian(self):
+        pass
+
+    def test_convert_cartesian_lidar(self):
+        pass
+
+    def test_calc_groups_runs(self):
+        pass
+
+    def test_group_changing(self):
+        pass
+
+    def tearDown(self):
+        pass
+
+
+if __name__ == '__main__':
+    unittest.main()
\ No newline at end of file
diff --git a/car/tests/test_lidar_cache.py b/car/tests/test_lidar_cache.py
new file mode 100644
index 0000000..7e782b8
--- /dev/null
+++ b/car/tests/test_lidar_cache.py
@@ -0,0 +1,10 @@
+import unittest
+import car.tracking as tracking
+
+class TestLidarCache(unittest.TestCase):
+    
+    def test_fake_scanner(self):
+        pass
+
+if __name__ == '__main__':
+    unittest.main()
\ No newline at end of file
diff --git a/protobuf/src/main/proto/car/tracking/lidar_tracker.proto b/protobuf/src/main/proto/car/tracking/lidar_tracker.proto
index b988413..860211f 100644
--- a/protobuf/src/main/proto/car/tracking/lidar_tracker.proto
+++ b/protobuf/src/main/proto/car/tracking/lidar_tracker.proto
@@ -16,7 +16,7 @@ message Int32Value{
 
 message Point{
     double angle = 1;
-    int32 distance = 2;
+    double distance = 2;
     int32 group_number = 3;
 }