Use multiprocessing for slam, add more debug prints

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()

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):