def particle_filter(ps, control, scan):
global weights
v, w = control
v_dt = v*dt
w_dt = w*dt
sigma = sqrt(v*v + w*w)/6.0 * dt
def motion_update(p):
x, y, t = p
new_p = (x + random.gauss(v_dt*cos(t), sigma),
y + random.gauss(v_dt*sin(t), sigma),
t + random.gauss(w_dt, sigma))
if not mcl_tools.map_hit(new_p[0], new_p[1]):
return new_p
else:
return mcl_tools.random_particle()
'''
p_prime = [motion_update(p) if not mcl_tools.map_hit(p[0], p[1]) else
mcl_tools.random_particle()
for p in ps]
'''
new_weights = np.array([particle_weight(p, scan) for p in ps])
weights *= new_weights
weights /= weights.sum()
wvar = 1./sum([w*w for w in weights])
if wvar < random.gauss(PAR_COUNT*.81, 60):
ps = mcl_tools.random_sample(ps, PAR_COUNT - NOISE, weights) + [mcl_tools.random_particle() for ii in xrange(NOISE)]
weights = [1.0 for ii in xrange(PAR_COUNT)]
else:
pass
return [motion_update(p) for p in ps]
def motion_update(p):
x, y, t = p
new_p = (x + random.gauss(v_dt * cos(t), sigma),
y + random.gauss(v_dt * sin(t), sigma),
t + random.gauss(w_dt, sigma))
if not mcl_tools.map_hit(new_p[0], new_p[1]):
return new_p
else:
return mcl_tools.random_particle()
def motion_update(p):
x, y, t = p
new_p = (x + random.gauss(v_dt*cos(t), sigma),
y + random.gauss(v_dt*sin(t), sigma),
t + random.gauss(w_dt, sigma))
if not mcl_tools.map_hit(new_p[0], new_p[1]):
return new_p
else:
return mcl_tools.random_particle()
def particle_filter(ps, control, scan):
# FIXME: Should really particle filter.
return [mcl_tools.random_particle() for ii in range(PAR_COUNT)]
import rospy
rospy.init_node('with_weights')
import mcl_tools
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
import time
import random
PAR_COUNT = 1500
cmd_vel = None
# State
parset = [mcl_tools.random_particle() for ii in range(PAR_COUNT)]
def particle_weight(particle, scan):
# FIXME: You should assign weight.
return 1.0
def particle_filter(ps, control, scan):
# FIXME: Should really particle filter.
return [mcl_tools.random_particle() for ii in range(PAR_COUNT)]
def got_scan(msg):
global parset
global cmd_vel
def particle_filter(ps, control, scan):
global last_time, PAR_COUNT, MID_LIFE, MID_UNCERT, MID_COUNT, \
LONG_LIFE, LONG_UNCERT, LONG_COUNT
if last_time is None:
last_time = rp.get_rostime()
# probabilistically move all the particles
new_pos = partial(integrate_control_to_distance, control, (rp.get_rostime() - last_time).to_sec())
last_time = rp.get_rostime()
new_ps = []
for part in ps:
# part[0] = x
# part[1] = y
if not mcl_tools.map_hit(part[0], part[1]):
# print "hit"
new_ps.append(new_pos(part))
else:
new_ps.append(mcl_tools.random_particle())
ps = new_ps # update our particle set
# update weights of each particle
weights = []
for part in ps:
weight = particle_weight(scan, part)
weights.append(weight)
# normalize the weights
weights = np.multiply(weights, 1.0 / np.sum(weights))
'''
############ RESAMPLING! ##############
'''
correction = False
# mid level corrections
if MID_COUNT > MID_LIFE: # crisis
correction = True
# resample 20%
ps = mcl_tools.random_sample(ps, PAR_COUNT - MID_UNCERT, weights)
rand_ps = []
for x in range(MID_UNCERT):
rand_ps.append(mcl_tools.random_particle())
ps.extend(rand_ps)
MID_COUNT = 0
print "mid"
return ps
else:
MID_COUNT += 1
# big corrections
if LONG_COUNT > LONG_LIFE:
correction = True
# resample 50%
ps = mcl_tools.random_sample(ps, PAR_COUNT - LONG_UNCERT, weights)
rand_ps = []
for x in range(LONG_UNCERT):
rand_ps.append(mcl_tools.random_particle())
ps.extend(rand_ps)
LONG_COUNT = 0
print "long"
return ps
else:
LONG_COUNT += 1
# no corrections (normal resample all particles)
if not correction:
ps = mcl_tools.random_sample(ps, PAR_COUNT, weights)
return ps
LONG_LIFE = 80 # How many time steps before adding random particles into array
LONG_COUNT = 0
odometry = None
STD_DEV_HIT = 1.0
MAX_DIST = mcl_tools.LASER_MAX
Z_HIT = 0.6
Z_RAND = 0.3
Z_MAX = 0.1
last_time = None
angles = None
# Initially uncertain
parset = [mcl_tools.random_particle() for ii in range(PAR_COUNT)]
# get the weight of a single particle given a laser scan
def particle_weight(scan, particle):
scan_min = scan.angle_min
# scan_inc = scan.angle_increment * 50
scan_inc = scan.angle_increment
prob = 1.0
for i in xrange(len(scan.ranges)):
if i % 50 is 0:
# print "scan: " + str(i)
sensed = scan.ranges[i]
val = scan_min + (i * scan_inc)
traced = mcl_tools.map_range(particle, val)
