def main(): src_path_map = '../data/map/wean.dat' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() sensor_model = SensorModel(occupancy_map) particleMeasurement = 500 probabilities = np.zeros(1000) index = 0 for actualMeasurement in range(1000): probabilities[index] = sensor_model.calculateProbability( actualMeasurement, particleMeasurement) index += 1 plotProbabilities(probabilities) numSamples = 1000 stdev = 100 gaussPDF = signal.gaussian(numSamples * 2, std=stdev) #plotProbabilities(gaussPDF) # my Bin-based version x = np.linspace( expon.ppf(0.01), expon.ppf(0.99), numSamples ) # Makes numSamples samples with a probability ranging from .99 to .1 expPDF = expon.pdf(x)
def main(): src_path_map = '../data/map/wean.dat' src_path_log = '../data/log/robotdata1.log' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() logfile = open(src_path_log, 'r') motion_model = MotionModel() vis_flag = 1 """ Monte Carlo Localization Algorithm : Main Loop """ if vis_flag: visualize_map(occupancy_map) first_time_idx = True X_bar = np.array([4250, 2270, 0]) for time_idx, line in enumerate(logfile): # Read a single 'line' from the log file (can be either odometry or laser measurement) meas_type = line[ 0] # L : laser scan measurement, O : odometry measurement meas_vals = np.fromstring( line[2:], dtype=np.float64, sep=' ') # convert measurement values from string to double odometry_robot = meas_vals[ 0:3] # odometry reading [x, y, theta] in odometry frame time_stamp = meas_vals[-1] # if ((time_stamp <= 0.0) | (meas_type == "O")): # ignore pure odometry measurements for now (faster debugging) # continue if (meas_type == "L"): odometry_laser = meas_vals[ 3:6] # [x, y, theta] coordinates of laser in odometry frame ranges = meas_vals[ 6:-1] # 180 range measurement values from single laser scan print("Processing time step " + str(time_idx) + " at time " + str(time_stamp) + "s") if (first_time_idx): u_t0 = odometry_robot first_time_idx = False continue # X_bar_new = np.zeros( (num_particles,4), dtype=np.float64) u_t1 = odometry_robot """ MOTION MODEL """ x_t0 = X_bar X_bar = motion_model.update(u_t0, u_t1, x_t0) print(X_bar) if vis_flag: visualize_timestep(X_bar, time_idx)
def main(): #x = np.array([94.234001, -139.953995, -1.342158]) # centimeters and radians #x = np.array([6300.0, 1400.0, 1.7]) # big open area on bottom right #x = np.array([4100.0, 4800.0, -1.342158]) # In hallway above room in upper map #x = np.array([4700.0, 900.0, -1.342158]) # bottom left corner of map #x = np.array([4130, 4000.0, 3.05]) # very close to correct location x = np.array([4050, 4000.0, 1.7]) # very close to correct location z = np.array([ 66, 66, 66, 66, 66, 65, 66, 66, 65, 66, 66, 66, 66, 66, 67, 67, 67, 66, 67, 66, 67, 67, 67, 68, 68, 68, 69, 67, 530, 514, 506, 508, 494, 481, 470, 458, 445, 420, 410, 402, 393, 386, 379, 371, 365, 363, 363, 364, 358, 353, 349, 344, 339, 335, 332, 328, 324, 321, 304, 299, 298, 294, 291, 288, 287, 284, 282, 281, 277, 277, 276, 274, 273, 271, 269, 268, 267, 266, 265, 265, 264, 263, 263, 263, 262, 261, 261, 261, 261, 261, 193, 190, 189, 189, 192, 262, 262, 264, 194, 191, 190, 190, 193, 269, 271, 272, 274, 275, 277, 279, 279, 281, 283, 285, 288, 289, 292, 295, 298, 300, 303, 306, 309, 314, 318, 321, 325, 329, 335, 340, 360, 366, 372, 378, 384, 92, 92, 91, 89, 88, 87, 86, 85, 84, 83, 82, 82, 81, 81, 80, 79, 78, 78, 77, 76, 76, 76, 75, 75, 74, 74, 73, 73, 72, 72, 72, 71, 72, 71, 71, 71, 71, 71, 71, 71, 71, 70, 70, 70, 70 ]) src_path_map = '../data/map/wean.dat' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() #map_obj.makeEdgeList() #edgeList = map_obj.edgeLocations #sensorObject = SensorModel(edgeList) sensorObject = SensorModel(occupancy_map) logProbability = sensorObject.beam_range_finder_model(z, x) vectorLaser = sensorObject.rangeLines #visualize_map(occupancy_map,vectorLaser) #visualize_map(occupancy_map,z) print("Laser") print(sensorObject.ranges) print("LIDAR") print(z) #visualizeRanges(z,sensorObject.ranges) particleX = x[0] particleY = x[1] particleAngle = x[2] rangeLines = np.zeros((180, 4)) for I in range(0, 180, 5): relativeAngle = (float(I - 90) / 180.0) * math.pi absoluteAngle = particleAngle + relativeAngle X = z[I] * math.cos( absoluteAngle) + particleX # This value is in centimeters Y = z[I] * math.sin(absoluteAngle) + particleY rangeLines[I][:] = [ particleX / 10, particleY / 10, X / 10, Y / 10 ] # all of the /10 are so it displays correctly on the map visualize_map(occupancy_map, vectorLaser)
def test_error(): src_path_map = '../data/map/wean.dat' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() model = SensorModel(occupancy_map) x = np.linspace(0, 8183, num=2500) y = model._total_error(x, np.ones(x.shape) * 2500) plt.plot(x, y) plt.axis([-100, 8283, 0, 1.5]) plt.show()
def test_beam_range_measurement(): src_path_map = '../data/map/wean.dat' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() model = SensorModel(occupancy_map) fig = plt.figure() mng = plt.get_current_fig_manager(); # mng.resize(*mng.window.maxsize()) plt.imshow(occupancy_map, cmap='Greys') # x = 4650.0 # y = 2200.0 # x = 6000.0 # y = 1450.0 # x = 1660.0 # y = 6100.0 # x = 4833 # y = 5111 while True: while True: y = np.random.uniform(0, 7000) x = np.random.uniform(3000, 7000) if occupancy_map[int(y / 10)][int(x / 10)] == 0: break plt.clf() plt.imshow(occupancy_map, cmap='Greys') test_points = np.zeros((360, 3)) for deg in np.linspace(0, 360, num=360, endpoint=False): th = math.radians(deg) test_points[np.floor(deg).astype(np.uint32), :] = (np.array([x, y, th])) dists = model.beam_range_measurement(test_points) for deg in range(360): th = math.radians(deg) dist = dists[deg] print "Distance of", dist, "for", deg, "degrees" x2 = x + dist * math.cos(th) y2 = y + dist * math.sin(th) plt.plot([x / 10, x2 / 10], [y / 10, y2 / 10], 'b-', linewidth=0.25) plt.plot([x / 10], [y / 10], marker='o', markersize=3, color="red") plt.axis([0, 800, 0, 800]) plt.pause(1)
def initialize_map(self, filename): from MapReader import MapReader import sys import os map_reader = MapReader(filename) map_reader.run() map_x = map_reader.map_x map_y = map_reader.map_y self.min_x = math.floor(min(map_reader.map_x)) self.max_x = math.ceil(max(map_reader.map_x)) self.og_width = self.max_x - self.min_x self.min_y = math.floor(min(map_reader.map_y)) self.max_y = math.ceil(max(map_reader.map_y)) self.og_height = self.max_y - self.min_y self.width = math.floor(self.og_width / self.robot_len) self.height = math.floor(self.og_height / self.robot_len) # initialize all the blocks in range for i in range(1, self.width + 1): for j in range(1, self.height + 1): self.vis[(i, j)] = 0 self.blocked[(i, j)] = 0 # mark all blocks with obstacles as blocked for i in range(len(map_reader.map_x)): self.blocked[self.to_point_coor(map_reader.map_x[i], map_reader.map_y[i])] = 1 # initializethe borders to avoid array out of bound for i in range(-10, self.width + 10): for j in range(-10, self.height + 10): if (self.is_valid_point(i, j)): self.vis[(i, j)] = 0 self.blocked[(i, j)] = 0 else: self.vis[(i, j)] = 0 self.blocked[(i, j)] = 1 # and count valid block numbers for i in range(0, self.width + 2, 2): for j in range(0, self.height + 2, 2): if (self.is_valid_block(i, j)): self.block_num = self.block_num + 1
def main(): src_path_map = '../data/map/wean.dat' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() visualize_map(occupancy_map) occupancy_map = binary_map(occupancy_map) xbar_path = 'Xbar3/' file_names = os.listdir(xbar_path) timestamp_list = [] for file in file_names: timestamp_list.append(int(file.split('.')[0])) timestamp_list = np.asarray(timestamp_list) index_list = np.argsort(timestamp_list) for index in index_list: X = np.load(xbar_path + file_names[index]) print(X.shape) visualize_timestep(X, timestamp_list[index], occupancy_map) plt.pause(0.01)
def genLookup(): """ Description of variables used u_t0 : particle state odometry reading [x, y, theta] at time (t-1) [odometry_frame] u_t1 : particle state odometry reading [x, y, theta] at time t [odometry_frame] x_t0 : particle state belief [x, y, theta] at time (t-1) [world_frame] x_t1 : particle state belief [x, y, theta] at time t [world_frame] X_bar : [num_particles x 4] sized array containing [x, y, theta, wt] values for all particles z_t : array of 180 range measurements for each laser scan """ """ Initialize Parameters """ src_path_map = '../data/map/wean.dat' src_path_log = '../data/log/robotdata1.log' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() logfile = open(src_path_log, 'r') count2 = 0.0 count = 0.0 total = 800.0*800.0*360.0 occ = np.empty((800,800,360), dtype=np.float64) ti = time.time() for y in range(0,800): for x in range(0,800): if occupancy_map[y][x] >= 0 and occupancy_map[y][x] < 1.0: for theta in range(0,360): count += 1.0 occ[y][x][theta] = trace_rays(np.array((x,y,theta)), occupancy_map) else: for theta in range(0,360): occ[y][x][theta] = -1 count2 += 1.0 print 'total:' , (count+count2)/total print 'Non -1 values:' , count/total elapsed = time.time() - ti print elapsed np.save('occ.txt',occ)
x_t1 = np.array([x1, y1, theta1]) #print(x_t1) return x_t1 if __name__ == "__main__": #motion_model = MotionModel() """ Code for testing motion model """ src_path_map = '../../data/map/wean.dat' src_path_log = '../../data/log/robotdata1.log' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() logfile = open(src_path_log, 'r') motion_model = MotionModel() # sensor_model = SensorModel(occupancy_map) # resampler = Resampling() num_particles = 1 X_bar = init_particles_random(num_particles, occupancy_map) #X_bar = np.array([[5000, 4000, np.pi,1]]) vis_flag = 1 first_time_idx = True
# print(occ_map[100,100]) # self._occupancy_map[self._occupancy_map != 0.0] == 0.5 # self._occupancy_map[self._occupancy_map == 0.0] == 0.0 # self._occupancy_map[self._occupancy_map == -1.0] == 1.0 return occ_map if __name__ == '__main__': src_path_map = '../../data/map/wean.dat' src_path_log = '../../data/log/robotdata1.log' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() occupancy_map = threshold_map(occupancy_map) map_obj.visualize_map() sensor_model = SensorModel(occupancy_map) motion_model = MotionModel() num_particles = 1 # X_bar = np.transpose(np.array([5.75300000e+02, 4.71200000e+02, -4.57011189e-01]))#This is obtained from random particles X_bar = np.transpose(np.array([4.4000000e+02, 3.95000000e+02, 3.14011189 ])) #This is obtained from random particles
def main(): """ Description of variables used u_t0 : particle state odometry reading [x, y, theta] at time (t-1) [odometry_frame] u_t1 : particle state odometry reading [x, y, theta] at time t [odometry_frame] x_t0 : particle state belief [x, y, theta] at time (t-1) [world_frame] x_t1 : particle state belief [x, y, theta] at time t [world_frame] X_bar : [num_particles x 4] sized array containing [x, y, theta, wt] values for all particles z_t : array of 180 range measurements for each laser scan """ """ Initialize Parameters """ src_path_map = '../data/map/wean.dat' src_path_log = '../data/log/robotdata2.log' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() vis_flag = 1 if vis_flag: visualize_map(occupancy_map) logfile = open(src_path_log, 'r') occupancy_map = binary_map(occupancy_map) motion_model = MotionModel() sensor_model = SensorModel(occupancy_map) resampler = Resampling() num_particles = 500 X_bar = init_particles_random(num_particles, occupancy_map) print('X_bar shape:', X_bar.shape) ## Monte Carlo Localization Algorithm : Main Loop first_time_idx = True for time_idx, line in enumerate(logfile): start_time = time.time() meas_type = line[ 0] # L : laser scan measurement, O : odometry measurement if (meas_type == "L"): meas_vals = np.fromstring(line[2:], dtype=np.float64, sep=' ') odometry_robot = meas_vals[0:3] time_stamp = meas_vals[-1] odometry_laser = meas_vals[ 3:6] # [x, y, theta] coordinates of laser in odometry frame ranges = meas_vals[6:-1] if (first_time_idx): u_t0 = odometry_robot first_time_idx = False continue u_t1 = odometry_robot if sum(abs(u_t1[:2] - u_t0[:2])) < 1 and abs(u_t1[2] - u_t0[2]) < (3.14 / 20): print('skip') ## skip states that doesn't moved continue if vis_flag: visualize_timestep(X_bar, time_idx, occupancy_map) # impplement multiprocessing for args = [[X_bar[m], u_t0, u_t1, ranges, motion_model, sensor_model] for m in range(0, num_particles)] with multiprocessing.Pool(processes=16) as p: res = p.starmap(combine_motion_sensor, args) X_bar = np.asarray(res) # np.save('log2_Xbar/'+str(time_idx)+'.npy',X_bar) # save Xbar for accelerate display u_t0 = u_t1 ## RESAMPLING X_bar = resampler.low_variance_sampler(X_bar) end_time = time.time() print("Processing time step " + str(time_idx) + " at time " + str(time_stamp) + "s") print('time_cost for 1 time step:', end_time - start_time) if time_idx > 300: # stop display when time_idx>300, for saving time vis_flag = 0
def main(): """ Description of variables used u_t0 : particle state odometry reading [x, y, theta] at time (t-1) [odometry_frame] u_t1 : particle state odometry reading [x, y, theta] at time t [odometry_frame] x_t0 : particle state belief [x, y, theta] at time (t-1) [world_frame] x_t1 : particle state belief [x, y, theta] at time t [world_frame] X_bar : [num_particles x 4] sized array containing [x, y, theta, wt] values for all particles z_t : array of 180 range measurements for each laser scan """ """ Initialize Parameters """ src_path_map = '../data/map/wean.dat' src_path_log = '../data/log/robotdata1.log' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() logfile = open(src_path_log, 'r') motion_model = MotionModel() sensor_model = SensorModel(occupancy_map) resampler = Resampling() num_particles = 700 X_bar = init_particles_freespace(num_particles, occupancy_map) vis_flag = 1 """ Monte Carlo Localization Algorithm : Main Loop """ if vis_flag: visualize_map(occupancy_map) first_time_idx = True for time_idx, line in enumerate(logfile): # Read a single 'line' from the log file (can be either odometry or laser measurement) meas_type = line[ 0] # L : laser scan measurement, O : odometry measurement meas_vals = np.fromstring( line[2:], dtype=np.float64, sep=' ') # convert measurement values from string to double odometry_robot = meas_vals[ 0:3] # odometry reading [x, y, theta] in odometry frame time_stamp = meas_vals[-1] # if ((time_stamp <= 0.0) | (meas_type == "O")): # ignore pure odometry measurements for now (faster debugging) # continue if (meas_type == "L"): odometry_laser = meas_vals[ 3:6] # [x, y, theta] coordinates of laser in odometry frame ranges = meas_vals[ 6:-1] # 180 range measurement values from single laser scan print("Processing time step " + str(time_idx) + " at time " + str(time_stamp) + "s") if (first_time_idx): u_t0 = odometry_robot first_time_idx = False continue X_bar_new = np.zeros((num_particles, 4), dtype=np.float64) u_t1 = odometry_robot for m in range(0, num_particles): """ MOTION MODEL """ x_t0 = X_bar[m, 0:3] x_t1 = motion_model.update(u_t0, u_t1, x_t0) """ SENSOR MODEL """ if (meas_type == "L"): z_t = ranges w_t = sensor_model.beam_range_finder_model(z_t, x_t1) X_bar_new[m, :] = np.hstack((x_t1, w_t)) else: X_bar_new[m, :] = np.hstack((x_t1, X_bar[m, 3])) X_bar = X_bar_new u_t0 = u_t1 """ RESAMPLING """ X_bar = resampler.low_variance_sampler(X_bar) if vis_flag: # meas_vals[3:6] - [x, y, theta] coordinates of laser in odometry frame # meas_vals[6:-1] # 180 range measurement values from single laser scan visualize_timestep(X_bar, time_idx)
def main(): """ Description of variables used u_t0 : particle state odometry reading [x, y, theta] at time (t-1) [odometry_frame] u_t1 : particle state odometry reading [x, y, theta] at time t [odometry_frame] x_t0 : particle state belief [x, y, theta] at time (t-1) [world_frame] x_t1 : particle state belief [x, y, theta] at time t [world_frame] X_bar : [num_particles x 4] sized array containing [x, y, theta, wt] values for all particles z_t : array of 180 range measurements for each laser scan """ """ Initialize Parameters """ src_path_map = '../data/map/wean.dat' src_path_log = '../data/log/robotdata1.log' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() logfile = open(src_path_log, 'r') motion_model = MotionModel() sensor_model = SensorModel(occupancy_map) resampler = Resampling() #num_particles = 500 num_particles = 100 X_bar = init_particles_random(num_particles, occupancy_map) #X_bar = np.array([[5000, 4000, np.pi,1]]) #X_bar[0,0:3] vis_flag = 1 """ Monte Carlo Localization Algorithm : Main Loop """ if vis_flag: visualize_map(occupancy_map) first_time_idx = True for time_idx, line in enumerate(logfile): # Read a single 'line' from the log file (can be either odometry or laser measurement) meas_type = line[ 0] # L : laser scan measurement, O : odometry measurement meas_vals = np.fromstring( line[2:], dtype=np.float64, sep=' ') # convert measurement values from string to double odometry_robot = meas_vals[ 0:3] # odometry reading [x, y, theta] in odometry frame time_stamp = meas_vals[-1] # if ((time_stamp <= 0.0) | (meas_type == "O")): # ignore pure odometry measurements for now (faster debugging) # continue if (meas_type == "L"): odometry_laser = meas_vals[ 3:6] # [x, y, theta] coordinates of laser in odometry frame ranges = meas_vals[ 6:-1] # 180 range measurement values from single laser scan #print "Processing time step " + str(time_idx) + " at time " + str(time_stamp) + "s" if (first_time_idx): u_t0 = odometry_robot first_time_idx = False continue X_bar_new = np.zeros((num_particles, 4), dtype=np.float64) u_t1 = odometry_robot for m in range(0, num_particles): """ MOTION MODEL """ x_t0 = X_bar[m, 0:3] x_t1 = motion_model.update(u_t0, u_t1, x_t0) """ SENSOR MODEL """ if (meas_type == "L"): z_t = ranges print("in sensor model") w_t = sensor_model.beam_range_finder_model(z_t, x_t1) print("outside sensor model") print(w_t) #input() # w_t = 1/num_particles X_bar_new[m, :] = np.hstack((x_t1, w_t)) else: X_bar_new[m, :] = np.hstack((x_t1, X_bar[m, 3])) X_bar = X_bar_new u_t0 = u_t1 # """ # RESAMPLING # """ X_bar = resampler.low_variance_sampler(X_bar) # fig = plt.figure() # # plt.switch_backend('TkAgg') # print("Bot-pos",X_bar[9,0:2]) # #print("Xbar",X_bar) # mng = plt.get_current_fig_manager(); # mng.resize(*mng.window.maxsize()) # plt.ion(); plt.imshow(occupancy_map, cmap='Greys'); plt.axis([0, 800, 0, 800]); # plt.plot(X_bar[:,0]/10.0,X_bar[:,1]/10.0,'o',color='red') # plt.plot(x_t1[0]/10.0,x_t1[1]/10.0,'o',color='yellow') # plt.show() # plt.pause(1) # plt.close() if vis_flag: visualize_timestep(X_bar, time_idx)
def main(): src_path_map = '../data/map/wean.dat' src_path_log = '../data/log/robotdata1.log' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() logfile = open(src_path_log, 'r') motion_model = MotionModel() sensor_model = SensorModel(occupancy_map) resampler = Resampling() num_particles = 1 #X_bar = np.transpose(np.array([5.75300000e+03, 1.71200000e+03, -4.57011189e-01])) X_bar = init_particles_random(num_particles, occupancy_map) print(X_bar.shape) vis_flag = 1 first_time_idx = True x_est_odom = [] y_est_odom = [] for time, line in enumerate(logfile): meas_type = line[ 0] # L : laser scan measurement, O : odometry measurement meas_vals = np.fromstring( line[2:], dtype=np.float64, sep=' ') # convert measurement values from string to double odometry_robot = meas_vals[ 0:3] # odometry reading [x, y, theta] in odometry frame time_stamp = meas_vals[-1] if (meas_type == "L"): odometry_laser = meas_vals[ 3:6] # [x, y, theta] coordinates of laser in odometry frame ranges = meas_vals[ 6:-1] # 180 range measurement values from single laser scan if (first_time_idx): u_t0 = odometry_robot first_time_idx = False continue X_bar_new = np.zeros((num_particles, 4), dtype=np.float64) u_t1 = odometry_robot flag = 0 for m in range(0, num_particles): #print("First",X_bar.shape) x_t0 = X_bar[0][0:3] #print(x_t0.shape) x_t1 = motion_model.update(u_t0, u_t1, x_t0) print("1---------", x_t1) #input() if (meas_type == "L"): z_t = ranges #w_t=1 w_t = sensor_model.beam_range_finder_model(z_t, x_t1) # flag=1; # break # w_t = 1/num_particles print("2-----------------", X_bar_new) X_bar_new[m, :] = np.hstack((x_t1, w_t)) else: X_bar_new[m, :] = np.hstack((x_t1, X_bar[m, 3])) print("3-------------------", X_bar_new) X_bar_new[m, :] = np.hstack((x_t1, X_bar[m, 3])) #print("Second",x_t1.shape) #print("Threee",X_bar_new.shape) #if(flag==1): #break print("4-------------------------", X_bar_new) X_bar = X_bar_new print("5--------------------------", X_bar) u_t0 = u_t1 x_est_odom.append(X_bar[0][0]) y_est_odom.append(X_bar[0][1]) plt.imshow(occupancy_map, cmap='Greys') #plt.show() #plt.subplot(1,2,1) plt.draw() plt.subplot(1, 2, 1) plt.plot(x_est_odom, y_est_odom) plt.show()
from AssetsReader import AssetsReader from GameCommon import GameCommon from GuiManager import GuiManager from MapReader import MapReader from TowersManager import TowersManager from WaveManager import WaveManager from objects.gui.Button import Button from objects.towers.Range import Range from settings.Settings import WINDOW_SIZE, LEFT_BUTTON, RIGHT_BUTTON, BACKGROUND_COLOR pygame.init() pygame.display.set_caption("Tower defence") screen = pygame.display.set_mode(WINDOW_SIZE, pygame.SRCALPHA) gameCommon = GameCommon() gameCommon.route = MapReader().get_route() AssetsReader() towersManager = TowersManager() map_elements_list = MapReader().get_map_elements() pygame.mouse.set_visible(False) clock = pygame.time.Clock() waveManager = WaveManager() all_sprites_list = Group(map_elements_list) all_sprites_list.add(gameCommon.enemies_list) all_sprites_list.add(gameCommon.towers_list)
def main(): """ Description of variables used u_t0 : particle state odometry reading [x, y, theta] at time (t-1) [odometry_frame] u_t1 : particle state odometry reading [x, y, theta] at time t [odometry_frame] x_t0 : particle state belief [x, y, theta] at time (t-1) [world_frame] x_t1 : particle state belief [x, y, theta] at time t [world_frame] X_bar : [num_particles x 4] sized array containing [x, y, theta, wt] values for all particles z_t : array of 180 range measurements for each laser scan """ """ Initialize Parameters """ src_path_map = '../data/map/wean.dat' src_path_log = '../data/log/robotdata2.log' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() # get the free space map occupancy_map = filter_close_map(occupancy_map) # occupancy_map = np.logical_and(occupancy_map<occu_thresh, occupancy_map>=0) logfile = open(src_path_log, 'r') motion_model = MotionModel() sensor_model = SensorModel(occupancy_map) resampler = Resampling() num_particles = init_n_particles # X_bar = init_particles_random(num_particles, occupancy_map) X_bar = init_particles_freespace(num_particles, occupancy_map) X_bar = np.vstack((X_bar, init_test_particle())) # X_bar = init_test_particle() num_particles, _ = X_bar.shape vis_flag = 1 """ Monte Carlo Localization Algorithm : Main Loop """ # if vis_flag: # visualize_map(occupancy_map) # draw_robot(X_bar) first_time_idx = True u_t0 = np.array([0]) for time_idx, line in enumerate(logfile): # Read a single 'line' from the log file (can be either odometry or laser measurement) meas_type = line[0] # L : laser scan measurement, O : odometry measurement meas_vals = np.fromstring(line[2:], dtype=np.float64, sep=' ') # convert measurement values from string to double odometry_robot = meas_vals[0:3] # odometry reading [x, y, theta] in odometry frame time_stamp = meas_vals[-1] # if ((time_stamp <= 0.0) | (meas_type == "O")): # ignore pure odometry measurements for now (faster debugging) # continue ranges = np.array([0]) if (meas_type == "L"): odometry_laser = meas_vals[3:6] # [x, y, theta] coordinates of laser in odometry frame ranges = meas_vals[6:-1] # 180 range measurement values from single laser scan print("Processing time step " + str(time_idx) + " at time " + str(time_stamp) + "s") if (first_time_idx): u_t0 = odometry_robot first_time_idx = False continue # X_bar_new = np.zeros( (num_particles,4), dtype=np.float64) u_t1 = odometry_robot #X_bar_new = X_bar # pool = Pool(4) for m in range(0, num_particles): X_bar[m, :] = parallel_motion_sensor_model(m, u_t0, u_t1, ranges, meas_type, sensor_model, motion_model, X_bar[m, :]) # X_bar_new[m,:] = pool.apply_async(parallel_motion_sensor_model, (m, u_t0, u_t1, ranges, meas_type, # sensor_model, motion_model, X_bar_new[m,:])) # pool.close() # pool.join() # for m in range(0, num_particles): # # """ # MOTION MODEL # """ # if np.linalg.norm(u_t0 - u_t1) != 0: # x_t0 = X_bar[m,:3] # x_t1 = motion_model.update(u_t0, u_t1, x_t0) # else: # x_t0 = X_bar[m, :3] # x_t1 = x_t0 # # # """ # SENSOR MODEL # """ # if (meas_type == "L" ) and (np.linalg.norm(u_t0-u_t1) != 0): # z_t = ranges # w_t = sensor_model.beam_range_finder_model(z_t, x_t1) # # w_t = 1/num_particles # X_bar_new[m,:] = np.hstack((x_t1, w_t)) # else: # X_bar_new[m,:] = np.hstack((x_t1, X_bar[m,3])) #X_bar = X_bar_new moved = np.linalg.norm(u_t0-u_t1) != 0 u_t0 = u_t1 """ RESAMPLING """ if moved: if meas_type == "L": X_bar = resampler.low_variance_sampler(X_bar) print (X_bar.shape) num_particles, _ = X_bar.shape if vis_flag: visualize_timestep(X_bar, time_idx, occupancy_map)
def main(): """ Description of variables used u_t0 : particle state odometry reading [x, y, theta] at time (t-1) [odometry_frame] u_t1 : particle state odometry reading [x, y, theta] at time t [odometry_frame] x_t0 : particle state belief [x, y, theta] at time (t-1) [world_frame] x_t1 : particle state belief [x, qy, theta] at time t [world_frame] X_bar : [num_particles x 4] sized array containing [x, y, theta, wt] values for all particles z_t : array of 180 range measurements for each laser scan """ """ Initialize Parameters """ src_path_map = '../data/map/wean.dat' src_path_log = '../data/log/robotdata1.log' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() logfile = open(src_path_log, 'r') motion_model = MotionModel() sensor_model = SensorModel(occupancy_map) resampler = Resampling() num_particles = 3000 og_num_particles = num_particles sumd = 0 # --------------------------------------------------- # Create intial set of particles X_bar = init_particles_freespace(num_particles, occupancy_map) # Useful for debugging, places particles near correct starting area for log1 #X_bar = init_debug(num_particles) # --------------------------------------------------- vis_flag = 1 # --------------------------------------------------- # Weights are dummy weights for testing motion model w0_vals = np.ones((1, num_particles), dtype=np.float64) w_t = w0_vals / num_particles #---------------------------------------------------- """ Monte Carlo Localization Algorithm : Main Loop """ if vis_flag: visualize_map(occupancy_map) iter_num = 0 first_time_idx = True for time_idx, line in enumerate(logfile): # Read a single 'line' from the log file (can be either odometry or laser measurement) meas_type = line[ 0] # L : laser scan measurement, O : odometry measurement meas_vals = np.fromstring( line[2:], dtype=np.float64, sep=' ') # convert measurement values from string to double odometry_robot = meas_vals[ 0:3] # odometry reading [x, y, theta] in odometry frame # print "odometry_robot = ", odometry_robot time_stamp = meas_vals[-1] #if ((time_stamp <= 0.0) | (meas_type == "O")): # ignore pure odometry measurements for now (faster debugging) #continue if (meas_type == "L"): odometry_laser = meas_vals[ 3:6] # [x, y, theta] coordinates of laser in odometry frame ranges = meas_vals[ 6:-1] # 180 range measurement values from single laser scan print "Processing time step " + str(time_idx) + " at time " + str( time_stamp) + "s" if (first_time_idx): u_t0 = odometry_robot first_time_idx = False continue X_bar_new = np.zeros((num_particles, 4), dtype=np.float64) u_t1 = odometry_robot yd = u_t1[1] - u_t0[1] xd = u_t1[0] - u_t0[0] d = math.sqrt(pow(xd, 2) + pow(yd, 2)) if d < 1.0: visualize_timestep(X_bar, time_idx, time_idx) continue if d > 20: # lost robot print('\nROBOT IS LOST!!!\nResetting particles...\n') X_bar = init_particles_freespace(og_num_particles, occupancy_map) num_particles = og_num_particles X_bar_new = np.zeros((num_particles, 4), dtype=np.float64) u_t0 = u_t1 visualize_timestep(X_bar, time_idx, time_idx) sumd = 0 else: sumd = sumd + d for m in range(0, num_particles): """ MOTION MODEL """ x_t0 = X_bar[m, 0:3] x_t1 = motion_model.update(u_t0, u_t1, x_t0) #motion_last = math.sqrt((x_t1[0,1]-x_t0[0,1])**2 + (x_t1[0,0]-x_t0[0,0])**2) # --------------------------------------------------- # For testing Motion Model # X_bar_new[m,:] = np.hstack((x_t1, w_t)) # --------------------------------------------------- """ SENSOR MODEL """ if (meas_type == "L"): z_t = ranges x_l1 = motion_model.laser_position(odometry_laser, u_t1, x_t1) #print w_t.shape w_t = sensor_model.beam_range_finder_model(z_t, x_l1) # #print w_t.shape # if w_t > 0.0 and X_bar[m,3] > 0.0: # w_new = math.log(X_bar[m,3]) + math.log(w_t) # w_new = math.exp(w_new) # else: # w_new = 0.0 X_bar_new[m, :] = np.hstack((x_t1, [[w_t]])) #time.sleep(10) else: X_bar_new[m, :] = np.hstack((x_t1, [[X_bar[m, 3]]])) # sorted_particles = X_bar[X_bar[:,3].argsort()] # print(sorted_particles[499,3]) X_bar = X_bar_new u_t0 = u_t1 X_bar[:, 3] = X_bar[:, 3] / sum(X_bar[:, 3]) """ RESAMPLING """ if sumd > 10.0: # X_bar = resampler.low_variance_sampler_rand(X_bar, occupancy_map) sumd = 0 if X_bar[:, 3].var() < 9.0e-8 and num_particles > 500: num_particles = num_particles - 300 print 'Adapting particles\nCurrent particle size = ', num_particles elif X_bar[:, 3].var() < 1.0e-7 and num_particles > 300: num_particles = num_particles - 100 print 'Adapting particles\nCurrent particle size = ', num_particles # if num_particles < og_num_particles and X_bar[:,3].var() > 5.0e-7: # num_particles = num_particles + 100 # print 'Adapting particles\nCurrent particle size = ', num_particles X_bar = resampler.low_variance_sampler(X_bar, num_particles) #print X_bar[:,3].var() if vis_flag: visualize_timestep(X_bar, time_idx, time_idx)
def main(): """ Description of variables used u_t0 : particle state odometry reading [x, y, theta] at time (t-1) [odometry_frame] u_t1 : particle state odometry reading [x, y, theta] at time t [odometry_frame] x_t0 : particle state belief [x, y, theta] at time (t-1) [world_frame] x_t1 : particle state belief [x, y, theta] at time t [world_frame] X_bar : [num_particles x 4] sized array containing [x, y, theta, wt] values for all particles z_t : array of 180 range measurements for each laser scan """ """ Initialize Parameters """ src_path_map = '../data/map/wean.dat' src_path_log = '../data/log/robotdata2.log' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() logfile = open(src_path_log, 'r') motion_model = MotionModel() sensor_model = SensorModel(occupancy_map) resampler = Resampling() num_particles = 300 X_bar = init_particles_random(num_particles, occupancy_map) vis_flag = 1 print("a1: ",motion_model.alpha_1,"\na2: ",motion_model.alpha_2,"\na3: ",\ motion_model.alpha_3,"\na4: ",motion_model.alpha_4,"\nstdv: ",sensor_model.stdDevHit, \ "\nlamd: ", sensor_model.lambdaShort,"\nzhit: ",sensor_model.zHit,"\nzsht: ",sensor_model.zShort, \ "\nzmax: ", sensor_model.zMax, "\nzrnd: ", sensor_model.zRand, "\ncert: ", sensor_model.certainty, \ "\nlsub: ", sensor_model.laserSubsample) """ Monte Carlo Localization Algorithm : Main Loop """ if vis_flag: visualize_map(occupancy_map) first_time_idx = True #initialize worker threads p = ThreadPool(15) for time_idx, line in enumerate(logfile): # Read a single 'line' from the log file (can be either odometry or laser measurement) meas_type = line[ 0] # L : laser scan measurement, O : odometry measurement meas_vals = np.fromstring( line[2:], dtype=np.float64, sep=' ') # convert measurement values from string to double odometry_robot = meas_vals[ 0:3] # odometry reading [x, y, theta] in odometry frame time_stamp = meas_vals[-1] #if ((time_stamp <= 0.0) | (meas_type == "O")): # ignore pure odometry measurements for now (faster debugging) # continue if (meas_type == "L"): odometry_laser = meas_vals[ 3:6] # [x, y, theta] coordinates of laser in odometry frame ranges = meas_vals[ 6:-1] # 180 range measurement values from single laser scan print("Processing time step " + str(time_idx) + " at time " + str(time_stamp) + "s") if (first_time_idx): u_t0 = odometry_robot first_time_idx = False continue X_bar_new = np.zeros((num_particles, 4), dtype=np.float64) u_t1 = odometry_robot # X_bar.shape[0] (length) decreases from 500 to 499 after time step 1 #PARALLELIZED MOTION MODEL x_t0Arr = [[u_t0, u_t1, X_bar[m, 0:3]] for m in range(0, X_bar.shape[0])] x_t1Arr = p.map(motion_model.par_update, x_t0Arr) #PARALLELIZED SENSOR MODEL if (meas_type == "L"): z_t = ranges sensInArr = [[z_t, x_t1Arr[m]] for m in range(0, X_bar.shape[0])] w_tArr = p.map(sensor_model.par_beam_range_finder_model, sensInArr) for m in range(0, X_bar.shape[0]): X_bar_new[m, :] = np.hstack((x_t1Arr[m], w_tArr[m])) else: for m in range(0, X_bar.shape[0]): X_bar_new[m, :] = np.hstack((x_t1Arr[m], X_bar[m, 3])) # for m in range(0, X_bar.shape[0]): # # #MOTION MODEL # # x_t0 = X_bar[m, 0:3] # x_t1 = motion_model.update(u_t0, u_t1, x_t0) # # # #SENSOR MODEL # # if (meas_type == "L"): # z_t = ranges # w_t = sensor_model.beam_range_finder_model(z_t, x_t1) # # w_t = 1/num_particles # X_bar_new[m,:] = np.hstack((x_t1, w_t)) # else: # X_bar_new[m,:] = np.hstack((x_t1, X_bar[m,3])) X_bar = X_bar_new u_t0 = u_t1 """ RESAMPLING """ X_bar = resampler.low_variance_sampler(X_bar) if vis_flag: visualize_timestep(X_bar, time_idx)
def main(): """ Description of variables used u_t0 : particle state odometry reading [x, y, theta] at time (t-1) [odometry_frame] u_t1 : particle state odometry reading [x, y, theta] at time t [odometry_frame] x_t0 : particle state belief [x, y, theta] at time (t-1) [world_frame] x_t1 : particle state belief [x, y, theta] at time t [world_frame] X_bar : [num_particles x 4] sized array containing [x, y, theta, wt] values for all particles z_t : array of 180 range measurements for each laser scan """ """ Initialize Parameters """ src_path_map = '../data/map/wean.dat' src_path_log = '../data/log/robotdata1.log' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() logfile = open(src_path_log, 'r') motion_model = MotionModel() sensor_model = SensorModel(occupancy_map) resampler = Resampling() num_particles = 100 time_period = 10 # X_bar = init_particles_random(num_particles, occupancy_map) X_bar = init_particles_freespace(num_particles, occupancy_map) vis_flag = 1 vis_type = 'mean' # {mean, max} """ Monte Carlo Localization Algorithm : Main Loop """ if vis_flag: visualize_map(occupancy_map) first_time_idx = True count = 0 for time_idx, line in enumerate(logfile): # if time_idx % 9 != 0: continue # Read a single 'line' from the log file (can be either odometry or laser measurement) meas_type = line[ 0] # L : laser scan measurement, O : odometry measurement meas_vals = np.fromstring( line[2:], dtype=np.float64, sep=' ') # convert measurement values from string to double odometry_robot = meas_vals[ 0:3] # odometry reading [x, y, theta] in odometry frame time_stamp = meas_vals[-1] if ((time_stamp <= 0.0) | (meas_type == "O") ): # ignore pure odometry measurements for now (faster debugging) continue count = count + 1 if (meas_type == "L"): odometry_laser = meas_vals[ 3:6] # [x, y, theta] coordinates of laser in odometry frame ranges = meas_vals[ 6:-1] # 180 range measurement values from single laser scan # print "Processing time step " + str(time_idx) + " at time " + str(time_stamp) + "s" if (first_time_idx): u_t0 = odometry_robot first_time_idx = False continue X_bar_new = np.zeros((num_particles, 4), dtype=np.float64) u_t1 = odometry_robot for m in range(0, num_particles): """ MOTION MODEL """ x_t0 = X_bar[m, 0:3] x_t1 = motion_model.update(u_t0, u_t1, x_t0) """ SENSOR MODEL """ if (meas_type == "L"): z_t = ranges w_t = sensor_model.beam_range_finder_model(z_t, x_t1) # w_t = 1/num_particles # X_bar_new[m,:] = np.hstack((x_t1, w_t)) new_wt = X_bar[m, 3] * motion_model.give_prior( x_t1, u_t1, x_t0, u_t0) X_bar_new[m, :] = np.hstack((x_t1, new_wt)) else: X_bar_new[m, :] = np.hstack((x_t1, X_bar[m, 3])) if (vis_type == 'max'): best_particle_idx = np.argmax(X_bar_new, axis=0)[-1] vis_particle = X_bar_new[best_particle_idx][:-1] elif (vis_type == 'mean'): # ipdb.set_trace() X_weighted = X_bar_new[:, :3] * X_bar_new[:, 3:4] X_mean = np.sum(X_weighted, axis=0) vis_particle = X_mean / sum(X_bar_new[:, 3:4]) # print(X_bar_new[:,-1].T) sensor_model.visualization = True sensor_model.plot_measurement = True # sensor_model.beam_range_finder_model(ranges, vis_particle) sensor_model.visualization = False sensor_model.plot_measurement = False X_bar = X_bar_new u_t0 = u_t1 """ RESAMPLING """ #if(np.mean(x_t1 - x_t0) > 0.2): X_bar = resampler.low_variance_sampler(X_bar) add_particles = num_particles / 5 # time_period = 10 if (count % time_period == 0 or sum(X_bar[:, -1]) == 0): X_bar_re_init = init_particles_freespace(add_particles, occupancy_map) X_bar[:, -1] = 1.0 / (num_particles + add_particles) X_bar_re_init[:, -1] = 1.0 / (num_particles + add_particles) X_bar = np.concatenate((X_bar, X_bar_re_init), axis=0) num_particles = X_bar.shape[0] print num_particles if (count % 100 == 0): time_period = time_period * 5 # X_bar = resampler.multinomial_sampler(X_bar) # check if importance too low # thres = 1e-29 # indices = np.where(X_bar[:,3] > thres)[0] # print(X_bar.shape[0] - indices.shape[0]) # temp = init_particles_freespace(X_bar.shape[0] - indices.shape[0], occupancy_map) # X_bar = np.concatenate((X_bar[indices], temp), axis = 0) # X_bar[:,-1] = 1.0/num_particles if vis_flag: visualize_timestep(X_bar, time_idx)
def main(): reader = MapReader() gamemap, pacman = reader.read('gamemap.txt') g = Game(gamemap, pacman) g.start()
def main(): """ Description of variables used u_t0 : particle state odometry reading [x, y, theta] at time (t-1) [odometry_frame] u_t1 : particle state odometry reading [x, y, theta] at time t [odometry_frame] x_t0 : particle state belief [x, y, theta] at time (t-1) [world_frame] x_t1 : particle state belief [x, y, theta] at time t [world_frame] X_bar : [num_particles x 4] sized array containing [x, y, theta, wt] values for all particles z_t : array of 180 range measurements for each laser scan """ """ Initialize Parameters """ src_path_map = '../data/map/wean.dat' src_path_log = '../data/log/robotdata3.log' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() logfile = open(src_path_log, 'r') #lookup = np.load('lookup_zero.npy') motion_model = MotionModel() sensor_model = SensorModel(occupancy_map, lookup_flag=True) resampler = Resampling() num_particles = 1000 #X_bar = init_particles_random(num_particles, occupancy_map) X_bar = init_particles_freespace(num_particles, occupancy_map) #X_bar = np.array([[6500,1500,1*np.pi/2,1]]) vis_flag = True count = 0 """ Monte Carlo Localization Algorithm : Main Loop """ if vis_flag: visualize_map(occupancy_map) first_time_idx = True for time_idx, line in enumerate(logfile): #vis_flag = count % 1 == 0 #vis_flag = False count += 1 # Read a single 'line' from the log file (can be either odometry or laser measurement) meas_type = line[0] # L : laser scan measurement, O : odometry measurement meas_vals = np.fromstring(line[2:], dtype=np.float64, sep=' ') # convert measurement values from string to double odometry_robot = meas_vals[0:3] # odometry reading [x, y, theta] in odometry frame time_stamp = meas_vals[-1] if ((time_stamp <= 0.0)): # ignore pure odometry measurements for now (faster debugging) #if ((time_stamp <= 0.0) or meas_type == "O"): # ignore pure odometry measurements for now (faster debugging) continue if (meas_type == "L"): odometry_laser = meas_vals[3:6] # [x, y, theta] coordinates of laser in odometry frame ranges = meas_vals[6:-1] # 180 range measurement values from single laser scan print("Processing time step " + str(time_idx) + " at time " + str(time_stamp) + "s") if (first_time_idx): u_t0 = odometry_robot first_time_idx = False continue X_bar_new = np.zeros( (num_particles,4), dtype=np.float64) u_t1 = odometry_robot num_rays = sensor_model.num_rays if meas_type == "O": xqs, yqs, xms, yms = None, None, None, None elif meas_type == "L": xqs = np.zeros((num_particles,num_rays)) yqs = np.zeros((num_particles,num_rays)) xms = np.zeros((num_particles,num_rays)) yms = np.zeros((num_particles,num_rays)) for m in range(0, num_particles): #print(m) """ MOTION MODEL """ x_t0 = X_bar[m, 0:3] #print('before motion: {}, {}, {}'.format(x_t0[0], x_t0[1], 180/np.pi*x_t0[2])) x_t1 = motion_model.update(u_t0, u_t1, x_t0) #print('after motion: {}, {}, {}'.format(x_t1[0], x_t1[1], 180/np.pi*x_t1[2])) x = int(x_t1[0]/10) y = int(x_t1[1]/10) if not(0 <= occupancy_map[y, x] <= 0.0) and meas_type == "L": #print('dumping particle') w_t = 0 X_bar_new[m, :] = np.hstack((x_t1, w_t)) continue """ SENSOR MODEL """ if (meas_type == "L"): z_t = ranges w_t, probs, z_casts, xqs[m], yqs[m], xms[m], yms[m] = sensor_model.beam_range_finder_model(z_t, x_t1) X_bar_new[m,:] = np.hstack((x_t1, w_t)) else: X_bar_new[m,:] = np.hstack((x_t1, X_bar[m,3])) #print(w_t) X_bar = X_bar_new u_t0 = u_t1 if vis_flag: #visualize_timestep(X_bar, time_idx, xqs, yqs, xms, yms) visualize_timestep(X_bar, time_idx) """ RESAMPLING """ if (meas_type == "L"): X_bar = resampler.low_variance_sampler(X_bar)
def get_occupancy_map(): src_path_map = '../data/map/wean.dat' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() return occupancy_map
def main(): """ Description of variables used u_t0 : particle state odometry reading [x, y, theta] at time (t-1) [odometry_frame] u_t1 : particle state odometry reading [x, y, theta] at time t [odometry_frame] x_t0 : particle state belief [x, y, theta] at time (t-1) [world_frame] x_t1 : particle state belief [x, y, theta] at time t [world_frame] particles : [num_particles x 4] sized array containing [x, y, theta, wt] values for all particles z_t : array of 180 range measurements for each laser scan """ """ Initialize Parameters """ ########################################### SET THE NUMBER OF PARTICLES ##################################### num_particles = 10000 ########################################### SET THE NUMBER OF PARTICLES ##################################### src_path_map = '../data/map/wean.dat' src_path_log = '../data/log/robotdata1.log' #src_path_log = '../data/log/robotdata5.log' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() map_size_x = map_obj.get_map_size_x() map_size_y = map_obj.get_map_size_y() logfile = open(src_path_log, 'r') motion_model = MotionModel() sensor_model = SensorModel(occupancy_map) resampler = Resampling() particles = init_particles_freespace(num_particles, occupancy_map) #particles = np.array([[4100,3990,3,1],[4060,3990,3,1],[4000,3990,3,1],[4000,3990,2,1],[6150,1270,1.7,1]]) # The particles above are # In the correct location with approximately the correct angle # Correct angle but a little farther out into the hallway # Correct angle but squarely in the hallway # In the center of the hallway and at wrong angle # Completely wrong in the big room at the bottom vis_flag = 1 """ Monte Carlo Localization Algorithm : Main Loop """ first_time_idx = True lastUsedOdometry = np.array([0, 0, 0]) imageNumber = 0 for time_idx, line in enumerate(logfile): # time_idx is just a counter # line is the text from a line in the file. # Read a single 'line' from the log file (can be either odometry or laser measurement) meas_type = line[ 0] # L : laser scan measurement, O : odometry measurement meas_vals = np.fromstring( line[2:], dtype=np.float64, sep=' ') # convert measurement values from string to double state = meas_vals[ 0:3] # odometry reading [x, y, theta] in odometry frame time_stamp = meas_vals[-1] # if ((time_stamp <= 0.0) | (meas_type == "O")): # ignore pure odometry measurements for now (faster debugging) # continue if (meas_type == "L"): # Laser data odometry_laser = meas_vals[ 3:6] # [x, y, theta] coordinates of laser in odometry frame #print(odometry_laser) delta = odometry_laser - lastUsedOdometry distance = math.sqrt(delta[0] * delta[0] + delta[1] * delta[1]) # Don't update if it didn't move if ((distance < 35) and (delta[2] < .05) ): # Don't update or do anything if the robot is stationary print( "Time: %f\tDistance: %f\tangle: %f\tDidn't move enough..." % (time_stamp, distance, delta[2])) continue print("Time: %f\tDistance: %f\tangle: %f" % (time_stamp, distance, delta[2])) lastUsedOdometry = odometry_laser ranges = meas_vals[ 6:-1] # 180 range measurement values from single laser scan else: #print("Skipping this record because it is an odometry record.") continue #print "Processing time step " + str(time_idx) + " at time " + str(time_stamp) + "s" if (first_time_idx): lastState = state first_time_idx = False continue #particles_new = np.zeros( (num_particles,4), dtype=np.float64) currentState = state # MOTION MODEL - move each particle startTime = time.time() for m in range(num_particles): oldParticle = particles[m, 0:3] particles[m, 0:3] = motion_model.update( lastState, currentState, oldParticle) # This is [X,Y,theta] # # Use this line for testing probabilities # newParticle = particles[m,0:3] ######### Don't update the position of the particle.#################### print("Motion model completed in %s seconds" % (time.time() - startTime)) # Typically takes .125 seconds for # 10000 particles # SENSOR MODEL - find the liklihood of each particle startTime = time.time() for m in range(num_particles): particles[m, 3] = sensor_model.beam_range_finder_model( ranges, particles[m, 0:3]) print("Sensor model completed in %s seconds" % (time.time() - startTime)) # Typically takes 7.85 seconds for # 10000 particles lastState = currentState # print '***********Particles before normalizing***************' # print(particles) # #normalize the weights #minWeight = min(particles[:,3]); #maxWeight = max(particles[:,3]); #weightRng = (maxWeight - minWeight); #if (abs(weightRng)<0.0000001): # particles[:,3] = (1/float(num_particles))*np.ones(num_particles); #else: # particles[:,3] = (particles[:,3] - minWeight)/weightRng; #print '***********Particles after normalizing***************' # #print(particles) #particles = resampler.low_variance_sampler(particles,num_particles) particles = resampler.multinomial_sampler(particles, num_particles) #print("Completed in %s seconds" % (time.time() - startTime)) # this is currently taking about .4 seconds per particle # Resampling typically takes 8 ms for 5000 particles. if vis_flag: imageNumber += 1 visualize_map(occupancy_map, particles, imageNumber)