From 8d4007dab268fbb86c826b334a8be7c5d3e3adde Mon Sep 17 00:00:00 2001
From: Piv <18462828+Piv200@users.noreply.github.com>
Date: Mon, 17 Feb 2020 22:22:56 +1030
Subject: [PATCH] Initialise lidar twice, print the actual mapbytes, don't
 ignore first scan

---
 SlamController/slam_streamer.py | 10 +++-------
 1 file changed, 3 insertions(+), 7 deletions(-)

diff --git a/SlamController/slam_streamer.py b/SlamController/slam_streamer.py
index ec27213..7abd3fe 100644
--- a/SlamController/slam_streamer.py
+++ b/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):