def __init__(self):
rospy.init_node('pf_node')
# Initialize subscribers to sensors and motors
rospy.Subscriber('/scan', LaserScan, self.read_sensor)
# Initialize publishers for visualization
self.particle_pose_pub = rospy.Publisher('/particle_pose_array',
PoseArray,
queue_size=10)
self.odom_pose_pub = rospy.Publisher('odom_pose',
PoseArray,
queue_size=10)
self.map_marker_pub = rospy.Publisher('/map_marker',
Marker,
queue_size=10)
# self.particle_obstacles_pub = rospy.Publisher('/particle_obstacles', Marker, queue_size=10)
self.latest_scan_ranges = []
# Class initializations
self.p_distrib = ParticleDistribution()
self.motion_model = MotionModel()
self.sensor_model = SensorModel()
self.map_model = MapModel()
self.tf_helper = TFHelper()
# When to run the particle filter
self.distance_moved_threshold = 0.2 # m
self.angle_turned_threshold = 10 # deg
# After map model has been initialized, create the initial particle distribution
self.p_distrib.init_particles(self.map_model)
self.particle_pose_pub.publish(
self.p_distrib.get_particle_pose_array())
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 __init__(self):
rospy.init_node('error_validation_node')
# Initialize subscribers to sensors and motors
rospy.Subscriber('/scan', LaserScan, self.read_sensor)
# Initialize publishers for visualization
self.error_markers_pub = rospy.Publisher('/error_markers',
MarkerArray,
queue_size=10)
self.odom_pose_pub = rospy.Publisher('odom_particle_pose',
Pose,
queue_size=10)
self.latest_scan_ranges = []
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
# Class initializations
self.map_model = MapModel()
self.tf_helper = TFHelper()
self.motion_model = MotionModel()
self.sensor_model = SensorModel()
"""for static error validation"""
self.static_particle = Particle(x=0, y=0, theta=0, weight=1)
self.sample_ranges = np.ones(361)
self.predicted_obstacle_x = 0.0
self.predicted_obstacle_y = 0.0
def main():
plt.figure(1)
motionModel = MotionModel()
pose = initParticles()
plt.scatter(pose[:, 0], pose[:, 1])
init = True
for i in xrange(len(control)):
if (init):
u_t0 = np.array([0, 0, 0])
u_t1 = control[i, :]
for p in xrange(numParticles):
pose[p, :] = motionModel.update(u_t0, u_t1, pose[p, :])
plt.scatter(pose[:, 0], pose[:, 1])
u_t0 = u_t1
plt.show()
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 = 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)
results_L_loc.append(
[floats[3], floats[4], floats[5],
floats[186]]) # x_loc, y_loc, angle, timestamp
results_L.append(floats[6:185]) # 180 laser scans
# results_L = results_L[20:-1]
# -------------------------------MAIN SCRIPT----------------------------------
# -----------------initialize variables-------------------
alpha1 = 0
alpha2 = 0
alpha3 = 0 # increasing these two variables appears to make the particles move farther
alpha4 = 0
mot = MotionModel(alpha1, alpha2, alpha3, alpha4)
M = 1000 # number of particles
map = MapBuilder('../map/wean.dat')
mapList = map.getMap()
mapInit(mapList)
sensorModel = SensorModel('../map/wean.dat')
resampler = Resampling()
# ------------initialize particles throughout map--------
# generate list of locations with '0' value
counter = 0
goodLocs_x = []
goodLocs_y = []
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, 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)
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')
with open(src_path_log, 'r') as f:
logs = f.readlines()
motion_model = MotionModel()
sensor_model = SensorModel(occupancy_map)
resampler = Resampling()
num_particles = 500
# 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}
addition = True
"""
Monte Carlo Localization Algorithm : Main Loop
"""
if vis_flag:
visualize_map(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]
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)
class ParticleFilter(object):
def __init__(self):
rospy.init_node('pf_node')
# Initialize subscribers to sensors and motors
rospy.Subscriber('/scan', LaserScan, self.read_sensor)
# Initialize publishers for visualization
self.particle_pose_pub = rospy.Publisher('/particle_pose_array',
PoseArray,
queue_size=10)
self.odom_pose_pub = rospy.Publisher('odom_pose',
PoseArray,
queue_size=10)
self.map_marker_pub = rospy.Publisher('/map_marker',
Marker,
queue_size=10)
# self.particle_obstacles_pub = rospy.Publisher('/particle_obstacles', Marker, queue_size=10)
self.latest_scan_ranges = []
# Class initializations
self.p_distrib = ParticleDistribution()
self.motion_model = MotionModel()
self.sensor_model = SensorModel()
self.map_model = MapModel()
self.tf_helper = TFHelper()
# When to run the particle filter
self.distance_moved_threshold = 0.2 # m
self.angle_turned_threshold = 10 # deg
# After map model has been initialized, create the initial particle distribution
self.p_distrib.init_particles(self.map_model)
self.particle_pose_pub.publish(
self.p_distrib.get_particle_pose_array())
'''
Function: read_sensor
Inputs: LaserScan scan_msg
Save the ranges of the laser scanner.
'''
def read_sensor(self, scan_msg):
self.latest_scan_ranges = scan_msg.ranges
'''
Function: update_pose_estimate
Inputs:
Returns a new pose estimate after running a particle filter. Called if the robot has
moved or turned enough to merit a new pose estimate.
The current particles (representing hypothetical poses) are propagated based on the
change in odom pose of the robot (how much it has moved since the last estimate). Each
new particle is assigned a new weight based on how likely it is to observe the laser
scan values.
The pose estimate is a weighted average of all of the particle's poses.
'''
def update_pose_estimate(self):
# Propagate the particles because the robot has moved. The sensor update
# should happen on the new poses.
# Display the new pose
self.odom_pose_pub.publish(self.motion_model.get_pose_array())
self.motion_model.propagate(self.p_distrib.particle_list,
self.tf_helper)
# self.p_distrib.print_distribution()
pose_array = self.p_distrib.get_particle_pose_array()
pose_array.header.stamp = rospy.Time(0)
self.particle_pose_pub.publish(pose_array)
# Update particle weights based on the sensor readings.
if (self.latest_scan_ranges != []):
scan_ranges = self.latest_scan_ranges # Assuming this will not change the object if we get a new scan.
self.sensor_model.update_particle_weights(
scan_ranges, self.p_distrib.particle_list, self.map_model)
self.p_distrib.normalize_weights()
# Resample the particle distribution
print("Resample")
self.p_distrib.resample()
self.p_distrib.normalize_weights()
# Display the new distribution
pose_array = self.p_distrib.get_particle_pose_array()
pose_array.header.stamp = rospy.Time(0)
self.particle_pose_pub.publish(pose_array)
new_pose_estimate = self.p_distrib.get_pose_estimate(
) # Just Pose, not stamped
return new_pose_estimate
def publish_map_markers(self):
# figure out and publish map origin
map_origin_pose = self.map_model.occupancy_field.map.info.origin
x = map_origin_pose.position.x
y = map_origin_pose.position.y
point = Point(x, y, 0.0)
quaternion = Quaternion(0, 0, 0, 0)
header = Header(frame_id="map", stamp=rospy.Time.now())
pose = map_origin_pose
scale = Vector3(.1, .1, .1)
color = ColorRGBA(255, 20, 147, 255)
type = 2
map_origin_marker = Marker(header=header,
pose=Pose(position=point,
orientation=quaternion),
color=color,
type=type,
scale=scale)
self.map_marker_pub.publish(map_origin_marker)
# publish
def run(self):
# Send the first map to odom transform using the 0, 0, 0 pose.
self.tf_helper.fix_map_to_odom_transform(Pose(), rospy.Time(0))
while not rospy.is_shutdown():
# continuously broadcast the latest map to odom transform
# Changes to the map to base_link come from our pose estimate from
# the particle filter.
self.publish_map_markers()
self.tf_helper.send_last_map_to_odom_transform()
if (self.motion_model.has_moved_enough(
self.tf_helper, self.distance_moved_threshold,
self.angle_turned_threshold)):
# Run the particle filter
new_pose_estimate = self.update_pose_estimate()
# Update the map to odom transform using new pose estimate
self.tf_helper.fix_map_to_odom_transform(
new_pose_estimate, rospy.Time(0))
def Astar(obstacle,start,goal):
G=0 # NEXT MOVE cost
path=mat([[0,0]]) # Create an empty path, zero will be removed after OPTIMAL PATH found
open_list=mat([[start[0,0]], # current position x
[start[0,1]], # current position y
[G+H(start,goal)], # total cost F=G+H, 当前点到终点的距离
[start[0,0]], # last position x, parent set
[start[0,1]]]).transpose() # last position y, parent set
# initialize CLOSE LIST, this all-zero row will be removed after OPTIMAL PATH found
close_list=mat([[0,0,0,0,0]])
# open_list=mat([[0,0]])
# open_list=delete(open_list,0,axis=0)
next_move=MotionModel() # set NEXT position motion model
findFlag=False # flag to determine whether the path can be found
# print(open_list)
while findFlag==False:
# first column of OPEN LIST is empty
if len(open_list)==0:
print("No path to GOAL.")
return
# sorting open list based on total cost
open_list=open_list[lexsort((open_list.view(ndarray)[:,2],))]
# print(open_list)
# compare the current position to GOAL position
if isSamePosition(open_list[0,0:2],goal):
print("Optimal path found.")
# put first row of OPEN LIST into CLOSE LIST
close_list=vstack((open_list[0,:],close_list))
# remove the least cost MOVE from OPEN LIST
open_list=delete(open_list,0,axis=0)
findFlag=True
break
# calculate the cost in NEXT MOTION MODEL
for index in range(0,len(next_move[:,0])):
m=mat([[open_list[0,0]+next_move[index,0]], # pick NEXT MOVE position x
[open_list[0,1]+next_move[index,1]], # pick NEXT MOVE position y
[0]]).transpose()
G=next_move[index,2]+H(m[0:2],goal) # NEXT MOVE cost G
m[0,2]=G
# print("m =",m)
# skip if current position is OBSTACLE
if isObstacle(m,obstacle):
# print("[{}, {}] is OBSTACLE".format(m[0,0],m[0,1]))
continue
# check that whether the next movement choice is in OPEN LIST or CLOSE LIST
list_flag=FindList(m,open_list,close_list)
# print("list flag =",list_flag)
# if it is in OPEN LIST or CLOSE LIST, skip
if list_flag==1: # in OPEN LIST
# print("[{}, {}] is in OPEN LIST".format(m[0,0],m[0,1]))
continue
elif list_flag==2: # in CLOSE LIST
# print("[{}, {}] is in CLOSE LIST".format(m[0,0],m[0,1]))
continue
else:
# append this MOVE and current position into OPEN LIST
# print("[{}, {}] has appended into OPEN LIST".format(m[0,0],m[0,1]))
temp=hstack((m,[[open_list[0,0]]],[[open_list[0,1]]]))
open_list=vstack((open_list,temp))
# print(open_list)
# if the NEXT MOVE is neither in OPEN LIST nor CLOSE LIST
if findFlag==False:
# append the least cost MOVE into CLOSE LIST, as moved
close_list=vstack((open_list[0,:],close_list))
# print("[{}, {}] has added into PATH".format(close_list[0,0],close_list[0,1]))
# remove the least cost MOVE from OPEN LIST
open_list=delete(open_list,0,axis=0)
# remove the last all-zero row in CLOSE LIST
close_list=delete(close_list,-1,axis=0)
# print(close_list)
# generate OPTIMAL PATH
path=GetPath(close_list,start)
return path
def __init__(self, pHardwareComm, pDatabase):
"""SystemModel constructor"""
# Remember the hardware and database layers
self.hardwareComm = pHardwareComm
self.database = pDatabase
# Pass a pointer to the system model so the hardware layer can update our state
self.hardwareComm.SetSystemModel(self)
# Create the empty system model and a lock to protect it
self.model = {}
self.modelLock = TimedLock.TimedLock()
# Load the system model from the INI file
sSystemModel = "/opt/elixys/core/SystemModel.ini"
if not os.path.exists(sSystemModel):
print "Invalid path to INI files"
return
self.SystemComponents = ConfigObj(sSystemModel)
# Build the system model
for key,value in self.SystemComponents.items():
if key == "CoolingSystem":
if value == "True":
self.model[key] = CoolingSystemModel(key, self.hardwareComm, self.modelLock)
elif key == "VacuumSystem":
if value == "True":
self.model[key] = VacuumSystemModel(key, self.hardwareComm, self.modelLock)
elif key == "Valves":
if value == "True":
self.model[key] = ValvesModel(key, self.hardwareComm, self.modelLock)
elif key == "PressureRegulators":
for nPressureRegulator in range(1, int(value) + 1):
sPressureRegulator = "PressureRegulator" + str(nPressureRegulator)
self.model[sPressureRegulator] = PressureRegulatorModel(sPressureRegulator, nPressureRegulator, self.hardwareComm, self.modelLock)
elif key == "LiquidSensors":
if value == "True":
self.model[key] = LiquidSensorsModel(key, self.hardwareComm, self.modelLock)
elif key == "ReagentDelivery":
if value == "True":
self.model[key] = ReagentDeliveryModel(key, self.hardwareComm, self.modelLock)
elif key[:7] == 'Reactor':
sReactor = key
nReactor = int(key[7:])
nStopcocks = int(value["Stopcocks"])
self.model[sReactor] = {}
self.model[sReactor]["Motion"] = MotionModel(sReactor, nReactor, self.hardwareComm, self.modelLock)
for nStopcock in range(1, nStopcocks + 1):
self.model[sReactor]["Stopcock" + str(nStopcock)] = StopcockValveModel(sReactor, nReactor, nStopcock, self.hardwareComm, self.modelLock)
self.model[sReactor]["Thermocouple"] = TemperatureControlModel(sReactor, nReactor, self.hardwareComm, self.modelLock)
self.model[sReactor]["Stir"] = StirMotorModel(sReactor, nReactor, self.hardwareComm, self.modelLock)
self.model[sReactor]["Radiation"] = RadiationDetectorModel(sReactor, nReactor, self.hardwareComm, self.modelLock)
else:
raise Exception("Unknown system component: " + key)
# Initialize variables
self.stateUpdateThread = None
self.stateUpdateThreadTerminateEvent = None
self.__pStateMonitor = None
self.__bStateMonitorError = False
self.__pUnitOperation = None
self.__pStateObject = None
self.__sStateString = ""
self.__pStateLock = TimedLock.TimedLock()
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)
class MotionModelTest(unittest.TestCase):
def setUp(self):
rospy.init_node("MotionModelTest")
self.tf_helper = TFHelper_Sketch()
self.motion_model = MotionModel()
# def testTFHelperSketchNoMovement(self):
# p = PoseStamped()
# p.header.frame_id = "base_link"
# transformed_p = self.tf_helper.tf_listener.transformPose("odom", p)
#
# self.assertEqual(transformed_p.header.frame_id, "odom")
# self.assertEqual(transformed_p.pose.position.x, 0)
# self.assertEqual(transformed_p.pose.position.y, 0)
# self.assertEqual(transformed_p.pose.orientation.w, 1)
# self.assertEqual(transformed_p.pose.orientation.z, 0)
#
# def testTFHelperSketchRotate90(self):
# self.tf_helper.tf_listener.setNewTransform(dx=0, dy=0, dtheta=radians(90))
# p = PoseStamped()
# p.header.frame_id = "base_link"
# transformed_p = self.tf_helper.tf_listener.transformPose("odom", p)
#
# self.assertEqual(transformed_p.header.frame_id, "odom")
# self.assertEqual(transformed_p.pose.position.x, 0)
# self.assertEqual(transformed_p.pose.position.y, 0)
# self.assertTrue(abs(transformed_p.pose.orientation.w - sqrt(2)/2) < 0.0001)
# self.assertTrue(abs(transformed_p.pose.orientation.z - sqrt(2)/2) < 0.0001)
#
#
# def testTFHelperSketchTranslate(self):
# self.tf_helper.tf_listener.setNewTransform(dx=2, dy=3, dtheta=0)
# p = PoseStamped()
# p.header.frame_id = "base_link"
# transformed_p = self.tf_helper.tf_listener.transformPose("odom", p)
#
# self.assertEqual(transformed_p.header.frame_id, "odom")
# self.assertEqual(transformed_p.pose.position.x, 2)
# self.assertEqual(transformed_p.pose.position.y, 3)
# self.assertEqual(transformed_p.pose.orientation.w, 1)
# self.assertEqual(transformed_p.pose.orientation.z, 0)
#
#
# def testNoChangeInMotion(self):
# self.tf_helper.tf_listener.setNewTransform(dx=0, dy=0, dtheta=0)
# self.assertEqual((0, 0, 0), self.motion_model.get_change_in_motion(self.tf_helper))
#
# def testChangeInMotionTranslation(self):
# self.tf_helper.tf_listener.setNewTransform(dx=2, dy=3.5, dtheta=0)
# self.assertEqual((2, 3.5, 0), self.motion_model.get_change_in_motion(self.tf_helper))
#
# def testChangeInMotionRotation(self):
# self.tf_helper.tf_listener.setNewTransform(dx=0, dy=0, dtheta=radians(67))
# self.assertEqual((0, 0, radians(67)), self.motion_model.get_change_in_motion(self.tf_helper))
#
# def testHasNotMovedEnough(self):
# self.tf_helper.tf_listener.setNewTransform(dx=1, dy=0.5, dtheta=radians(35))
# self.assertFalse(self.motion_model.has_moved_enough(self.tf_helper, 3, 40))
#
# def testHasTurnedEnough(self):
# self.tf_helper.tf_listener.setNewTransform(dx=1, dy=0.5, dtheta=radians(75))
# self.assertTrue(self.motion_model.has_moved_enough(self.tf_helper, 3, 40))
#
# def testHasMovedEnough(self):
# self.tf_helper.tf_listener.setNewTransform(dx=5, dy=0.5, dtheta=radians(5))
# self.assertTrue(self.motion_model.has_moved_enough(self.tf_helper, 3, 40))
#
# def testPropagateParticleNoRotationOneParticleAtOrigin(self):
# self.tf_helper.tf_listener.setNewTransform(dx=5, dy=0.5, dtheta=0)
# p_list = [Particle(x=0, y=0, theta=0)]
# correct_list = [Particle(x=5, y=0.5, theta=0)]
# self.motion_model.propagate(p_list, self.tf_helper)
# self.assertEqual(p_list, correct_list)
#
# def testPropagateParticleOnlyRotationParticles(self):
# self.tf_helper.tf_listener.setNewTransform(dx=0, dy=0, dtheta=radians(42))
# p_list = [Particle(x=0, y=0, theta=0), Particle(x=0.4, y=5, theta=354), Particle(x=5, y=-100, theta=42)]
# correct_list = [Particle(x=0, y=0, theta=42),Particle(x=0.4, y=5, theta=36), Particle(x=5, y=-100, theta=84)]
# self.motion_model.propagate(p_list, self.tf_helper)
# self.assertEqual(p_list, correct_list)
#
# def testPropagateParticleParticlesAtOriginOnlyTranslation(self):
# self.tf_helper.tf_listener.setNewTransform(dx=1, dy=1, dtheta=0)
# p_list = [Particle(x=0, y=0, theta=0),Particle(x=0, y=0, theta=90), Particle(x=0, y=0, theta=45)]
# correct_list = [Particle(x=1, y=1, theta=0),Particle(x=-1, y=1, theta=90), Particle(x=0, y=sqrt(2), theta=45)]
# self.motion_model.propagate(p_list, self.tf_helper)
# self.assertEqual(p_list, correct_list)
#
# def testPropagateParticleParticlesNotAtOriginOnlyTranslation(self):
# self.tf_helper.tf_listener.setNewTransform(dx=1, dy=1, dtheta=0)
# p_list = [Particle(x=1, y=2, theta=0), Particle(x=5, y=-7, theta=90), Particle(x=-0.2, y=-0.5, theta=45)]
# correct_list = [Particle(x=2, y=3, theta=0),Particle(x=4, y=-6, theta=90), Particle(x=-0.2, y=sqrt(2) - 0.5, theta=45)]
# self.motion_model.propagate(p_list, self.tf_helper)
# self.assertEqual(p_list, correct_list)
def testPropagateParticleParticlesAtOriginOnlyTranslationAndRotation(self):
self.tf_helper.tf_listener.setNewTransform(dx=1, dy=1, dtheta=180)
p_list = [
Particle(x=1, y=2, theta=0),
Particle(x=5, y=-7, theta=90),
Particle(x=-0.2, y=-0.5, theta=45)
]
correct_list = [
Particle(x=2, y=3, theta=180),
Particle(x=4, y=-6, theta=270),
Particle(x=-0.2, y=sqrt(2) - 0.5, theta=225)
]
self.motion_model.propagate(p_list, self.tf_helper)
self.assertEqual(p_list, correct_list)
def setUp(self):
rospy.init_node("MotionModelTest")
self.tf_helper = TFHelper_Sketch()
self.motion_model = MotionModel()
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/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/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)
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
X_bar = np.transpose(np.array([4.4000000e+03, 3.95000000e+03, 0.00]))
X_bar = X_bar.reshape((3, 1))
# test = X_bar[0:3,0]
# rays = sensor_model.ray_cast_laser_state(np.transpose(np.array([390,304,0])))
# print(len(rays))
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()
