def main():
# Bozo filter for input args
if len(argv) < 4:
print('Usage: %s <dataset> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
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)
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
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)
# Set up a SLAM display, named by dataset
display = SlamShow(MAP_SIZE_PIXELS,
MAP_SIZE_METERS * 1000 / MAP_SIZE_PIXELS, dataset)
# Pose will be modified in our threaded code
pose = [0, 0, 0]
# Launch the data-collection / update thread
thread = Thread(target=threadfunc,
args=(robot, slam, timestamps, lidars,
odometries if use_odometry else None, mapbytes,
pose))
thread.daemon = True
thread.start()
# Loop forever,displaying current map and pose
while True:
# Display map and robot pose
display.displayMap(mapbytes)
display.setPose(*pose)
# Refresh the display, exiting gracefully if user closes it
if not display.refresh():
exit(0)
def __init__(self, size_pixels, size_meters):
self.map_size_pixels = size_pixels
self.map_size_meters = size_meters
self.map_scale_meters_per_pixel = float(size_meters) / float(
size_pixels)
self.mapbytes = bytearray(self.map_size_pixels * self.map_size_pixels)
self.robot = None
self.control = False
self.lidarShow = True
self.routine = False
#slam算法初始化
self.slam = RMHC_SLAM(MinesLaser(),
self.map_size_pixels,
self.map_size_meters,
map_quality=_DEFAULT_MAP_QUALITY,
hole_width_mm=_DEFAULT_HOLE_WIDTH_MM,
random_seed=999,
sigma_xy_mm=_DEFAULT_SIGMA_XY_MM,
sigma_theta_degrees=_DEFAULT_SIGMA_THETA_DEGREES,
max_search_iter=_DEFAULT_MAX_SEARCH_ITER)
self.map_view = OpenMap(self.map_size_pixels, self.map_size_meters)
self.pose = [0, 0, 0]
def main():
# Bozo filter for input args
if len(argv) < 3:
print('Usage: %s <dataset> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0
# Load the data from the file, ignoring timestamps
_, lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
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)
# Report what we're doing
nscans = len(lidars)
print('Processing %d scans with%s odometry / with%s particle filter...' % \
(nscans, \
'' if use_odometry else 'out', '' if seed else 'out'))
progbar = ProgressBar(0, nscans, 80)
# Start with an empty trajectory of positions
trajectory = []
# Start timing
start_sec = time()
# Loop over scans
for scanno in range(nscans):
if use_odometry:
# Convert odometry to velocities
velocities = robot.computePoseChange(odometries[scanno])
# Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocities)
else:
# Update SLAM with lidar alone
slam.update(lidars[scanno])
# Get new position
x_mm, y_mm, theta_degrees = slam.getpos()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
# Tame impatience
progbar.updateAmount(scanno)
stdout.write('\r%s' % str(progbar))
stdout.flush()
# Report elapsed time
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' %
(nscans, elapsed_sec, nscans / elapsed_sec))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get final map
slam.getmap(mapbytes)
# Pickle the map to a file
pklname = dataset + '.map'
print('Writing map to file ' + pklname)
pickle.dump(mapbytes, open(pklname, 'wb'))
def main():
# Bozo filter for input args
if len(argv) < 3:
print('Usage: %s <dataset> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
lidarobj = Laser(360, 12, 360, 8000)
# Create a CoreSLAM object with laser params and robot object
slam = RMHC_SLAM(lidarobj, MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Start with an empty trajectory of positions
trajectory = []
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
suffix = 1
while (True):
if (use_odometry):
mutex.acquire()
mainLCMQ = lcmQ
mainODOQ = odoQ
# Clear Queues once copied from thread into main for next batch of data
lcmQ.queue.clear()
odoQ.queue.clear()
mutex.release()
velocities = robot.computePoseChange(mainODOQ.get())
slam.update(mainLCMQ.get(), velocities)
x_mm, y_mm, theta_degrees = slam.getpos()
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
trajectory.append((y_pix, x_pix))
slam.getmap(mapbytes)
trajLen = len(trajectory)
for i in range(trajLen):
if (i == (trajLen - 1)):
mapbytes[trajectory[i][0] * MAP_SIZE_PIXELS +
trajectory[i][1]] = 0
else:
mapbytes[trajectory[i][0] * MAP_SIZE_PIXELS +
trajectory[i][1]] = 120
filename = dataset + str(suffix)
pgm_save('%s.pgm' % filename, mapbytes,
(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
suffix += 1
if (keyPressed == 's'):
#Wrap up last map using leftover data
pgm_save('%s.pgm' % filename, mapbytes,
(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
'''
This will take all the maps generated and place them into pgmbagfolder
For this to work, make sure your destination directory has a folder called pgmbagfolder
Change the directory:
/home/Shaurya98/rplidar_workspace/src/mapping/BreezySLAM/examples
and
/home/Shaurya98/rplidar_workspace/src/mapping/BreezySLAM/examples/pgmbagfolder
With your own destination directory. It it recommended to put pgmbagfolder under the examples
directory
'''
os.chdir(
"/home/pi/rplidar_workspace/src/mapping/BreezySLAM/examples/pgmbagfolder"
)
for pgm_file in glob.iglob('*.pgm'):
os.remove(pgm_file)
print("\nEmptied pgmbagfolder")
os.chdir(
"/home/pi/rplidar_workspace/src/mapping/BreezySLAM/examples"
)
for pgm_file in glob.iglob('*.pgm'):
shutil.copy2(
pgm_file,
"/home/pi/rplidar_workspace/src/mapping/BreezySLAM/examples/pgmbagfolder"
)
os.remove(pgm_file)
print("\nFiles recorded and sent to pgmbagfolder")
#Terminate threads before exiting main()
thread1.join()
thread2.join()
thread3.join()
break
def main():
# Bozo filter for input args
if len(argv) < 3:
print('Usage: %s <dataset> <use_odometry> <random_seed>' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0
# Load the data from the file
lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
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)
# Report what we're doing
nscans = len(lidars)
print('Processing %d scans with%s odometry / with%s particle filter...' % \
(nscans, \
'' if use_odometry else 'out', '' if seed else 'out'))
progbar = ProgressBar(0, nscans, 80)
# Start with an empty trajectory of positions
trajectory = []
# Start timing
start_sec = time()
# Loop over scans
for scanno in range(nscans):
if use_odometry:
# Convert odometry to velocities
velocities = robot.computeVelocities(odometries[scanno])
# Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocities)
else:
# Update SLAM with lidar alone
slam.update(lidars[scanno])
# Get new position
x_mm, y_mm, theta_degrees = slam.getpos()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
# Tame impatience
progbar.updateAmount(scanno)
stdout.write('\r%s' % str(progbar))
stdout.flush()
# Report elapsed time
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' %
(nscans, elapsed_sec, nscans / elapsed_sec))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get final map
slam.getmap(mapbytes)
# Put trajectory into map as black pixels
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0
# Save map and trajectory as PNG file
image = Image.frombuffer('L', (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS), mapbytes,
'raw', 'L', 0, 1)
image.save('%s.png' % dataset)