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)