def testParticleFilter(): print 'Testing the ParticleFilter class ...' # Read map map = Map() map.readMap('../data/map/wean.dat') # basic tests filter = ParticleFilter() filter.initParticles(map) X = [Particle(0,0,0, 0.1), Particle(0,0,0, 0.1), Particle(0,0,0, 0.1)] print 'X after resampling', filter._resample(X) # test for update function pos = filter._update((5.0, 6.0, 0.0), (1, 2, 0.0), (2, 2, 0.0)) print pos # this should print something close to (6, 6, 0) ''' ind = weighted_choice([0.4, 0, 0]) if ind != 0: print 'FAILED weighted_choice test.' ind = weighted_choice([0.4, .6, 0.2]) print 'ind = ', ind ''' return
def main(): parser = argparse.ArgumentParser(description='Laser unit test') parser.add_argument('--map', type=str, default='../data/map/wean.dat', help='Ground truth occupancy map') parser.add_argument('--log', type=str, default='../data/log/robotdata1.log', help='Robot data log file') parser.add_argument('--lfield', type=str, default='./config/lfield_40.csv', help='Likelihood field') parser.add_argument('--save', type=str, default='./test/', help='Which folder to save to.') args = parser.parse_args() # Read in map. map = Map() map.readMap(args.map) # Number of data points. num_odometry = 0; num_laser = 0; # Laser scan data. z = [] # Read in log file. with open(args.log) as log: for line in log: token = line.split() if token[0] == 'O': num_odometry += 1 elif token[0] == 'L': num_laser += 1 z.append(np.array(token[1:])) # n = len(z) # z = [z[i] for i in range(0, n-1, 3)] # Convert z into full numpy array. z = np.array(z, dtype='float64') # Load likelihood field. L = LikelihoodField(0.01, 40, 8) L.loadField(args.lfield) # Run filter filter = ParticleFilter() # filter.setSensorModel(SensorModel(map, 0.7, 0.29, 0.01)) filter.setLikelihoodField(L) filter.run(map, z, args.save)
def main(): parser = argparse.ArgumentParser(description='Laser unit test') parser.add_argument('--map', type=str, default='./data/map/wean.dat', help='Ground truth occupancy map') parser.add_argument('--log', type=str, default='./data/log/robotdata1.log', help='Robot data log file') args = parser.parse_args() # Read in map. map = Map() map.readMap(args.map) # Number of data points. num_odometry = 0; num_laser = 0; # Laser scan data. z = [] # Odometry data. x = [] # Time data. t_z = [] t_x = [] # Read in log file. with open(args.log) as log: for line in log: token = line.split() if token[0] == 'O': num_odometry += 1 x.append(np.array(token[1:4])) t_x.append(token[4]) elif token[0] == 'L': num_laser += 1 z.append(np.array(token[1:187])) t_z.append(token[187]) # Convert data into full numpy arrays. x = np.array(x, dtype='float64') z = np.array(z, dtype='float64') t_x = np.array(t_x, dtype='float64') t_z = np.array(t_z, dtype='float64') # Clip laser readings at 3000 z = np.clip(z, -3000, 3000) # Plot the odometry trajectory. pl.plot(x[:,0], x[:,1]) # pl.show() pl.show(block=False) print z.shape[0] # Generate a list of x, y points to plot. for i in range(z.shape[0]): x_o = z[i,3] y_o = z[i,4] t_o = z[i,5] # Build matrix of laser angles cosines and sines for projection. t_s = np.linspace(-np.pi/2.0, np.pi/2.0, 180, True) # scan theta cos_s = np.cos(t_o + t_s) sin_s = np.sin(t_o + t_s) # Get laser measurements. z_t = z[i, 6:] # Project measurements into world frame. x_z = (x_o + np.multiply(z_t, cos_s)) y_z = (y_o + np.multiply(z_t, sin_s)) pl.cla() pl.plot(x[:,0], x[:,1]) pl.plot(x_z, y_z, ',r') pl.draw() time.sleep(0.01) pl.show()