From 65d5e94c0dc1afb45f6d822fd272d6bb7ba09835 Mon Sep 17 00:00:00 2001
From: Piv <18462828+Piv200@users.noreply.github.com>
Date: Wed, 12 Feb 2020 22:03:25 +1030
Subject: [PATCH] Use multiprocessing for slam,  add more debug prints

---
 SlamController/slam_servicer.py | 5 +++--
 SlamController/slam_streamer.py | 6 ++++--
 2 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/SlamController/slam_servicer.py b/SlamController/slam_servicer.py
index 3f8b790..f76a114 100644
--- a/SlamController/slam_servicer.py
+++ b/SlamController/slam_servicer.py
@@ -1,7 +1,7 @@
 from . import SlamController_pb2_grpc
 from . import SlamController_pb2
 from . import slam_streamer as slam
-from threading import Thread
+from multiprocessing import Process
 
 
 class SlamServicer(SlamController_pb2_grpc.SlamControlServicer):
@@ -19,12 +19,13 @@ class SlamServicer(SlamController_pb2_grpc.SlamControlServicer):
             self.slam.port = request.port
             self.slam.map_pixels = request.map_size_pixels
             self.slam.map_meters = request.map_size_meters
-            self.slam_thread = Thread(target=self.slam.start)
+            self.slam_thread = Process(target=self.slam.start)
             self.slam_thread.start()
         return SlamController_pb2.Empty()
 
     def stop_streaming(self, request, context):
         if self.slam_thread is not None:
             self.slam.stop_scanning()
+            self.slam_thread.join()
             self.slam = None
         return SlamController_pb2.Empty()
diff --git a/SlamController/slam_streamer.py b/SlamController/slam_streamer.py
index ed80ffe..64f1a77 100644
--- a/SlamController/slam_streamer.py
+++ b/SlamController/slam_streamer.py
@@ -32,6 +32,7 @@ class SlamStreamer:
         calling this method will have no effect.
         '''
 
+        print('Starting to stream')
         self._socket = self._start_socket(self._create_socket(self._zmq_context), self._port)
         
         # Block until user opens zmq.
@@ -60,10 +61,10 @@ class SlamStreamer:
             # Extract distances and angles from triples
             distances = [item[2] for item in items]
             angles    = [item[1] for item in items]
-
+            print('Updating map')
             # Update SLAM with current Lidar scan and scan angles
             slam.update(distances, scan_angles_degrees=angles)
-
+            print('Map updated')
             self._push_map(slam.getmap(mapbytes), slam.getpos())
 
     def _push_map(self, mapbytes, location):
@@ -74,6 +75,7 @@ class SlamStreamer:
         ''' 
         protoScan = SlamScan(map = bytes(mapbytes), \
             location = SlamLocation(x = location[0], y = location[1], theta = location[3]))
+        print('Sending map')
         self._socket.send(protoScan.SerializeToString())
 
     def stop_scanning(self):