run-headless

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import sys
import time
import cPickle as pickle
import numpy as N
N.random.seed(0x5EABED)
import pylab as P
import rangeonly
import rangebearing
import plot
from data import ds1, ds1_beacons

# Generators to provide data to the particle filter in the correct format
def range_state_generator(data):
  for row in data:
    yield row['dt'], N.array([row['v'], row['w']]), row['z'][0,:].T
def rangebearing_state_generator(data):
  for row in data:
    yield row['dt'], N.array([row['v'], row['w']]), row['z'].T

def main(run_rangeonly=True):
    
  # Choose the correct SLAM class and generator -- rangeonly or rangebearing?
  if run_rangeonly:
    state_generator = range_state_generator(ds1)
    FastSLAMClass = rangeonly.FastSLAM
  else:
    state_generator = rangebearing_state_generator(ds1)
    FastSLAMClass = rangebearing.FastSLAM

  # Create the SLAM instance, and init. particles so output lines up in plots
  robot = FastSLAMClass(state_generator, 45)
  robot.setall(N.array([0, 0, N.pi/2.]))

  # Now step the SLAM instance, and plot each frame
  for i, particles in enumerate(robot.run()):
    # Provide an indication of progress
    print "%d      \r" % i, ; sys.stdout.flush()

  ### Write out final data ###
  pickled = open('%s.pkl' % ("ro" if run_rangeonly else "rb"), 'wb')
  pickle.dump(particles, pickled, -1)
  pickled.close()

if __name__ == '__main__':
  if len(sys.argv) < 2:
    print """Usage: %s ro|rb
    ro == rangeonly, rb == range and bearing.""" % sys.argv[0]
    sys.exit(1)
  else:
    main(sys.argv[1] == "ro")