Initialise lidar twice, print the actual mapbytes, don't ignore first scan

SlamController/slam_streamer.py

@@ -39,7 +39,8 @@ class SlamStreamer:
         print('Started and bound zmq socket.')
 
         # Adapted from BreezySLAM rpslam example.
-        # Connect to Lidar unit
+        # Connect to Lidar unit. For some reason it likes to be done twice, otherwise it errors out...
+        lidar = Lidar(self._lidar_connection)
         lidar = Lidar(self._lidar_connection)
 
         print('Initialised lidar')
@@ -59,11 +60,6 @@ class SlamStreamer:
 
         print('Scanning')
 
-        # First scan is crap, so ignore it
-        next(iterator)
-
-        print('Going to next scan')
-
         while self.can_scan:
             # Extract (quality, angle, distance) triples from current scan
             items = [item for item in next(iterator)]
@@ -76,7 +72,7 @@ class SlamStreamer:
             slam.update(distances, scan_angles_degrees=angles)
             print('Map updated')
             slam.getmap(mapbytes)
-            
+            print(mapbytes)
             self._push_map(mapbytes, slam.getpos())
 
     def _push_map(self, mapbytes, location):