diff --git a/SlamController/slam_streamer.py b/SlamController/slam_streamer.py index 0e9d539..b444d3a 100644 --- a/SlamController/slam_streamer.py +++ b/SlamController/slam_streamer.py @@ -31,29 +31,39 @@ class SlamStreamer: to calling this method, and changing those values after calling this method will have no effect. ''' - + self.can_scan = True print('Starting to stream') self._socket = self._start_socket( self._create_socket(self._zmq_context), self._port) - print('Started and bound zmq socket.') + print('Started and bound zmq socket.' # Adapted from BreezySLAM rpslam example. # Connect to Lidar unit lidar = Lidar(self._lidar_connection) + print('Initialised lidar') + # Create an RMHC SLAM object with a laser model and optional robot model slam = RMHC_SLAM(LaserModel(), self._map_pixels, self._map_meters) + print('initialised slam') + # Initialize empty map mapbytes = bytearray(self.map_pixels * self.map_pixels) + + print('Initialised byte []') # Create an iterator to collect scan data from the RPLidar iterator = lidar.iter_scans() + 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)]