From ef83365fd82bd62e17bc47ff9ce080b1ee928ede Mon Sep 17 00:00:00 2001
From: Piv <18462828+Piv200@users.noreply.github.com>
Date: Mon, 9 Mar 2020 22:01:36 +1030
Subject: [PATCH] Add lidar loading utility, fixup main in motorserver.

---
 control/MotorServer.py   | 11 +++++++----
 tracking/lidar_loader.py | 18 ++++++++++++++++++
 2 files changed, 25 insertions(+), 4 deletions(-)
 create mode 100644 tracking/lidar_loader.py

diff --git a/control/MotorServer.py b/control/MotorServer.py
index 68da24c..a9e6977 100755
--- a/control/MotorServer.py
+++ b/control/MotorServer.py
@@ -74,8 +74,11 @@ class MotorServicer(motorService_pb2_grpc.CarControlServicer):
 
         return grpc.ssl_server_credentials([[pvtKeyBytes, pvtCertBytes]])
 
-vehicle = Vehicle() if __name__ == '__main__' else MockVehicle()
-servicer = MotorServicer(vehicle)
 
-service_thread = Thread(target=servicer.start_server)
-service_thread.start()
+if __name__ == '__main__':
+    vehicle = Vehicle()
+    servicer = MotorServicer(vehicle)
+
+    # Can't remember why I do this, is it even needed?
+    service_thread = Thread(target=servicer.start_server)
+    service_thread.start()
diff --git a/tracking/lidar_loader.py b/tracking/lidar_loader.py
new file mode 100644
index 0000000..6e8a867
--- /dev/null
+++ b/tracking/lidar_loader.py
@@ -0,0 +1,18 @@
+from rplidar import RPLidar
+import pickle
+
+
+def get_scans(num_scans, device='/dev/ttyUSB0', measurements_per_scan=100):
+    lidar = RPLidar(device)
+    scans = lidar.iter_scans(measurements_per_scan)
+    return [next(scans) for i in range(0, num_scans)]
+
+
+def save_scans_bytes(scans, filename='out.pickle'):
+    with open(filename, 'wb') as f:
+        pickle.dump(scans, f)
+
+
+def load_scans_bytes_file(filename):
+    with open(filename, 'rb') as f:
+        return pickle.load(f)