def __init__(self, odom_rate, sens_median, sens_std_dev, vel_uniform_dist,
num_particles, base_scan, start_pos=None, debug=False):
# Odom rate is used for re-positioning of particles and assumed pose after applying filter
self.odom_rate = odom_rate
# Need sensory uncertainty distribution for calculating particle beam probabilities
self.sens_median = sens_median
self.sens_std_dev = sens_std_dev
# Need odometry uncertainty for calculating movement probabilities
self.vel_uniform_dist = vel_uniform_dist
# Want to keep track of how many particles we are going to have in the swarm
self.num_particles = num_particles
# If there is a starting position for the robot we want to make use of it
# in order to instantly converge on that spot
self.start_pos = start_pos
# If we want debug messages, let us know!
self.debug = debug
# Pre-calculate factors for calculating beam unertainty since these are
# done millions of times per filter (Well... up to 50 times per beam per
# particle, 30 beams or so, say 1000 particles -> 1.5 million times)
self.laser_prob_f_1 = (1 / (math.sqrt(2.0*math.pi*(self.sens_std_dev**2.0))))
self.laser_prob_f_2 = -0.5 / (self.sens_std_dev**2.0)
# Set up map storage facilities
self.map_s = MapStorage(self.debug)
# Set up a publisher for publishing the (assumed) position of the robot
# found using the particle filter
self.particle_p = ParticlePose("map")
# Set up a transform listener and broadcaster for utility use
self.tf = tf.TransformListener()
# Pre-allocate odometry buffers for use when calculating the movement
# of particles in the swarm
self.lin_vel_buffer = list()
self.ang_vel_buffer = list()
# Receive odometry
self.odom = Odometry()
rospy.Subscriber("odom", Odometry, self.get_odom)
# Receive laser scan
self.scan = LaserScan()
rospy.Subscriber(base_scan, LaserScan, self.get_scan)
# Initialize particle swarm
self.particles = list()
for index in range(self.num_particles):
self.particles.append(copy.deepcopy(Particle()))
self.initiate_particles()
# Sleep for a bit to allow data and transforms to be received
rospy.sleep(0.2)
# Create default marker for display of swarm / debug
point_marker = Marker()
"""
Settings for particle markers with pose
"""
point_marker.type = Marker.ARROW
point_marker.action = Marker.ADD
point_marker.scale.x = 1.25
point_marker.scale.y = 0.25
point_marker.scale.z = 0.25
point_marker.color.r = 0.0
point_marker.color.g = 1.0
point_marker.color.b = 0.0
point_marker.color.a = 1.0
"""
Settings for beam tracing particles
point_marker.type = Marker.SPHERE
point_marker.action = Marker.ADD
point_marker.scale.x = 0.05
point_marker.scale.y = 0.05
point_marker.scale.z = 0.05
point_marker.color.r = 0.0
point_marker.color.g = 1.0
point_marker.color.b = 0.0
point_marker.color.a = 1.0
"""
self.mark_p = MarkerPlacer("rviz_particles", "map", 1000, point_marker)
class ParticleFilter(object):
"""docstring for ParticleFilter"""
def __init__(self, odom_rate, sens_median, sens_std_dev, vel_uniform_dist,
num_particles, base_scan, start_pos=None, debug=False):
# Odom rate is used for re-positioning of particles and assumed pose after applying filter
self.odom_rate = odom_rate
# Need sensory uncertainty distribution for calculating particle beam probabilities
self.sens_median = sens_median
self.sens_std_dev = sens_std_dev
# Need odometry uncertainty for calculating movement probabilities
self.vel_uniform_dist = vel_uniform_dist
# Want to keep track of how many particles we are going to have in the swarm
self.num_particles = num_particles
# If there is a starting position for the robot we want to make use of it
# in order to instantly converge on that spot
self.start_pos = start_pos
# If we want debug messages, let us know!
self.debug = debug
# Pre-calculate factors for calculating beam unertainty since these are
# done millions of times per filter (Well... up to 50 times per beam per
# particle, 30 beams or so, say 1000 particles -> 1.5 million times)
self.laser_prob_f_1 = (1 / (math.sqrt(2.0*math.pi*(self.sens_std_dev**2.0))))
self.laser_prob_f_2 = -0.5 / (self.sens_std_dev**2.0)
# Set up map storage facilities
self.map_s = MapStorage(self.debug)
# Set up a publisher for publishing the (assumed) position of the robot
# found using the particle filter
self.particle_p = ParticlePose("map")
# Set up a transform listener and broadcaster for utility use
self.tf = tf.TransformListener()
# Pre-allocate odometry buffers for use when calculating the movement
# of particles in the swarm
self.lin_vel_buffer = list()
self.ang_vel_buffer = list()
# Receive odometry
self.odom = Odometry()
rospy.Subscriber("odom", Odometry, self.get_odom)
# Receive laser scan
self.scan = LaserScan()
rospy.Subscriber(base_scan, LaserScan, self.get_scan)
# Initialize particle swarm
self.particles = list()
for index in range(self.num_particles):
self.particles.append(copy.deepcopy(Particle()))
self.initiate_particles()
# Sleep for a bit to allow data and transforms to be received
rospy.sleep(0.2)
# Create default marker for display of swarm / debug
point_marker = Marker()
"""
Settings for particle markers with pose
"""
point_marker.type = Marker.ARROW
point_marker.action = Marker.ADD
point_marker.scale.x = 1.25
point_marker.scale.y = 0.25
point_marker.scale.z = 0.25
point_marker.color.r = 0.0
point_marker.color.g = 1.0
point_marker.color.b = 0.0
point_marker.color.a = 1.0
"""
Settings for beam tracing particles
point_marker.type = Marker.SPHERE
point_marker.action = Marker.ADD
point_marker.scale.x = 0.05
point_marker.scale.y = 0.05
point_marker.scale.z = 0.05
point_marker.color.r = 0.0
point_marker.color.g = 1.0
point_marker.color.b = 0.0
point_marker.color.a = 1.0
"""
self.mark_p = MarkerPlacer("rviz_particles", "map", 1000, point_marker)
def get_odom(self, odom):
self.odom = odom
# Buffer odom data for use when updating particles
self.lin_vel_buffer.append(self.odom.twist.twist.linear.x)
self.ang_vel_buffer.append(self.odom.twist.twist.angular.z)
if self.debug:
# print(self.odom)
pass
def get_scan(self, scan):
self.scan = scan
if self.debug:
# print(self.scan)
pass
def initiate_particles(self):
# If we have a starting position, use that
if self.start_pos is not None:
for particle in self.particles:
particle.p[0] = self.start_pos[0]
particle.p[1] = self.start_pos[1]
particle.o = self.start_pos[2]
# If we do not spread the particles out throughout the map
else:
min_x = self.map_s.min_x_pos
max_x = self.map_s.max_x_pos
min_y = self.map_s.min_y_pos
max_y = self.map_s.max_y_pos
for particle in self.particles:
particle.randomize(min_x, max_x, min_y, max_y)
def move_particles(self):
# Copy and zero odom buffers
lin_vel_list = self.lin_vel_buffer
self.lin_vel_buffer = list()
ang_vel_list = self.ang_vel_buffer
self.ang_vel_buffer = list()
# Store the most recent scan for use with filter after movement is
# complete
self.odom_synced_scan = copy.deepcopy(self.scan)
# For each particle change its position and rotation according to each
# buffered data input with a uniform distribution
for particle in self.particles:
for i in range(len(lin_vel_list)):
lin_vel = lin_vel_list[i] + \
np.random.uniform(-self.vel_uniform_dist,
self.vel_uniform_dist)
ang_vel = ang_vel_list[i] + \
np.random.uniform(-self.vel_uniform_dist,
self.vel_uniform_dist)
d_theta = ang_vel / self.odom_rate
d_x = (lin_vel * math.cos(particle.o))/self.odom_rate
d_y = (lin_vel * math.sin(particle.o))/self.odom_rate
particle.p[0] += d_x
particle.p[1] += d_y
particle.o += d_theta
def place_markers(self):
pos_list = list()
or_list = list()
for index in range(len(self.particles)):
# print("placing markers index: " + str(index))
pos_list.append(Point(self.particles[index].p[0], self.particles[index].p[1], 0.0))
quat = quaternion_from_euler(0.0, 0.0, self.particles[index].o)
or_list.append(Quaternion(quat[0], quat[1], quat[2], quat[3]))
self.mark_p.place_marker(pos_list, or_list)
def sensor_probability(self, meas, exp):
f_2 = math.exp(self.laser_prob_f_2 * (meas-exp)**2.0)
prob = self.laser_prob_f_1 * f_2
return prob
def apply_filter(self):
# Copy all scan parameters so that they do not change while we are
# looping through all the parameters since this takes a while
d_theta = self.odom_synced_scan.angle_increment
scan_min_angle = self.odom_synced_scan.angle_min
scan_max_angle = self.odom_synced_scan.angle_max
scan_max_range = self.odom_synced_scan.range_max
ranges = self.odom_synced_scan.ranges
map_res = self.map_s.resolution
# Make an empty list for storing the probability for each particle
probability_list = list()
max_probability = 0
# For all particles:
min_x = self.map_s.min_x_pos
max_x = self.map_s.max_x_pos
min_y = self.map_s.min_y_pos
max_y = self.map_s.max_y_pos
for i in range(len(self.particles)):
# Check if the particle is in a valid position. If it is not
# regenerate the position.
x_r_m = self.particles[i].p[0]
y_r_m = self.particles[i].p[1]
while self.map_s.xy_pos_is_occupied(x_r_m, y_r_m):
self.particles[i].randomize(min_x, max_x, min_y, max_y)
x_r_m = self.particles[i].p[0]
y_r_m = self.particles[i].p[1]
# Get P(x_r_m,y_r_m,theta_r_m) (robot in map frame) from stored
# particle
point_r_m = PointStamped()
point_r_m.point.x = self.particles[i].p[0]
point_r_m.point.y = self.particles[i].p[1]
point_r_m.point.z = 0.0
point_r_m.header.frame_id = "base_link"
point_r_m.header.stamp = self.tf.getLatestCommonTime("base_link", "base_laser_link")
# Transform in to L(x_L_m,y_L_m,theta_L_m) (laser in map frame)
# using /base_link to /base_laser_link (need a transform listener!)
point_l_m = self.tf.transformPoint("base_laser_link", point_r_m)
x_l_m = point_l_m.point.x
y_l_m = point_l_m.point.y
# The orientation of the laser scanner in the map frame is the
# same as the robot, so we are taking a shortcut here
theta_l_m = self.particles[i].o
# Publish a transform that will be used to translate from a point
# in the laser frame of reference to the laser in map frame of
# reference.
theta_b_l = scan_min_angle
theta_b_l_max = scan_max_angle
beam_index = 0
probability = 0
"""
Debug to trace beam!
pos_list = list()
or_list = list()
"""
# For each beam angle theta_b_L (beam in laser frame of reference)
while theta_b_l < theta_b_l_max + d_theta/10.0:
# Find check angle theta_b_m =
# theta_b_L + theta_L_m (beam in map frame of reference)
theta_b_m = theta_l_m + theta_b_l
x_slope = math.cos(theta_b_m)*float(map_res)
y_slope = math.sin(theta_b_m)*float(map_res)
# For each N deltaDistance, N = 1 ->
# N = maxDistance/resolution, deltaDistance = map resolution
for N in range(1, int(scan_max_range/map_res)):
# Check if x_b_m =
# x_L_m + x_b_L =
# x_L_m + N*dDist*cos(theta_L_m + theta_b_L) and
# y_b_m =
# y_L_m + y_b_L =
# y_L_m + N*dDist*sin(theta_L_m + theta_b_L) is occupied.
x_b_m = x_l_m + float(N)*x_slope
y_b_m = y_l_m + float(N)*y_slope
"""
Debug to trace beam!
pos_list.append(Point(x_b_m, y_b_m, 0.0))
or_list.append(Quaternion(0.0, 0.0, 0.0, 0.0))
"""
# If occupied calculate expected distance between them and
# exit loop
if self.map_s.xy_pos_is_occupied(x_b_m, y_b_m):
exp_dist = self.dist(x_l_m, y_l_m, x_b_m, y_b_m)
break
# If no intersection was detected use max range as expected
# distance
# (PS: I was amazed this was possible - an else clause if the
# break in the loop was not executed!)
else:
exp_dist = scan_max_range
# Calculate probability for this beam angle
measured_range = np.random.normal(ranges[beam_index], self.sens_std_dev)
exp_dist = exp_dist
"""
measured_range = Decimal(ranges[beam_index])
exp_dist = Decimal(exp_dist)
"""
probability_beam = self.sensor_probability(measured_range, exp_dist)
# Add to previous calculated probabilities
probability += probability_beam
# Increment theta of beam and index of beam
theta_b_l += 2.0*d_theta
beam_index += 2
"""
Debug to trace beam
self.mark_p.place_marker(pos_list, or_list)
"""
# Add probability for particle to list
probability_list.append(probability)
# Check if the beam probability is the "best", if it is store the
# position as the assumed particle swarm position
if probability > max_probability:
max_probability = probability
self.avg_x = x_r_m
self.avg_y = y_r_m
self.avg_theta = theta_l_m
# print(probability_list)
# Do hat-search using the weighting in probability list and add found
# particles to new list
new_list = self.hat_search(probability_list)
# Assign new list to be used for next iteration
self.particles = new_list
def dist(self, x0, y0, x1, y1):
d_x = abs(x0-x1)
d_y = abs(y0-y1)
return math.sqrt(d_x**2.0 + d_y**2.0)
def hat_search(self, prob_list):
new_part_list = list()
sum_x = 0.0
sum_y = 0.0
sum_theta = 0.0
sum_count = 0
total = sum(prob_list)
# Ensure that we have at least one value with probability larger than 0
if total >= 0:
end_hat = len(self.particles) - len(self.particles)/20
for i in range(end_hat):
rnd_num = sp.random.uniform(0.0, total)
pick_index = 0
picking = True
while picking:
rnd_num -= prob_list[pick_index]
if rnd_num <= 0:
# Add x, y, theta to sum
"""
sum_x += self.particles[pick_index].p[0]
sum_y += self.particles[pick_index].p[1]
sum_theta += self.particles[pick_index].o
sum_count += 1
"""
new_part = Particle([self.particles[pick_index].p[0],
self.particles[pick_index].p[1]],
self.particles[pick_index].o)
new_part_list.append(new_part)
picking = False
else:
pick_index += 1
min_x = self.map_s.min_x_pos
max_x = self.map_s.max_x_pos
min_y = self.map_s.min_y_pos
max_y = self.map_s.max_y_pos
for i in range(end_hat, len(self.particles)):
new_part = Particle()
new_part.randomize(min_x, max_x, min_y, max_y)
new_part_list.append(copy.deepcopy(new_part))
# If the total for some reason ended up being all zeroes let us just
# use the old list, but also need to do the centre of mass calculation
# for publishing centre off mass
else:
new_part_list = self.particles
for i in range(self.particles):
# Add x, y, theta to sum
sum_x += self.particles[i].p[0]
sum_y += self.particles[i].p[1]
sum_theta += self.particles[i].o
sum_count += 1
"""
self.avg_x = sum_x/sum_count
self.avg_y = sum_y/sum_count
self.avg_theta = sum_theta/sum_count
"""
print("total sum: " + str(total))
return new_part_list
def run(self):
# First we want to update the particle locations using the most recent
# odometry data
self.move_particles()
# Then apply filter to produce a list of new and higher probability
# particles
self.apply_filter()
# Place a selection of markers in rviz to illustrate the particle cloud
# self.mark_p.clear_markers()
# self.place_markers()
# Update the position with most recent odometry updates received since
# particle filter was started
self.update_particle_pose()
# Publish the centre off mass position and orientation for particles
avg_rot = quaternion_from_euler(0.0, 0.0, self.avg_theta)
self.particle_p.publish(Point(self.avg_x, self.avg_y, 0.0), Quaternion(*avg_rot))
def update_particle_pose(self):
lin_vel_list = self.lin_vel_buffer
ang_vel_list = self.ang_vel_buffer
for i in range(len(lin_vel_list)):
lin_vel = lin_vel_list[i] + \
np.random.uniform(-self.vel_uniform_dist,
self.vel_uniform_dist)
ang_vel = ang_vel_list[i] + \
np.random.uniform(-self.vel_uniform_dist,
self.vel_uniform_dist)
d_theta = ang_vel / self.odom_rate
d_x = (lin_vel * math.cos(self.avg_theta))/self.odom_rate
d_y = (lin_vel * math.sin(self.avg_theta))/self.odom_rate
self.avg_x += d_x
self.avg_y += d_y
self.avg_theta += d_theta