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

Added CoreSLAM::setmap() method; cleaned up scripts

Browse Source
This commit is contained in:
simondlevy
2017-11-12 17:33:32 -05:00
parent 3ff0ab8bc4
commit f9341fbe3a
5 changed files with 27 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/log2pgm.py
View File

@@ -56,7 +56,7 @@ def main():
# Bozo filter for input args # Bozo filter for input args
if len(argv) < 3: if len(argv) < 3:
print('Usage: %s <dataset> <use_odometry> <random_seed>' % argv[0]) print('Usage: %s <dataset> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 1 9999' % argv[0]) print('Example: %s exp2 1 9999' % argv[0])
exit(1) exit(1)

2
examples/log2pkl.py
View File

@@ -38,7 +38,7 @@ def main():
# Bozo filter for input args # Bozo filter for input args
if len(argv) < 3: if len(argv) < 3:
print('Usage: %s <dataset> <use_odometry> <random_seed>' % argv[0]) print('Usage: %s <dataset> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 1 9999' % argv[0]) print('Example: %s exp2 1 9999' % argv[0])
exit(1) exit(1)

2
examples/log2png.py
View File

@@ -57,7 +57,7 @@ def main():
# Bozo filter for input args # Bozo filter for input args
if len(argv) < 3: if len(argv) < 3:
print('Usage: %s <dataset> <use_odometry> <random_seed>' % argv[0]) print('Usage: %s <dataset> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 1 9999' % argv[0]) print('Example: %s exp2 1 9999' % argv[0])
exit(1) exit(1)

13
examples/logmovie.py
View File

@@ -43,8 +43,9 @@ from mines import MinesLaser, Rover, load_data
from pltslamshow import SlamShow from pltslamshow import SlamShow
from sys import argv, exit from sys import argv, exit
from time import time, sleep from time import sleep
from threading import Thread from threading import Thread
import pickle
def threadfunc(robot, slam, timestamps, lidars, odometries, use_odometry, mapbytes, pose): 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(): def main():
# Bozo filter for input args # Bozo filter for input args
if len(argv) < 3: if len(argv) < 4:
print('Usage: %s <dataset> <use_odometry> <random_seed>' % argv[0]) print('Usage: %s <dataset> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 1 9999' % argv[0]) print('Example: %s exp2 1 9999' % argv[0])
exit(1) exit(1)
@@ -95,6 +96,9 @@ def main():
dataset = argv[1] dataset = argv[1]
use_odometry = True if int(argv[2]) else False use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0 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 # Load the data from the file
timestamps, lidars, odometries = load_data('.', dataset) 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) \ slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \ if seed \
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) 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 # Set up a SLAM display, named by dataset
display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, dataset) display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, dataset)

27
python/breezyslam/algorithms.py
View File

@@ -57,9 +57,9 @@ class CoreSLAM(object):
Implementing classes should provide the method 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, def __init__(self, laser, map_size_pixels, map_size_meters,
@@ -90,7 +90,7 @@ class CoreSLAM(object):
# Initialize the map # Initialize the map
self.map = pybreezyslam.Map(map_size_pixels, map_size_meters) 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 Updates the scan and odometry, and calls the the implementing class's _updateMapAndPointcloud method with
the specified velocities. 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 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 attribute of the Laser object passed to the CoreSlam constructor
velocities is a tuple of velocities (dxy_mm, dtheta_degrees, dt_seconds) for odometry 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 # 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) self.velocities = (new_dxy_mm, new_dtheta_degrees, 0)
# Implementing class updates map and pointcloud # Implementing class updates map and pointcloud
self._updateMapAndPointcloud(velocities, shouldUpdateMap) self._updateMapAndPointcloud(velocities, should_update_map)
def getmap(self, mapbytes): 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__(). to CoreSLAM.__init__().
''' '''
self.map.get(mapbytes) 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): def __str__(self):
return 'CoreSLAM: %s \n map quality = %d / 255 \n hole width = %7.0f mm' % \ 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 init_coord_mm = 500 * map_size_meters # center of map
self.position = pybreezyslam.Position(init_coord_mm, init_coord_mm, 0) 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() 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). 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() self.position.y_mm -= self.laser.offset_mm * self._sintheta()
# Update the map with this new position if indicated # 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) self.map.update(self.scan_for_mapbuild, new_position, self.map_quality, self.hole_width_mm)
def getpos(self): def getpos(self):
@@ -247,13 +254,13 @@ class RMHC_SLAM(SinglePositionSLAM):
self.sigma_theta_degrees = sigma_theta_degrees self.sigma_theta_degrees = sigma_theta_degrees
self.max_search_iter = max_search_iter 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: if not velocities:
velocities = (0, 0, 0) 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): def _getNewPosition(self, start_position):
''' '''