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.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 PGM file
pgm_save('%s.pgm' % dataset, mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
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(URG04(), 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 PGM file
pgm_save('%s.pgm' % dataset, mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
def slamThread():
global SLAMrot
global SLAMvel
# Connect to Lidar unit
lidar = Lidar(LIDAR_DEVICE)
# Create an RMHC SLAM object with a laser model and optional robot model
slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize an empty trajectory
trajectory = []
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Create an iterator to collect scan data from the RPLidar
iterator = lidar.iter_scans()
# We will use these to store previous scan in case current scan is inadequate
previous_distances = None
previous_angles = None
# First scan is crap, so ignore it
next(iterator)
# start time
start_time = time.time()
prevTime = start_time
print("start")
while True:
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
slam.update(distances, pose_change = ( (SLAMvel, SLAMrot, time.time() - prevTime)),scan_angles_degrees=angles)
prevTime = time.time()
previous_distances = copy.copy(distances)
previous_angles = copy.copy(angles)
print("updated - if")
print((SLAMvel, SLAMrot, time.time() - prevTime))
# If not adequate, use previous
elif previous_distances is not None:
slam.update(previous_distances, pose_change = ( (SLAMvel, SLAMrot, time.time() - prevTime)),scan_angles_degrees=previous_angles)
prevTime = time.time()
print("updated - else")
print((SLAMvel, SLAMrot, time.time() - prevTime))
# Get current robot position
x, y, theta = slam.getpos()
# Add new position to trajectory
trajectory.append((x, y))
# Get current map bytes as grayscale
slam.getmap(mapbytes)
if(time.time() - start_time > 30):
# 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;
pgm_save('ok.pgm', mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
exit(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
# 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
prevTime = time.time()
previous_distances = copy.copy(distances)
previous_angles = copy.copy(angles)
print("updated - if")
# If not adequate, use previous
elif previous_distances is not None:
slam.update(previous_distances,
pose_change=((40, 0, time.time() - prevTime)),
scan_angles_degrees=previous_angles)
prevTime = time.time()
print("updated - else")
# Get current robot position
x, y, theta = slam.getpos()
# Get current map bytes as grayscale
slam.getmap(mapbytes)
if (time.time() - start_time > 5):
pgm_save('ok.pgm', mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
exit(0)
# Display map and robot pose, exiting gracefully if user closes it
# if not viz.display(x/1000., y/1000., theta, mapbytes):
# exit(0)
# Shut down the lidar connection
lidar.stop()
lidar.disconnect()
def main():
# Grab input args
dataset = "testLidar"
use_odometry = True
seed = 0
# Load the data from the file, ignoring timestamps
lidars, velocities = load_data('.', dataset)
# Build a robot model if we want odometry
robot = RoboPorter() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
slam = RMHC_SLAM(roboPorterLaser(),
MAP_SIZE_PIXELS,
MAP_SIZE_METERS,
random_seed=seed) # \
#if seed \
#else Deterministic_SLAM(roboPorterLaser(), 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):
print(len(lidars[scanno]))
# Convert odometry to velocities
velocitity = velocities[
scanno] #dxyMillimeters, dthetaDegrees, dtSeconds
# Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocitity)
# 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 PGM file
pgm_save('%s.pgm' % dataset, mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
for chunk_row in range(num_chunks):
for chunk_col in range(num_chunks):
# begin current chunk
sum = 0
for sub_row in range(CHUNK_SIZE):
for sub_col in range(CHUNK_SIZE):
sum += bytemap[(chunk_row * CHUNK_SIZE + sub_row) * MAP_SIZE +
(chunk_col * CHUNK_SIZE + sub_col)]
avg = sum // (CHUNK_SIZE * CHUNK_SIZE)
if (avg < 127):
avg = 0
compressed_map[(chunk_row * num_chunks) + chunk_col] = avg
compressed_map_2d[chunk_row][chunk_col] = avg
# approx middle pixel
compressed_map[(SEARCH_ROW) * 80 + SEARCH_COL] = 0
# raw distance search from given point
outuple = search()
if (outuple is not None):
(dest_row, dest_col) = outuple
print(dest_row, dest_col)
compressed_map[dest_row * num_chunks + dest_col] = 0
else:
print("None found")
#print(compressed_map)
pgm_save('../resources/compressed_map.pgm', compressed_map,
(num_chunks, num_chunks))