From f9341fbe3a0bbee7987bda3afff6b60a8e694549 Mon Sep 17 00:00:00 2001 From: simondlevy Date: Sun, 12 Nov 2017 17:33:32 -0500 Subject: [PATCH] Added CoreSLAM::setmap() method; cleaned up scripts --- examples/log2pgm.py | 2 +- examples/log2pkl.py | 2 +- examples/log2png.py | 2 +- examples/logmovie.py | 13 +++++++------ python/breezyslam/algorithms.py | 27 +++++++++++++++++---------- 5 files changed, 27 insertions(+), 19 deletions(-) diff --git a/examples/log2pgm.py b/examples/log2pgm.py index 6638327..172b665 100755 --- a/examples/log2pgm.py +++ b/examples/log2pgm.py @@ -56,7 +56,7 @@ def main(): # Bozo filter for input args if len(argv) < 3: - print('Usage: %s ' % argv[0]) + print('Usage: %s [random_seed]' % argv[0]) print('Example: %s exp2 1 9999' % argv[0]) exit(1) diff --git a/examples/log2pkl.py b/examples/log2pkl.py index e2d6200..810b692 100755 --- a/examples/log2pkl.py +++ b/examples/log2pkl.py @@ -38,7 +38,7 @@ def main(): # Bozo filter for input args if len(argv) < 3: - print('Usage: %s ' % argv[0]) + print('Usage: %s [random_seed]' % argv[0]) print('Example: %s exp2 1 9999' % argv[0]) exit(1) diff --git a/examples/log2png.py b/examples/log2png.py index c9f0058..52e2704 100755 --- a/examples/log2png.py +++ b/examples/log2png.py @@ -57,7 +57,7 @@ def main(): # Bozo filter for input args if len(argv) < 3: - print('Usage: %s ' % argv[0]) + print('Usage: %s [random_seed]' % argv[0]) print('Example: %s exp2 1 9999' % argv[0]) exit(1) diff --git a/examples/logmovie.py b/examples/logmovie.py index 31dd9a1..b546b5d 100755 --- a/examples/logmovie.py +++ b/examples/logmovie.py @@ -43,8 +43,9 @@ from mines import MinesLaser, Rover, load_data from pltslamshow import SlamShow from sys import argv, exit -from time import time, sleep +from time import sleep from threading import Thread +import pickle def threadfunc(robot, slam, timestamps, lidars, odometries, use_odometry, mapbytes, pose): ''' @@ -86,8 +87,8 @@ def threadfunc(robot, slam, timestamps, lidars, odometries, use_odometry, mapbyt def main(): # Bozo filter for input args - if len(argv) < 3: - print('Usage: %s ' % argv[0]) + if len(argv) < 4: + print('Usage: %s [random_seed]' % argv[0]) print('Example: %s exp2 1 9999' % argv[0]) exit(1) @@ -95,6 +96,9 @@ def main(): dataset = argv[1] use_odometry = True if int(argv[2]) else False seed = int(argv[3]) if len(argv) > 3 else 0 + + # Allocate byte array to receive map updates + mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) # Load the data from the file timestamps, lidars, odometries = load_data('.', dataset) @@ -106,9 +110,6 @@ def main(): slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \ if seed \ else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) - - # Create a byte array to receive the computed maps - mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) # Set up a SLAM display, named by dataset display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, dataset) diff --git a/python/breezyslam/algorithms.py b/python/breezyslam/algorithms.py index f513229..81db3fa 100644 --- a/python/breezyslam/algorithms.py +++ b/python/breezyslam/algorithms.py @@ -57,9 +57,9 @@ class CoreSLAM(object): Implementing classes should provide the method - _updateMapAndPointcloud(scan_mm, velocities, shouldUpdateMap) + _updateMapAndPointcloud(scan_mm, velocities, should_update_map) - to update the point-cloud (particle cloud) and map (if shouldUpdateMap true) + to update the point-cloud (particle cloud) and map (if should_update_map true) ''' def __init__(self, laser, map_size_pixels, map_size_meters, @@ -90,7 +90,7 @@ class CoreSLAM(object): # Initialize the map self.map = pybreezyslam.Map(map_size_pixels, map_size_meters) - def update(self, scans_mm, velocities, shouldUpdateMap=True): + def update(self, scans_mm, velocities, should_update_map=True): ''' Updates the scan and odometry, and calls the the implementing class's _updateMapAndPointcloud method with the specified velocities. @@ -98,7 +98,7 @@ class CoreSLAM(object): scan_mm is a list of Lidar scan values, whose count is specified in the scan_size attribute of the Laser object passed to the CoreSlam constructor velocities is a tuple of velocities (dxy_mm, dtheta_degrees, dt_seconds) for odometry - shouldUpdateMap flags for whether you want to update the map + should_update_map flags for whether you want to update the map ''' # Build a scan for computing distance to map, and one for updating map @@ -112,16 +112,23 @@ class CoreSLAM(object): self.velocities = (new_dxy_mm, new_dtheta_degrees, 0) # Implementing class updates map and pointcloud - self._updateMapAndPointcloud(velocities, shouldUpdateMap) + self._updateMapAndPointcloud(velocities, should_update_map) def getmap(self, mapbytes): ''' - Fills bytearray mapbytes with map pixels, where bytearray length is square of map size passed + Fills bytearray mapbytes with current map pixels, where bytearray length is square of map size passed to CoreSLAM.__init__(). ''' self.map.get(mapbytes) + def setmap(self, mapbytes): + ''' + Sets current map pixels to values in bytearray, where bytearray length is square of map size passed + to CoreSLAM.__init__(). + ''' + self.map.set(mapbytes) + def __str__(self): return 'CoreSLAM: %s \n map quality = %d / 255 \n hole width = %7.0f mm' % \ @@ -159,7 +166,7 @@ class SinglePositionSLAM(CoreSLAM): init_coord_mm = 500 * map_size_meters # center of map self.position = pybreezyslam.Position(init_coord_mm, init_coord_mm, 0) - def _updateMapAndPointcloud(self, velocities, shouldUpdateMap): + def _updateMapAndPointcloud(self, velocities, should_update_map): ''' Updates the map and point-cloud (particle cloud). Called automatically by CoreSLAM.update() velocities is a tuple of the form (dxy_mm, dtheta_degrees, dt_seconds). @@ -186,7 +193,7 @@ class SinglePositionSLAM(CoreSLAM): self.position.y_mm -= self.laser.offset_mm * self._sintheta() # Update the map with this new position if indicated - if shouldUpdateMap: + if should_update_map: self.map.update(self.scan_for_mapbuild, new_position, self.map_quality, self.hole_width_mm) def getpos(self): @@ -247,13 +254,13 @@ class RMHC_SLAM(SinglePositionSLAM): self.sigma_theta_degrees = sigma_theta_degrees self.max_search_iter = max_search_iter - def update(self, scan_mm, velocities=None, shouldUpdateMap=True): + def update(self, scan_mm, velocities=None, should_update_map=True): if not velocities: velocities = (0, 0, 0) - CoreSLAM.update(self, scan_mm, velocities, shouldUpdateMap) + CoreSLAM.update(self, scan_mm, velocities, should_update_map) def _getNewPosition(self, start_position): '''