vato007/picar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Piv
2020-02-17 22:22:56 +10:30
parent 2e0d8ad345
commit 8d4007dab2
1 changed files with 3 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
SlamController/slam_streamer.py
View File

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