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 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 viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 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, exiting gracefully if user closes it if not viz.display(pose[0]/1000., pose[1]/1000., pose[2], mapbytes): 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 # 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 # 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 main(): # Bozo filter for input args if len(argv) < 3: print('Usage: %s <dataset> <npoints>' % argv[0]) print('Example: %s <exp2> 100000' % argv[0]) exit(1) # Grab input args dataset = argv[1] npoints = int(argv[2]) # Load the Lidar scan data from the log file lidarscans, _ = load_data('.', dataset) # Load the map from the PGM file mapbytes, mapsize_pixels = pgm_load('%s.pgm' % dataset) # Map is square mapsize_pixels = mapsize_pixels[0] # Create a Map object from the bytes; call it mymap to avoid name collision with Python's built-in map function mymap = Map(mapsize_pixels, MAP_SIZE_METERS, mapbytes) # Create an empty Scan object with URG04_360 laser parameters scan = Scan(URG04()) # Create a progresss bar to report what we're doing nscans = len(lidarscans) print('Processing %d points for %d scans...' % (npoints, nscans)) progbar = ProgressBar(0, nscans, 80) # Start timing start_sec = time() # Precompute map size in mm mapsize_mm = mapsize_pixels * MAP_SIZE_METERS # Create a random-number generator for points rand = Random() # Loop over scans for scanno in range(nscans): # Update the Scan object with the Lidar scan values scan.update(lidarscans[scanno], HOLE_WIDTH_MM) # Test a lot of points on this new scan and the pre-loaded map for pointno in range(npoints): point = Position(rand.uniform(0,mapsize_mm), rand.uniform(0,mapsize_mm), rand.uniform(-180,180)) distanceScanToMap(mymap, scan, point) # Tame impatience progbar.updateAmount(scanno) stdout.write('\r%s' % str(progbar)) stdout.flush() # Report results elapsed_sec = time() - start_sec totalpoints = nscans * npoints print('\nProcessed %d points in %f seconds = %d points / sec' % \ (totalpoints, elapsed_sec, int(totalpoints / elapsed_sec)))
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)