def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
# TODO: implement this
#print str(msg.range[0])
#We ctually need to go through all of the range so a for loop form 0 to 360
#We also need to delete any ranges that return 0 because that is a false reading and causes problems
#print msg
for part in self.particle_cloud:
total_distace = 0
average_distance = 0
count = 0
for angle in range(359):
distance_in_front = msg.ranges[angle]
if distance_in_front == 0:
pass
else:
count +=1
rad = angle/360 * 2 * math.pi
part.x = part.x + distance_in_front*math.cos(part.theta + rad)
part.y = part.y + distance_in_front*math.sin(part.theta + rad)
distance = OccupancyField.get_closest_obstacle_distance(self.occupancy_field, part.x, part.y)
total_distace += distance
average_distance = total_distace/count
part.w=(math.e**((-average_distance)**2))
class ParticleMatcher(object):
#map is a 2D array passed with a singular data value on whether or not there is
#something in the coordinate
def __init__(self):
#self.map_ = map_
self.OF = OccupancyField()
def d2p(self, d, eps=1e-3):
# return d.max() - d + eps # linear-mode
# return (1.0 / (d + eps)) # inverse-mode
kp, kx = 0.5, 1.0 # configure as p=50% match at 1.0m distance
k = (-np.log(kp) / kx)
return np.exp(-k * d)
def match(self, particle_list, scan, min_num=5):
if (len(scan) <= min_num):
return None
#scan is a list of [angle, theta]
min_dist = np.min(scan[:, 1])
#print('ps', particle_list)
#dist = [self.OF.get_closest_obstacle_distance(p[0], p[1]) for p in particle_list]
ps = particle_list
dist = self.OF.get_closest_obstacle_distance(ps[:, 0], ps[:, 1])
dist = np.asarray(dist, dtype=np.float32)
dist[np.isnan(
dist
)] = np.inf # WARNING : setting to np.inf doesn't work for linear-mode d2p.
cost = np.abs(np.subtract(dist, min_dist))
#print('dist stats : min {} max {} std {}'.format(dist.min(),dist.max(),dist.std()))
#cost = np.abs(np.subtract(dist, min_dist))
#cost[np.isnan(cost)] = 0 # set nan cost to zero to prevent artifacts
#weight = cost.max() - cost + 1e-3
weight = self.d2p(cost)
#weight = 1.0 / cost
# TODO : determine inverse vs. linear cost performance comparison
#weight[np.isnan(dist)] = 0 # set nan weight to zero to make sure it doesn't get sampled
#print('ws', weight)
return weight
class MapModel(object):
def __init__(self):
self.occupancy_field = OccupancyField()
'''
Function: get_valid_point
Inputs:
Generate a random point from the map.
'''
def get_valid_point(self):
#TODO: Use the pose estimate function, like in the example. This may need to move out of map model
# x axis is width
max_width = self.occupancy_field.map.info.width * self.occupancy_field.map.info.resolution / 4.0
# y axis is height
max_height = self.occupancy_field.map.info.height * self.occupancy_field.map.info.resolution / 4.0
return (random.uniform(-1.0 * max_width, max_width),
random.uniform(-1.0 * max_height, max_height))
'''
Function: get_predicted_obstacle_error
Inputs: int distance_reading (measurement from the laser scan)
float x: (map frame)
float y: (map frame)
int angle: degrees (map frame) (must have accounted for robot's yaw)
Use a laser scan reading to project the theoretical position of where an obstacle
should be in a direction (angle).
Return the distance to the closest obstacle from this position on the given map.
This distance corresponds to the error between the predicted obstacle position
and its location on the map given a hypothetical robot position.
A smaller error means the reading is more likely from the given position
'''
def get_predicted_obstacle_error(self, distance_reading, x, y, angle):
# Predict location of objects based on laser scan reading.
predicted_obstacle_x, predicted_obstacle_y = self.move_coordinate(
x, y, angle, distance_reading)
# Find the closest obstacle to the predicted obstacle position
predicted_reading = self.occupancy_field.get_closest_obstacle_distance(
predicted_obstacle_x, predicted_obstacle_y)
print("Predicted x: {}, y: {}, reading: {}".format(
predicted_obstacle_x, predicted_obstacle_y, predicted_reading))
return predicted_reading
'''
Function: move_coordinate
Inputs: float x
float y
int angle: degrees
int distance
Return a point which is the given distance away from (x, y) in the angle direction.
'''
def move_coordinate(self, x, y, angle, distance):
return (x + cos(radians(angle)) * distance,
y + sin(radians(angle)) * distance)
def get_origin_marker(self):
origin_pose = self.occupancy_field.map.info.origin
"""header = Header(frame_id = "map",stamp=rospy.Time(0))
pose = origin_pose
scale = Vector3(1,1,1)
color = ColorRGBA(255,0,0,255)
type = 2
origin_marker = Marker(header=header,pose=pose,scale=scale,color=color,type=type)"""
return origin_pose
class ParticleFilter:
""" The class that represents a Particle Filter ROS Node
Attributes list:
initialized: a Boolean flag to communicate to other class methods that initializaiton is complete
base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots)
map_frame: the name of the map coordinate frame (should be "map" in most cases)
odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases)
scan_topic: the name of the scan topic to listen to (should be "scan" in most cases)
n_particles: the number of particles in the filter
d_thresh: the amount of linear movement before triggering a filter update
a_thresh: the amount of angular movement before triggering a filter update
laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation
pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI)
particle_pub: a publisher for the particle cloud
laser_subscriber: listens for new scan data on topic self.scan_topic
tf_listener: listener for coordinate transforms
tf_broadcaster: broadcaster for coordinate transforms
particle_cloud: a list of particles representing a probability distribution over robot poses
current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed.
The pose is expressed as a list [x,y,theta] (where theta is the yaw)
map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid
"""
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node('pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.n_particles = 500 # the number of particles to use
self.d_thresh = 0.2 # the amount of linear movement before performing an update
self.a_thresh = math.pi/6 # the amount of angular movement before performing an update
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
self.sigma = 0.1
# TODO: define additional constants if needed
# Setup pubs and subs
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10)
# laser_subscriber listens for data from the lidar
self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.particle_cloud = []
self.current_odom_xy_theta = []
rospy.wait_for_service("static_map")
static_map = rospy.ServiceProxy("static_map", GetMap)
try:
map = static_map().map
except:
print("Could not receive map")
# for now we have commented out the occupancy field initialization until you can successfully fetch the map
self.occupancy_field = OccupancyField(map)
self.initialized = True
def update_robot_pose(self):
""" Update the estimate of the robot's pose given the updated particles.
There are two logical methods for this:
(1): compute the mean pose
(2): compute the most likely pose (i.e. the mode of the distribution)
"""
# first make sure that the particle weights are normalized
self.normalize_particles()
mean_x = 0
mean_y = 0
mean_theta = 0
mean_x_vector = 0
mean_y_vector = 0
for p in self.particle_cloud:
mean_x += p.x*p.w
mean_y += p.y*p.w
mean_x_vector += math.cos(p.theta)*p.w
mean_y_vector += math.sin(p.theta)*p.w
mean_theta = math.atan2(mean_y_vector, mean_x_vector)
self.robot_pose = Particle(x=mean_x,y=mean_y,theta=mean_theta).as_pose()
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = {'x': new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
'y': new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
'theta': new_odom_xy_theta[2] - self.current_odom_xy_theta[2]}
delta['r'] = math.sqrt(delta['x']**2 + delta['y']**2)
delta['rot'] = angle_diff(math.atan2(delta['y'],delta['x']), old_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
for p in self.particle_cloud:
p.x += delta['r']*math.cos(delta['rot'] + p.theta)
p.y += delta['r']*math.sin(delta['rot'] + p.theta)
p.theta += delta['theta']
def map_calc_range(self,x,y,theta):
""" Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """
# TODO(NOPE): nothing unless you want to try this alternate likelihood model
pass
def resample_particles(self):
""" Resample the particles according to the new particle weights.
The weights stored with each particle should define the probability that a particular
particle is selected in the resampling step. You may want to make use of the given helper
function draw_random_sample.
"""
# make sure the distribution is normalized
self.normalize_particles()
indices = [i for i in range(len(self.particle_cloud))]
probs = [p.w for p in self.particle_cloud]
# print('b')
# print(probs)
new_indices = self.draw_random_sample(choices=indices, probabilities=probs, n=(self.n_particles))
new_particles = []
for i in new_indices:
clean_index = int(i)
old_particle = self.particle_cloud[clean_index]
new_particles.append(Particle(x=old_particle.x+gauss(0,.05),y=old_particle.y+gauss(0,.05),theta=old_particle.theta+gauss(0,.05)))
self.particle_cloud = new_particles
self.normalize_particles()
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
for p in self.particle_cloud:
weight_sum = 0
for i in range(360):
n_o = p.nearest_obstacle(i, msg.ranges[i])
error = self.occupancy_field.get_closest_obstacle_distance(n_o[0], n_o[1])
weight_sum += math.exp(-error*error/(2*self.sigma**2))
p.w = weight_sum / 360
# print(p.w)
self.normalize_particles()
@staticmethod
def weighted_values(values, probabilities, size):
""" Return a random sample of size elements from the set values with the specified probabilities
values: the values to sample from (numpy.ndarray)
probabilities: the probability of selecting each element in values (numpy.ndarray)
size: the number of samples
"""
bins = np.add.accumulate(probabilities)
return values[np.digitize(random_sample(size), bins)]
@staticmethod
def draw_random_sample(choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(random_sample(n), bins)]
samples = []
for i in inds:
samples.append(deepcopy(choices[int(i)]))
return samples
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose)
self.initialize_particle_cloud(xy_theta)
self.fix_map_to_odom_transform(msg)
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
for i in range(self.n_particles):
self.particle_cloud.append(Particle(x=xy_theta[0]+gauss(0,0.25),y=xy_theta[1]+gauss(0,0.25),theta=xy_theta[2]+gauss(0,0.25)))
self.normalize_particles()
self.update_robot_pose()
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
w = [deepcopy(p.w) for p in self.particle_cloud]
z = sum(w)
print(z)
if z > 0:
for i in range(len(self.particle_cloud)):
self.particle_cloud[i].w = w[i] / z
else:
for i in range(len(self.particle_cloud)):
self.particle_cloud[i].w = 1/len(self.particle_cloud)
print(sum([p.w for p in self.particle_cloud]))
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not(self.initialized):
# wait for initialization to complete
return
if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
p = PoseStamped(header=Header(stamp=rospy.Time(0),
frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame,p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not(self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose() # update robot's pose
self.resample_particles() # resample particles to focus on areas of high density
self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def fix_map_to_odom_transform(self, msg):
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer """
(translation, rotation) = convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation),
header=Header(stamp=msg.header.stamp,frame_id=self.base_frame))
self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
(self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map
to odom transformation. This is necessary so things like
move_base can work properly. """
if not(hasattr(self,'translation') and hasattr(self,'rotation')):
return
self.tf_broadcaster.sendTransform(self.translation,
self.rotation,
rospy.get_rostime(),
self.odom_frame,
self.map_frame)
class ParticleFilter:
""" The class that represents a Particle Filter ROS Node
Attributes list:
initialized: a Boolean flag to communicate to other class methods that initializaiton is complete
base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots)
map_frame: the name of the map coordinate frame (should be "map" in most cases)
odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases)
scan_topic: the name of the scan topic to listen to (should be "scan" in most cases)
n_particles: the number of particles in the filter
d_thresh: the amount of linear movement before triggering a filter update
a_thresh: the amount of angular movement before triggering a filter update
laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation
pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI)
particle_pub: a publisher for the particle cloud
laser_subscriber: listens for new scan data on topic self.scan_topic
tf_listener: listener for coordinate transforms
tf_broadcaster: broadcaster for coordinate transforms
particle_cloud: a list of particles representing a probability distribution over robot poses
current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed.
The pose is expressed as a list [x,y,theta] (where theta is the yaw)
map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid
"""
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node(
'pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "stable_scan" # the topic where we will get laser scans from
self.n_particles = 100 # the number of particles to use
self.d_thresh = 0.12 # the amount of linear movement before performing an update
self.a_thresh = math.pi / 5 # the amount of angular movement before performing an update
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
# Define additional constants
self.initial_position_deviation = 0.3 # the std deviation (meters) to use for the initial particles' position distribution
self.initial_angle_deviation = math.pi / 6 # the std deviation (degrees) to use for the initial particles' angle distribution
self.resample_position_deviation = 0.05
self.resample_angle_deviation = math.pi / 10
# Setup pubs and subs
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
# laser_subscriber listens for data from the lidar
rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.particle_cloud = []
# change use_projected_stable_scan to True to use point clouds instead of laser scans
self.use_projected_stable_scan = False
self.last_projected_stable_scan = None
if self.use_projected_stable_scan:
# subscriber to the odom point cloud
rospy.Subscriber("projected_stable_scan", PointCloud,
self.projected_scan_received)
self.current_odom_xy_theta = []
# request the map from the map server, the map should be of type nav_msgs/OccupancyGrid
# TODO: fill in the appropriate service call here. The resultant map should be assigned be passed
# into the init method for OccupancyField
rospy.wait_for_service('static_map')
getMap = rospy.ServiceProxy('static_map', GetMap)
# for now we have commented out the occupancy field initialization until you can successfully fetch the map
self.occupancy_field = OccupancyField(getMap().map)
self.initialized = True
def update_robot_pose(self):
""" Update the estimate of the robot's pose given the updated particles.
There are two logical methods for this:
(1): compute the mean pose
(2): compute the most likely pose (i.e. the mode of the distribution)
"""
# First make sure that the particle weights are normalized
self.normalize_particles()
x, y, unit_x, unit_y = (0, 0, 0, 0)
for p in self.particle_cloud:
x += p.x
y += p.y
unit_x += math.cos(p.theta)
unit_y += math.sin(p.theta)
# Assign the latest pose into self.robot_pose as a geometry_msgs.Pose object
self.robot_pose = Pose(
Point(x / len(self.particle_cloud), y / len(self.particle_cloud),
0),
Quaternion(*tf.transformations.quaternion_from_euler(
0, 0, math.atan2(unit_y, unit_x))))
def projected_scan_received(self, msg):
self.last_projected_stable_scan = msg
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
dx, dy, dtheta = delta
ds = math.hypot(dx, dy)
rotationOne = math.atan2(dy, dx) - self.current_odom_xy_theta[2]
rotationTwo = dtheta - rotationOne
for particle in self.particle_cloud:
# Rotate particle to face its destination coordinate
particle.rotate(rotationOne)
# Advance the particle forward in its coordinate frame
particle.x += ds * math.cos(particle.theta)
particle.y += ds * math.sin(particle.theta)
# Rotate particle to face its final heading
particle.rotate(rotationTwo)
def map_calc_range(self, x, y, theta):
""" Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """
# TODO: nothing unless you want to try this alternate likelihood model
pass
def resample_particles(self):
""" Resample the particles according to the new particle weights.
The weights stored with each particle should define the probability that a particular
particle is selected in the resampling step. You may want to make use of the given helper
function draw_random_sample.
"""
# make sure the distribution is normalized
self.normalize_particles()
self.particle_cloud = self.draw_random_sample(
self.particle_cloud, [p.w for p in self.particle_cloud],
self.n_particles)
# Add noise to resample
self.particle_cloud = [
Particle(
float(normal(p.x, self.resample_position_deviation)),
float(normal(p.y, self.resample_position_deviation)),
float(normal(p.theta, self.resample_angle_deviation)),
) for p in self.particle_cloud
]
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
for particle in self.particle_cloud:
coordinate_list = []
x, y, theta = (particle.x, particle.y, particle.theta)
for lidarAngle, dist in enumerate(msg.ranges):
if dist != 0.0:
lidarAngle = math.radians(lidarAngle)
angle = angle_normalize(theta + lidarAngle)
coordinate_list.append((x + dist * math.cos(angle),
y + dist * math.sin(angle)))
likelihood = 0
for point in coordinate_list:
closestDist = self.occupancy_field.get_closest_obstacle_distance(
point[0], point[1])
likelihood += math.e**-(closestDist * 100)
particle.w *= likelihood
@staticmethod
def draw_random_sample(choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(random_sample(n), bins)]
samples = []
for i in inds:
samples.append(deepcopy(choices[int(i)]))
return samples
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose)
self.initialize_particle_cloud(xy_theta)
self.fix_map_to_odom_transform(msg)
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
x, y, theta = xy_theta
self.particle_cloud = [
Particle(
float(normal(x, self.initial_position_deviation)),
float(normal(y, self.initial_position_deviation)),
float(normal(theta, self.initial_angle_deviation)),
) for n in xrange(self.n_particles)
]
self.normalize_particles()
self.update_robot_pose()
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
normFactor = sum([p.w for p in self.particle_cloud])
for particle in self.particle_cloud:
particle.w /= normFactor
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(
PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not (self.initialized):
# wait for initialization to complete
return
if not (self.tf_listener.canTransform(
self.base_frame, msg.header.frame_id, msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame,
msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
p = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not (self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (len(self.current_odom_xy_theta) < 3
or math.fabs(new_odom_xy_theta[0] -
self.current_odom_xy_theta[0]) > self.d_thresh
or math.fabs(new_odom_xy_theta[1] -
self.current_odom_xy_theta[1]) > self.d_thresh
or math.fabs(new_odom_xy_theta[2] -
self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
if self.last_projected_stable_scan:
last_projected_scan_timeshift = deepcopy(
self.last_projected_stable_scan)
last_projected_scan_timeshift.header.stamp = msg.header.stamp
self.scan_in_base_link = self.tf_listener.transformPointCloud(
"base_link", last_projected_scan_timeshift)
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose() # update robot's pose
self.resample_particles(
) # resample particles to focus on areas of high density
self.fix_map_to_odom_transform(
msg
) # update map to odom transform now that we have new particles
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def fix_map_to_odom_transform(self, msg):
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer
TODO: if you want to learn a lot about tf, reimplement this... I can provide
you with some hints as to what is going on here. """
(translation,
rotation) = convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=convert_translation_rotation_to_pose(
translation, rotation),
header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame))
self.tf_listener.waitForTransform(self.base_frame,
self.odom_frame, msg.header.stamp,
rospy.Duration(1.0))
self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
(self.translation,
self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map
to odom transformation. This is necessary so things like
move_base can work properly. """
if not (hasattr(self, 'translation') and hasattr(self, 'rotation')):
return
self.tf_broadcaster.sendTransform(self.translation, self.rotation,
rospy.get_rostime(), self.odom_frame,
self.map_frame)
class ParticleFilter(object):
"""
Class to represent Particle Filter ROS Node
Subscribes to /initialpose for initial pose estimate
Publishes top particle estimate to /particlebest and all particles in cloud to /particlecloud
"""
def __init__(self):
"""
__init__ function to create main attributes, setup threshold values, setup rosnode subs and pubs
"""
rospy.init_node('pf')
self.initialized = False
self.num_particles = 150
self.d_thresh = 0.2 # the amount of linear movement before performing an update
self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update
self.particle_cloud = []
self.lidar_points = []
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
self.best_particle_pub = rospy.Publisher("particlebest",
PoseStamped,
queue_size=10)
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.best_guess = (
None, None) # (index of particle with highest weight, its weight)
self.particles_to_replace = .075
self.n_effective = 0 # this is a measure of the particle diversity
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
# laser_subscriber listens for data from the lidar
rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)
# create instances of two helper objects that are provided to you
# as part of the project
self.current_odom_xy_theta = []
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.initialized = True
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
msg.pose.pose)
print("xy_theta", xy_theta)
self.initialize_particle_cloud(
msg.header.stamp,
xy_theta) # creates particle cloud at position passed in
# by message
print("INITIALIZING POSE")
# Use the helper functions to fix the transform
def initialize_particle_cloud(self, timestamp, xy_theta):
"""
Creates initial particle cloud based on robot pose estimate position
"""
self.particle_cloud = []
angle_variance = math.pi / 10 # POint the points in the general direction of the robot
x_cur = xy_theta[0]
y_cur = xy_theta[1]
theta_cur = self.transform_helper.angle_normalize(xy_theta[2])
# print("theta_cur: ", theta_cur)
for i in range(self.num_particles):
# Generate values for and add a new particle!!
x_rel = random.uniform(-.3, .3)
y_rel = random.uniform(-.3, .3)
new_theta = (random.uniform(theta_cur - angle_variance,
theta_cur + angle_variance))
# TODO: Could use a tf transform to add x and y in the robot's coordinate system
new_particle = Particle(x_cur + x_rel, y_cur + y_rel, new_theta)
self.particle_cloud.append(new_particle)
print("Done initializing particles")
self.normalize_particles()
# publish particles (so things like rviz can see them)
self.publish_particles()
print("normalized correctly")
self.update_robot_pose(timestamp)
print("updated robot pose")
def normalize_particles(self):
"""
Normalizes particle weights to total but retains weightage
"""
total_weights = sum([particle.w for particle in self.particle_cloud])
# if your weights aren't normalized then normalize them
if total_weights != 1.0:
for i in self.particle_cloud:
i.w = i.w / total_weights
def update_robot_pose(self, timestamp):
""" Update the estimate of the robot's pose in the map frame given the updated particles.
There are two logical methods for this:
(1): compute the mean pose based on all the high weight particles
(2): compute the most likely pose (i.e. the mode of the distribution)
"""
# first make sure that the particle weights are normalized
self.normalize_particles()
print("Normalized particles in update robot pose")
# create average pose for robot pose based on entire particle cloud
average_x = 0
average_y = 0
average_theta = 0
# walk through all particles, calculate weighted average for x, y, z, in particle map.
for p in self.particle_cloud:
average_x += p.x * p.w
average_y += p.y * p.w
average_theta += p.theta * p.w
# # create new particle representing weighted average values, pass in Pose to new robot pose
self.robot_pose = Particle(average_x, average_y,
average_theta).as_pose()
print(timestamp)
self.transform_helper.fix_map_to_odom_transform(
self.robot_pose, timestamp)
print("Done fixing map to odom")
def publish_particles(self):
"""
Publish entire particle cloud as pose array for visualization in RVIZ
Also publish the top / best particle based on its weight
"""
# Convert the particles from xy_theta to pose!!
pose_particle_cloud = []
for p in self.particle_cloud:
pose_particle_cloud.append(p.as_pose())
self.particle_pub.publish(
PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=pose_particle_cloud))
# doing shit based off best pose
best_pose_quat = max(self.particle_cloud,
key=attrgetter('w')).as_pose()
#self.best_particle_pub.publish(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), pose=best_pose_quat)
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
# TODO: FIX noise incorporation into movement.
min_travel = 0.2
xy_spread = 0.02 / min_travel # More variance with driving forward
theta_spread = .005 / min_travel
random_vals_x = np.random.normal(0, abs(delta[0] * xy_spread),
self.num_particles)
random_vals_y = np.random.normal(0, abs(delta[1] * xy_spread),
self.num_particles)
random_vals_theta = np.random.normal(0, abs(delta[2] * theta_spread),
self.num_particles)
for p_num, p in enumerate(self.particle_cloud):
# compute phi, or basically the angle from 0 that the particle
# needs to be moving - phi equals OG diff angle - robot angle + OG partilce angle
# ADD THE NOISE!!
noisy_x = (delta[0] + random_vals_x[p_num])
noisy_y = (delta[1] + random_vals_y[p_num])
ang_of_dest = math.atan2(noisy_y, noisy_x)
# calculate angle needed to turn in angle_to_dest
ang_to_dest = self.transform_helper.angle_diff(
self.current_odom_xy_theta[2], ang_of_dest)
d = math.sqrt(noisy_x**2 + noisy_y**2)
phi = p.theta + ang_to_dest
p.x += math.cos(phi) * d
p.y += math.sin(phi) * d
p.theta += self.transform_helper.angle_normalize(
delta[2] + random_vals_theta[p_num])
self.current_odom_xy_theta = new_odom_xy_theta
def update_particles_with_laser(self, msg):
"""
calculate particle weights based off laser scan data passed into param
"""
# print("Updating particles with Laser")
lidar_points = msg.ranges
for p_deg, p in enumerate(self.particle_cloud):
# do we need to compute particle pos in diff frame?
p.occ_scan_mapped = [] # reset list
for scan_distance in lidar_points:
# handle edge case
if scan_distance == 0.0:
continue
# calc a delta theta and use that to overlay scan data onto the particle headings
pt_rad = deg2rad(p_deg)
particle_pt_theta = self.transform_helper.angle_normalize(
p.theta + pt_rad)
particle_pt_x = p.x + math.cos(
particle_pt_theta) * scan_distance
particle_pt_y = p.y + math.sin(
particle_pt_theta) * scan_distance
# calculate distance from every single scan point in particle frame
occ_value = self.occupancy_field.get_closest_obstacle_distance(
particle_pt_x, particle_pt_y)
# Think about cutting off max penalty if occ_value is too big
p.occ_scan_mapped.append(occ_value)
# assign weights based off newly assigned occ_scan_mapped
# apply gaussian e**-d**2 to every weight, then cube to emphasize
p.occ_scan_mapped = [(math.e / (d)**2) if (d)**2 != 0 else
(math.e / (d + .01)**2)
for d in p.occ_scan_mapped]
p.occ_scan_mapped = [d**3 for d in p.occ_scan_mapped]
p.w = sum(p.occ_scan_mapped)
#print("Set weight to: ", p.w)
p.occ_scan_mapped = []
self.normalize_particles()
@staticmethod
def draw_random_sample(choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(random_sample(n), bins)]
samples = []
for i in inds:
samples.append(deepcopy(choices[int(i)]))
return samples
def resample_particles(self):
"""
Re initialize particles in self.particle_cloud
based on preivous weightages.
"""
weights = [p.w for p in self.particle_cloud]
# after calculating all particle weights, we want to calc the n_effective
# self.n_effective = 0
self.n_effective = 1 / sum(
[w**2 for w in weights]) # higher is more diversity, so less noise
print("n_effective: ", self.n_effective)
temp_particle_cloud = self.draw_random_sample(
self.particle_cloud, weights,
int((1 - self.particles_to_replace) * self.num_particles))
# temp_particle_cloud = self.draw_random_sample(self.particle_cloud, weights, self.num_particles)
particle_cloud_to_transform = self.draw_random_sample(
self.particle_cloud, weights, self.num_particles - int(
(1 - self.particles_to_replace) * self.num_particles))
# NOISE POLLUTION - larger noise, smaller # particles
# normal_std_xy = .25
normal_std_xy = 10 / self.n_effective # feedback loop? 8,3
normal_std_theta = 3 / self.n_effective
# normal_std_theta = math.pi/21
random_vals_x = np.random.normal(0, normal_std_xy,
len(particle_cloud_to_transform))
random_vals_y = np.random.normal(0, normal_std_xy,
len(particle_cloud_to_transform))
random_vals_theta = np.random.normal(0, normal_std_theta,
len(particle_cloud_to_transform))
for p_num, p in enumerate(
particle_cloud_to_transform): # add in noise in x,y, theta
p.x += random_vals_x[p_num]
p.y += random_vals_y[p_num]
p.theta += random_vals_theta[p_num]
# reset the partilce cloud based on the newly transformed particles
self.particle_cloud = temp_particle_cloud + particle_cloud_to_transform
def scan_received(self, msg):
"""
Callback function for recieving laser scan - should pass data into global scan object
"""
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, we hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not (self.initialized):
# wait for initialization to complete
return
if not (self.tf_listener.canTransform(
self.base_frame, msg.header.frame_id, msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame,
msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative to the robot base
p = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
# grab from listener & store the the odometry pose in a more convenient format (x,y,theta)
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
if not self.current_odom_xy_theta:
self.current_odom_xy_theta = new_odom_xy_theta
return
# Now we've done all calcs, we exit the scan_recieved() method by either initializing a cloud
if not (self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
# TODO: Where do we get the xy_theta needed for initialize_particle_cloud?
self.initialize_particle_cloud(msg.header.stamp,
self.current_odom_xy_theta)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) >
self.d_thresh
or math.fabs(new_odom_xy_theta[1] -
self.current_odom_xy_theta[1]) > self.d_thresh
or math.fabs(new_odom_xy_theta[2] -
self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg)
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose(msg.header.stamp) # update robot's pose
self.resample_particles(
) # resample particles to focus on areas of high density
# # publish particles (so things like rviz can see them)
self.publish_particles()
def run(self):
"""
main run loop for rosnode
"""
r = rospy.Rate(5)
print("Nathan and Adi ROS Loop code is starting!!!")
while not (rospy.is_shutdown()):
# in the main loop all we do is continuously broadcast the latest
# map to odom transform
self.transform_helper.send_last_map_to_odom_transform()
r.sleep()
class ParticleFilter:
""" The class that represents a Particle Filter ROS Node
Attributes list:
initialized: a Boolean flag to communicate to other class methods that initializaiton is complete
base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots)
map_frame: the name of the map coordinate frame (should be "map" in most cases)
odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases)
scan_topic: the name of the scan topic to listen to (should be "scan" in most cases)
n_particles: the number of particles in the filter
d_thresh: the amount of linear movement before triggering a filter update
a_thresh: the amount of angular movement before triggering a filter update
laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation
pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI)
particle_pub: a publisher for the particle cloud
laser_subscriber: listens for new scan data on topic self.scan_topic
tf_listener: listener for coordinate transforms
tf_broadcaster: broadcaster for coordinate transforms
particle_cloud: a list of particles representing a probability distribution over robot poses
current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed.
The pose is expressed as a list [x,y,theta] (where theta is the yaw)
map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid
"""
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node('pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.n_particles = 100 # the number of particles to use
self.d_thresh = 0.2 # the amount of linear movement before performing an update
self.a_thresh = math.pi/6 # the amount of angular movement before performing an update
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
self.sigma = 0.08
# TODO: define additional constants if needed
# Setup pubs and subs
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10)
self.marker_pub = rospy.Publisher("markers", MarkerArray, queue_size=10)
# laser_subscriber listens for data from the lidar
# Dados do Laser: Mapa de verossimilhança/Occupancy field/Likehood map e Traçado de raios.
# Traçado de raios: Leitura em ângulo que devolve distância, através do sensor. Dado o mapa,
# extender a direção da distância pra achar um obstáculo.
self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)
# enable listening for and broadcasting coordinate transforms
#atualização de posição(odometria)
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.particle_cloud = []
self.current_odom_xy_theta = []
#Chamar o mapa a partir de funcao externa
mapa = chama_mapa.obter_mapa()
# request the map from the map server, the map should be of type nav_msgs/OccupancyGrid
# TODO: fill in the appropriate service call here. The resultant map should be assigned be passed
# into the init method for OccupancyField
# for now we have commented out the occupancy field initialization until you can successfully fetch the map
self.occupancy_field = OccupancyField(mapa)
self.initialized = True
def update_robot_pose(self):
print("Update Robot Pose")
""" Update the estimate of the robot's pose given the updated particles.
There are two logical methods for this:
(1): compute the mean pose
(2): compute the most likely pose (i.e. the mode of the distribution)
"""
# first make sure that the particle weights are normalized
self.normalize_particles()
# TODO: assign the lastest pose into self.robot_pose as a geometry_msgs.Pose object
# just to get started we will fix the robot's pose to always be at the origin
#Variaveis para soma do X,Y e angulo Theta da particula
x_sum = 0
y_sum = 0
theta_sum = 0
#Loop de soma para as variaveis relativas a probabilidade da particula
for p in self.particle_cloud:
x_sum += p.x * p.w
y_sum += p.y * p.w
theta_sum += p.theta * p.w
#Atributo robot_pose eh o pose da melhor particula possivel a partir das variaveis de soma
self.robot_pose = Particle(x=x_sum, y=y_sum, theta=theta_sum).as_pose()
def update_particles_with_odom(self,msg):
print("Update Particles with Odom")
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
#R eh o raio feito a partir de um pitagoras com o X e Y da variacao Delta
r = math.sqrt(delta[0]**2 + delta[1]**2)
#Funcao para conseguir um valor entre -pi e pi e subtrair o antigo valor de theta. (Achei um pouco confusa, )
phi = math.atan2(delta[1], delta[0])-old_odom_xy_theta[2]
for particle in self.particle_cloud:
particle.x += r*math.cos(phi+particle.theta)
particle.y += r*math.sin(phi+particle.theta)
particle.theta += delta[2]
# TODO: modify particles using delta
# For added difficulty: Implement sample_motion_odometry (Prob Rob p 136)
def map_calc_range(self,x,y,theta):
""" Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """
# TODO: nothing unless you want to try this alternate likelihood model
pass
def resample_particles(self):
""" Resample the particles according to the new particle weights.
The weights stored with each particle should define the probability that a particular
particle is selected in the resampling step. You may want to make use of the given helper
function draw_random_sample.
"""
#Primeiro de tudo, normalizar particulas
self.normalize_particles()
#Criar array do numpy vazia do tamanho do numero de particulas.
values = np.empty(self.n_particles)
#Preencher essa lista com os indices das particulas
for i in range(self.n_particles):
values[i] = i
#Criar uma lista para novas particulas
new_particles = []
#Criar lista com os indices das particulas com mais probabilidade
random_particles = ParticleFilter.weighted_values(values,[p.w for p in self.particle_cloud],self.n_particles)
for i in random_particles:
#Transformar o I em inteiro para corrigir bug de float
int_i = int(i)
#Pegar particula na possicao I na nuvem de particulas.
p = self.particle_cloud[int_i]
#Adicionar particulas somando um valor aleatorio da distribuicao gauss com media = 0 e desvio padrao = 0.025
new_particles.append(Particle(x=p.x+gauss(0,.025),y=p.y+gauss(0,.025),theta=p.theta+gauss(0,.025)))
#Igualar nuvem de particulas a novo sample criado
self.particle_cloud = new_particles
#Normalizar mais uma vez as particulas.
self.normalize_particles()
def update_particles_with_laser(self, msg):
print("Update Particles with laser")
""" Updates the particle weights in response to the scan contained in the msg """
for p in self.particle_cloud:
for i in range(360):
#Distancia no angulo I
distancia = msg.ranges[i]
x = distancia * math.cos(i + p.theta)
y = distancia * math.sin(i + p.theta)
#Verificar se distancia minima eh diferente de nan
erro_nan = self.occupancy_field.get_closest_obstacle_distance(x,y)
if erro_nan is not float('nan'):
# Adicionar valor para corrigir erro de nan (Retirado de outro codigo)
p.w += math.exp(-erro_nan*erro_nan/(2*self.sigma**2))
#Normalizar particulas e criar um novo sample das mesmas
self.normalize_particles()
self.resample_particles()
@staticmethod
def weighted_values(values, probabilities, size):
""" Return a random sample of size elements from the set values with the specified probabilities
values: the values to sample from (numpy.ndarray)
probabilities: the probability of selecting each element in values (numpy.ndarray)
size: the number of samples
"""
bins = np.add.accumulate(probabilities)
return values[np.digitize(random_sample(size), bins)]
@staticmethod
#Nao estou usando esse metodo. Apenas o weighted_values
def draw_random_sample(choices, probabilities, n):
print("Draw Random Sample")
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(random_sample(n), bins)]
samples = []
for i in inds:
samples.append(deepcopy(choices[int(i)]))
return samples
def update_initial_pose(self, msg):
print("Update Initial Pose")
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose)
self.initialize_particle_cloud(xy_theta)
self.fix_map_to_odom_transform(msg)
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
# TODO create particles
# Criando particula
print("Inicializacao da Cloud de Particulas")
#Valor auxiliar para nao dar erro na hora de criacao das particulas. Irrelevante
valor_aux = 0.3
for i in range (self.n_particles):
self.particle_cloud.append(Particle(0, 0, 0, valor_aux))
# Randomizar particulas em volta do robo.
for i in self.particle_cloud:
i.x = xy_theta[0]+ random.uniform(-1,1)
i.y = xy_theta[1]+ random.uniform(-1,1)
i.theta = xy_theta[2]+ random.uniform(-45,45)
#Normalizar as particulas e dar update na posicao do robo
self.normalize_particles()
self.update_robot_pose()
print(xy_theta)
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
#Soma total das probabilidades das particulas
w_sum = sum([p.w for p in self.particle_cloud])
#Dividir cada probabilidade de uma particula pela soma total
for particle in self.particle_cloud:
particle.w /= w_sum
# TODO: implement this
def publish_particles(self, msg):
print("Publicar Particulas.")
print(len(self.particle_cloud))
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not(self.initialized):
print("Not Initialized")
# wait for initialization to complete
return
if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,rospy.Time(0))):
print("Not 2")
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,rospy.Time(0))):
print("Not 3")
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
p = PoseStamped(header=Header(stamp=rospy.Time(0),
frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame,p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp = rospy.Time(0),
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
print("PASSOU")
if not(self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose() # update robot's pose
self.resample_particles() # resample particles to focus on areas of high density
self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
# direcionar particulas quando um obstaculo é identificado.
def fix_map_to_odom_transform(self, msg):
print("Fix Map to Odom Transform")
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer """
(translation, rotation) = convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation),
header=Header(stamp=rospy.Time(0),frame_id=self.base_frame))
self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
(self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)
def broadcast_last_transform(self):
print("Broadcast")
""" Make sure that we are always broadcasting the last map
to odom transformation. This is necessary so things like
move_base can work properly. """
if not(hasattr(self,'translation') and hasattr(self,'rotation')):
return
self.tf_broadcaster.sendTransform(self.translation,
self.rotation,
rospy.get_rostime(),
self.odom_frame,
self.map_frame)
class ParticleFilter:
""" The class that represents a Particle Filter ROS Node
Attributes list:
initialized: a Boolean flag to communicate to other class methods that initializaiton is complete
base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots)
map_frame: the name of the map coordinate frame (should be "map" in most cases)
odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases)
scan_topic: the name of the scan topic to listen to (should be "scan" in most cases)
n_particles: the number of particles in the filter
d_thresh: the amount of linear movement before triggering a filter update
a_thresh: the amount of angular movement before triggering a filter update
laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation
pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI)
particle_pub: a publisher for the particle cloud
laser_subscriber: listens for new scan data on topic self.scan_topic
tf_listener: listener for coordinate transforms
tf_broadcaster: broadcaster for coordinate transforms
particle_cloud: a list of particles representing a probability distribution over robot poses
current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed.
The pose is expressed as a list [x,y,theta] (where theta is the yaw)
map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid
"""
def __init__(self, map_fname):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node(
'pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "batman/base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "batman/odom" # the name of the odometry coordinate frame
self.scan_topic = "batman/scan" # the topic where we will get laser scans from
self.n_particles = 500 # the number of particles to use
self.d_thresh = 0.2 # the amount of linear movement before performing an update
self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
self.sigma = 0.02 # guess for how inaccurate lidar readings are in meters
self.beacon_sigma = 2
# Setup pubs and subs
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
self.pose_listener = rospy.Subscriber("initialpose",
PoseWithCovarianceStamped,
self.update_initial_pose)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("bl_particlecloud",
PoseArray,
queue_size=10)
self.marker_pub = rospy.Publisher("markers",
MarkerArray,
queue_size=10)
self.pose_pub = rospy.Publisher("/batman/pose",
PoseStamped,
queue_size=10)
# laser_subscriber listens for data from the lidar
self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan,
self.scan_received)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.particle_cloud = []
self.laser_pose = None
self.current_odom_xy_theta = []
self.cnt = 0
# request the map from the map server, the map should be of type nav_msgs/OccupancyGrid
self.map_server = rospy.ServiceProxy('static_map', GetMap)
self.map = self.map_server().map
# for now we have commented out the occupancy field initialization until you can successfully fetch the map
self.occupancy_field = OccupancyField(self.map)
self.initialize_particle_cloud()
with open(map_fname, 'r') as map_file:
self.refpoints = [
yaml.load(x) for x in map_file.read().split('---')[:-1]
]
self.ref_vector_dimension = len(self.refpoints[0]['rssi'].keys())
self.beacons_subscriber = rospy.Subscriber(
'/batman/beacon_localization/distances/probabilistic',
BeaconsScan, self.beacons_received)
self.initialized = True
def update_robot_pose(self):
""" Update the estimate of the robot's pose given the updated particles.
Computed by taking the weighted average of poses.
"""
# first make sure that the particle weights are normalized
self.normalize_particles()
x = 0
y = 0
theta = 0
angles = []
for particle in self.particle_cloud:
x += particle.x * particle.w
y += particle.y * particle.w
v = [
particle.w * math.cos(math.radians(particle.theta)),
particle.w * math.sin(math.radians(particle.theta))
]
angles.append(v)
theta = sum_vectors(angles)
orientation_tuple = tf.transformations.quaternion_from_euler(
0, 0, theta)
self.robot_pose = Pose(position=Point(x=x, y=y),
orientation=Quaternion(x=orientation_tuple[0],
y=orientation_tuple[1],
z=orientation_tuple[2],
w=orientation_tuple[3]))
self.pose_pub.publish(
PoseStamped(header=Header(stamp=rospy.Time.now(), frame_id="map"),
pose=self.robot_pose))
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
for particle in self.particle_cloud:
r1 = math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2]
d = math.sqrt((delta[0]**2) + (delta[1]**2))
particle.theta += r1 % 360
particle.x += d * math.cos(particle.theta) + normal(0, 0.1)
particle.y += d * math.sin(particle.theta) + normal(0, 0.1)
particle.theta += (delta[2] - r1 + normal(0, 0.1)) % 360
# For added difficulty: Implement sample_motion_odometry (Prob Rob p 136)
def map_calc_range(self, x, y, theta):
""" Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """
pass
def resample_particles(self):
""" Resample the particles according to the new particle weights.
The weights stored with each particle should define the probability that a particular
particle is selected in the resampling step. You may want to make use of the given helper
function draw_random_sample.
"""
# make sure the distribution is normalized
self.normalize_particles()
newParticles = []
for i in range(len(self.particle_cloud)):
# resample the same # of particles
choice = random_sample()
# all the particle weights sum to 1
csum = 0 # cumulative sum
for particle in self.particle_cloud:
csum += particle.w
if csum >= choice:
# if the random choice fell within the particle's weight
newParticles.append(deepcopy(particle))
break
self.particle_cloud = newParticles
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
for particle in self.particle_cloud:
tot_prob = 0
for index, scan in enumerate(msg.ranges):
x, y = self.transform_scan(particle, scan, index)
# transform scan to view of the particle
d = self.occupancy_field.get_closest_obstacle_distance(x, y)
# calculate nearest distance to particle's scan (should be near 0 if it's on robot)
if not math.isnan(d):
tot_prob += math.exp((-d**2) / (2 * self.sigma**2))
# add probability (0 to 1) to total probability
tot_prob = tot_prob / len(msg.ranges)
# normalize total probability back to 0-1
particle.w *= tot_prob
# assign particles weight
self.update_robot_pose()
def transform_scan(self, particle, distance, theta):
""" Calculates the x and y of a scan from a given particle
particle: Particle object
distance: scan distance (from ranges)
theta: scan angle (range index)
"""
return (particle.x +
distance * math.cos(math.radians(particle.theta + theta)),
particle.y +
distance * math.sin(math.radians(particle.theta + theta)))
@staticmethod
def weighted_values(values, probabilities, size):
""" Return a random sample of size elements from the set values with the specified probabilities
values: the values to sample from (numpy.ndarray)
probabilities: the probability of selecting each element in values (numpy.ndarray)
size: the number of samples
"""
bins = np.add.accumulate(probabilities)
return values[np.digitize(random_sample(size), bins)]
@staticmethod
def draw_random_sample(choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(random_sample(n), bins)]
samples = []
for i in inds:
samples.append(deepcopy(choices[int(i)]))
return samples
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose)
self.initialize_particle_cloud(xy_theta)
self.fix_map_to_odom_transform(msg)
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
self.particle_cloud = []
nonoccupied_points = self.occupancy_field.free_cells[:]
for i in range(self.n_particles):
indx = random.randint(0, len(nonoccupied_points) - 1)
particle = Particle(nonoccupied_points[indx]['x'],
nonoccupied_points[indx]['y'],
random.random() * 360)
self.particle_cloud.append(particle)
del nonoccupied_points[indx]
self.normalize_particles()
self.update_robot_pose()
self.publish_particles()
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
tot_weight = sum([particle.w for particle in self.particle_cloud]) or 1
for particle in self.particle_cloud:
particle.w = particle.w / tot_weight
def publish_particles(self):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(
PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
marker_array = []
for index, particle in enumerate(self.particle_cloud):
marker = Marker(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
pose=particle.as_pose(),
type=0,
scale=Vector3(x=particle.w * 2,
y=particle.w * 1,
z=particle.w * 5),
id=index,
color=ColorRGBA(r=1, a=1))
marker_array.append(marker)
self.marker_pub.publish(MarkerArray(markers=marker_array))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not (self.initialized):
# wait for initialization to complete
return
if not self.laser_pose:
self.tf_listener.waitForTransform(self.base_frame,
msg.header.frame_id,
rospy.Time(0),
rospy.Duration(10.0))
p = PoseStamped(header=Header(stamp=rospy.Time(0),
frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(
self.base_frame, p)
self.recent_scan = msg
# self.update_particles_with_laser(msg) # update based on laser scan
# self.update_robot_pose() # update robot's pose
# self.resample_particles() # resample particles to focus on areas of high density
# self.publish_particles()
def beacons_received(self, msg):
if not self.refpoints:
return
distribution = []
for point in self.refpoints:
squared_dist = 0.0
for beacon in msg.beacons:
squared_dist += (beacon.rssi - point['rssi'][beacon.name])**2
dist = np.sqrt(squared_dist)
measurement = {
'x': point['x'],
'y': point['y'],
'dist': dist,
}
distribution.append(measurement)
sorted_dist = sorted(distribution[:], key=lambda x: x['dist'])
avg_pose = {'x': 0, 'y': 0}
selected_ones = sorted_dist[:1]
for measurement in selected_ones:
avg_pose['x'] += measurement['x'] * measurement['dist']
avg_pose['y'] += measurement['y'] * measurement['dist']
avg_pose['x'] /= sum(item['dist'] for item in selected_ones)
avg_pose['y'] /= sum(item['dist'] for item in selected_ones)
# Make cloud great again
for particle in self.particle_cloud:
dist = math.sqrt((particle.x - avg_pose['x'])**2 +
(particle.y - avg_pose['y'])**2)
prob = math.exp((-dist**2) / (2 * self.beacon_sigma**2))
particle.w = prob
# Create new cloud
# for i in range(self.n_particles-1):
# # initial facing of the particle
# theta = random.random() * 360
#
# # compute params to generate x,y in a circle
# other_theta = random.random() * 360
# radius = random.random() * 1.5
# # x => straight ahead
# x = radius * math.sin(other_theta) + avg_pose['x']
# y = radius * math.cos(other_theta) + avg_pose['y']
# particle = Particle(x, y, theta)
# self.particle_cloud.append(particle)
self.normalize_particles()
if self.cnt is 0:
self.resample_particles(
) # resample particles to focus on areas of high density
self.cnt = 5
self.cnt -= 1
self.update_particles_with_laser(self.recent_scan)
self.update_robot_pose()
self.publish_particles()
def fix_map_to_odom_transform(self, msg):
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer """
(translation,
rotation) = convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=convert_translation_rotation_to_pose(
translation, rotation),
header=Header(stamp=rospy.Time(0),
frame_id=self.base_frame))
self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
(self.translation,
self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map
to odom transformation. This is necessary so things like
move_base can work properly. """
if not (hasattr(self, 'translation') and hasattr(self, 'rotation')):
return
self.tf_broadcaster.sendTransform(self.translation, self.rotation,
rospy.get_rostime(), self.odom_frame,
self.map_frame)
class ParticleFilter(object):
""" The class that represents a Particle Filter ROS Node
"""
def __init__(self, top_particle_pub, particle_cloud_pub, particle_cloud_marker_pub):
# # pose_listener responds to selection of a new approximate robot
# # location (for instance using rviz)
# rospy.Subscriber("initialpose",
# PoseWithCovarianceStamped,
# self.update_initial_pose)
self.top_particle_pub = top_particle_pub
self.particle_cloud_pub = particle_cloud_pub
self.particle_cloud_marker_pub = particle_cloud_marker_pub
# create instances of two helper objects that are provided to you
# as part of the project
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.particles = []
self.markerArray = MarkerArray()
def gauge_particle_position(self):
xs = [particle.x for particle in self.particles]
ys = [particle.y for particle in self.particles]
print("min x: {} --- average x: {} --- max x: {} \nmin y: {} --- average y: {} --- max y: {}".format(
min(xs), sum(xs)/len(xs), max(xs), min(ys), sum(ys)/len(ys), max(ys)
))
def get_marker(self, x, y):
marker = Marker()
marker.header.frame_id = "base_link"
marker.type = marker.SPHERE
marker.pose.position.x = x
marker.pose.position.y = y
marker.pose.position.z = 0
marker.scale.x = 0.3
marker.scale.y = 0.3
marker.scale.z = 0.3
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
return marker
def draw_markerArray(self):
markerArray = MarkerArray()
for p in self.particles:
m = self.get_marker(p.x, p.y)
markerArray.markers.append(m)
self.particle_cloud_marker_pub.publish(markerArray)
def publish_particle_cloud(self):
msg = PoseArray()
msg.header.frame_id = "map"
# Make pose from particle for all particles
msg.poses = [particle.get_pose() for particle in self.particles]
# Publish
self.particle_cloud_pub.publish(msg)
def publish_top_particle(self):
msg = PoseArray()
top_particle = self.particles[0]
for particle in self.particles:
if particle.weight > top_particle.weight:
top_particle = particle
msg.poses.append(top_particle.get_pose())
# print(msg)
self.top_particle_pub.publish(msg)
return top_particle.get_pose()
def gen_init_particles(self):
"""Generating random particles with x, y, and t values"""
# width = self.occupancy_field.map.info.width
# height = self.occupancy_field.map.info.height
width = 10
height = 10
print("map width: {}, height: {}".format(width, height))
for i in range(500):
# x = r.randrange(0,width)
# y = r.randrange(0,height)
x = r.uniform(-5, 5)
y = r.uniform(-5, 5)
t = math.radians(r.randrange(0,360))
p = Particle(x,y,t)
self.particles.append(p)
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter
based on a pose estimate. These pose estimates could be generated
by another ROS Node or could come from the rviz GUI """
xy_theta = \
self.transform_helper.convert_pose_to_xy_and_theta(msg.pose.pose)
# TODO this should be deleted before posting
self.transform_helper.fix_map_to_odom_transform(msg.pose.pose,
msg.header.stamp)
# initialize your particle filter based on the xy_theta tuple
def resample_particles(self):
"""Resample particles with replacement."""
if len(self.particles):
weights = [particle.weight if not math.isnan(particle.weight) else 0.0001 for particle in self.particles]
total_weight = sum(weights)
weights = [weight / total_weight for weight in weights]
before = time.time()
self.particles = [particle.deep_copy() for particle in list(np.random.choice(
# self.particles = [particle for particle in list(np.random.choice(
self.particles,
size=len(self.particles),
replace=True,
p=weights,
))]
after = time.time()
print("************************************timer: {}".format(after-before))
print("number of particles: {}".format(len(self.particles)))
else:
print("No particles to resample from")
return None
def update_all_particles(self, transform):
for particle in self.particles:
# self.update_particle(particle, transform)
self.update_particle_with_randomness(particle, transform)
def update_particle_with_randomness(self, particle, transform):
# TODO(matt): Make this a tunable param
DISTANCE_VAR_SCALE = 0.001
ANGLE_VAR_SCALE = math.radians(0.5)
# NOTE: We scale the variance instead of the standard deviation because
# that makes it independent of the update time (the noise in one update
# will be the same as the sum of the noise in two updates)
distance = math.sqrt(transform['translation'][0]**2 + transform['translation'][1]**2)
translation_mean, translation_var = 0, DISTANCE_VAR_SCALE #* distance # scale with magnitude
rotation_mean, rotation_var = 0, ANGLE_VAR_SCALE
modified_transform = transform
# modified_transform['translation'][0] += float(np.random.normal(translation_mean, math.sqrt(translation_var), 1))
# modified_transform['translation'][1] += float(np.random.normal(translation_mean, math.sqrt(translation_var), 1))
# modified_transform['rotation'] += float(np.random.normal(rotation_mean, math.sqrt(rotation_var)))
modified_transform['translation'][0] += float(np.random.uniform(-0.01, 0.01, 1))
modified_transform['translation'][1] += float(np.random.uniform(-0.01, 0.01, 1))
modified_transform['rotation'] += float(np.random.uniform(-math.radians(5), -math.radians(5), 1))
self.update_particle(particle, modified_transform)
def update_particle(self, particle, transform):
# rotate translation in the particle's direction
particle_translation = self.transform_helper.rotate_2d_vector(transform['translation'], particle.theta)
particle.translate(particle_translation)
particle.rotate(transform['rotation'])
# def compare_points(self):
# """Compares translated particle to lidar scans, returns weights values"""
# distances = []
# errordis = 0
# for a in range(500):
# particle.ParticleCloud(self.particle[a])
# for b in range(8):
# d[b] = OccupancyField.get_closest_obstacle_distance(particle.ParticleCloud[b][1],particle.ParticleCloud[b][2])
# particle.Particle.weight = 1 / (sum(d) + .01)
def compare_points(self, robo_pts):
""" This function determines the weights for each particle.
Args:
robo_pts (list): is a list of lidar readings for n directions. It can
be obtained by calling get_x_directions in robot localizerself.
"""
for p in self.particles:
p_cloud = ParticleCloud(p)
p_cloud.generate_points(robo_pts)
d = []
for pt in p_cloud.pts:
if pt[1] != 0:
d dist = abs(self.occupancy_field.get_closest_obstacle_distance(pt[0],pt[1]))
d.append(math.exp(-d**2/1))
p.weight = 1 / (sum(d) + .01)
def run(self):
pass
class ParticleFilter(object):
""" The class that represents a Particle Filter ROS Node
"""
def __init__(self):
rospy.init_node('pf')
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("initialpose",
PoseWithCovarianceStamped,
self.update_initial_pose)
# publisher for the particle cloud for visualizing in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
# create instances of two helper objects that are provided to you
# as part of the project
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.particles = Particles()
self.particles.initialize_particles()
self.ranges = []
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter
based on a pose estimate. These pose estimates could be generated
by another ROS Node or could come from the rviz GUI """
xy_theta = \
self.transform_helper.convert_pose_to_xy_and_theta(msg.pose.pose)
# TODO this should be deleted before posting
self.transform_helper.fix_map_to_odom_transform(msg.pose.pose,
msg.header.stamp)
# initialize your particle filter based on the xy_theta tuple
def add_noise_to_particles(self, position_change):
# OUTPUT FORMAT : List[Tuple(x, y, theta, original particle position)]
current_particles = self.particles.get_locations()
new_particles = []
for particle in current_particles:
for i in range(NEW_PARTICLES):
x_noise = np.random.normal(loc=0, scale=.25)
y_noise = np.random.normal(loc=0, scale=.25)
theta_noise = np.random.randint(360)
# appending (x, y, theta, original particle position)
new_particles.append((particle[0] + position_change.x + x_noise, particle[1] + position_change.y + y_noise, (particle[2] + position_change.z + theta_noise) % 360, particle))
return new_particles
def calculate_particle_probs(self, particles):
# OUTPUT FORMAT : List[Tuple(cur_loc, prev_loc, confidence weight)]
# iterate through particles, determine likelihood of each
total_weight = 0
potential_locations = []
for loc_tuple in particles:
# get actual measured distance and the map's distance to the closest obstacle
measured_distance = self.get_closest_obstacle_from_laserscan()[1]
map_distance = self.occupancy_field.get_closest_obstacle_distance(loc_tuple[0], loc_tuple[1])
# basic weight calculator based on measured and map distances
if measured_distance == 0 and map_distance == 0:
new_weight == 0
elif measured_distance == 0 or map_distance == 0:
new_weight == 0
elif measured_distance-map_distance != 0:
new_weight = 1/abs(measured_distance-map_distance)
else:
new_weight = 500
# appending in format (cur_loc, prev_loc, confidence weight)
potential_locations.append(((loc_tuple[0], loc_tuple[1], loc_tuple[2]), loc_tuple[3], new_weight))
total_weight += new_weight
# normalize the weight to be a probability
if total_weight:
return [(cur_loc, prev_loc, new_weight / total_weight) for cur_loc, prev_loc, new_weight in potential_locations]
return potential_locations
def get_closest_obstacle_from_laserscan(self):
# this function is used to calculate probabilties of each particle
closest_laserscan = (0,0)
for i, scan_val in enumerate(self.ranges):
if scan_val > 0 and (scan_val < closest_laserscan[1] or closest_laserscan[1] == 0):
closest_laserscan = (i, scan_val)
return closest_laserscan
def run(self):
r = rospy.Rate(5)
scan_sub = rospy.Subscriber('/scan', LaserScan, self.update_ranges)
scan_sub = rospy.Subscriber('/pos_change', Vector3, self.position_update_listener)
while not(rospy.is_shutdown()):
# in the main loop all we do is continuously broadcast the latest
# map to odom transform
self.transform_helper.send_last_map_to_odom_transform()
r.sleep()
def position_update_listener(self, msg):
# function serves to update the positions of the particles each time it is called
pos_change = msg
new_particles = self.add_noise_to_particles(pos_change)
potential_locations = self.calculate_particle_probs(new_particles)
self.particles.update_locations(potential_locations)
def update_ranges(self, msg):
# callback function from listener
self.ranges = msg.ranges
class ParticleFilter:
""" The class that represents a Particle Filter ROS Node
Attributes list:
initialized: a Boolean flag to communicate to other class methods that initializaiton is complete
base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots)
map_frame: the name of the map coordinate frame (should be "map" in most cases)
odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases)
scan_topic: the name of the scan topic to listen to (should be "scan" in most cases)
number_of_particles: the number of particles in the filter
d_thresh: the amount of linear movement before triggering a filter update
a_thresh: the amount of angular movement before triggering a filter update
laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation
pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI)
particle_pub: a publisher for the particle cloud
laser_subscriber: listens for new scan data on topic self.scan_topic
tf_listener: listener for coordinate transforms
tf_broadcaster: broadcaster for coordinate transforms
particle_cloud: a list of particles representing a probability distribution over robot poses
current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed.
The pose is expressed as a list [x,y,theta] (where theta is the yaw)
map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid
"""
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node('pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.number_of_particles = 1000 # the number of particles to use
self.d_thresh = 0.2 # the amount of linear movement before performing an update
self.a_thresh = math.pi/6 # the amount of angular movement before performing an update
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10)
# laser_subscriber listens for data from the lidar
self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.particle_cloud = []
self.current_odom_xy_theta = []
# Fetch map using OccupancyField
rospy.wait_for_service('static_map')
static_map = rospy.ServiceProxy('static_map', GetMap)
self.occupancy_field = OccupancyField(static_map().map)
self.initialized = True
def update_robot_pose(self):
""" Update the estimate of the robot's pose given the updated particles.
There are two logical methods for this:
(1): compute the mean pose
(2): compute the most likely pose (i.e. the mode of the distribution)
"""
# first make sure that the particle weights are normalized
self.normalize_particles()
avg_x = 0
avg_y = 0
theta_x = 0
theta_y = 0
# Multiple x and y by particle weights to find new robot pose
for particle in self.particle_cloud:
avg_x += particle.x * particle.w
avg_y += particle.y * particle.w
theta_x += math.cos(particle.theta) * particle.w
theta_y += math.sin(particle.theta) * particle.w
# Calculate theta using arc tan of x and y components of all thetas multiplied by particle weights
avg_theta = math.atan2(theta_y, theta_x)
avg_particle = Particle(x=avg_x, y=avg_y, theta=avg_theta)
# Update robot pose based on average particle
self.robot_pose = avg_particle.as_pose()
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
temp = []
# Use trigonometry to update particles based on new odometry pose
for particle in self.particle_cloud:
psy = math.atan2(delta[1],delta[0])-old_odom_xy_theta[2]
intermediate_theta = particle.theta + psy
# Calculate radius based on change in x and y
r = math.sqrt(delta[0]**2 + delta[1]**2)
# Update x and y based on radius and new angle
new_x = particle.x + r*math.cos(intermediate_theta) + np.random.randn()*0.1
new_y = particle.y + r*math.sin(intermediate_theta) + np.random.randn()*0.1
# Add change in angle to old angle
new_theta = delta[2]+particle.theta + np.random.randn()*0.1
temp.append(Particle(new_x,new_y,new_theta))
self.particle_cloud = temp
def map_calc_range(self,x,y,theta):
""" Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """
# TODO: nothing unless you want to try this alternate likelihood model
pass
def resample_particles(self):
""" Resample the particles according to the new particle weights.
The weights stored with each particle should define the probability that a particular
particle is selected in the resampling step. You may want to make use of the given helper
function draw_random_sample.
"""
probabilities = []
# create list of particle weights to pass into draw_random_sample for resampling
for particle in self.particle_cloud:
probabilities.append(particle.w)
print particle.w
print '\n'
temp_particle_cloud = self.draw_random_sample(self.particle_cloud, probabilities, 100)
self.particle_cloud = []
for particle in temp_particle_cloud:
for i in range(10):
self.particle_cloud.append(deepcopy(particle))
self.normalize_particles()
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
temp = 0
ranges = []
min_range = 5
for item in msg.ranges:
# set ranges to 5 if the laser scan is 0
if item == 0:
ranges.append(5)
else:
ranges.append(item)
# do weighted averages for cleaner data
for i in range(355):
avg = sum(ranges[i:i+5]) / len(ranges[i:i+5])
if avg < min_range:
min_range = avg
min_theta = (i + 2.5)*math.pi / 180.0
# find the minimum range across 360 angles, this probably caused an issue
r = min_range
# Update particle x, y, theta based on min range, previous particles
for particle in self.particle_cloud:
x = particle.x+r*math.cos(particle.theta + min_theta)
y = particle.y+r*math.sin(particle.theta + min_theta)
temp = self.occupancy_field.get_closest_obstacle_distance(x,y)
# Update particle weights using a sharp Gaussian
particle.w = np.exp(-np.power(temp, 2.) / (2 * np.power(0.3, 2.)))
@staticmethod
def weighted_values(values, probabilities, size):
""" Return a random sample of size elements from the set values with the specified probabilities
values: the values to sample from (numpy.ndarray)
probabilities: the probability of selecting each element in values (numpy.ndarray)
size: the number of samples
"""
bins = np.add.accumulate(probabilities)
return values[np.digitize(random_sample(size), bins)]
@staticmethod
def draw_random_sample(choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(random_sample(n), bins)]
samples = []
for i in inds:
samples.append(deepcopy(choices[int(i)]))
return samples
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose)
self.initialize_particle_cloud(xy_theta)
self.fix_map_to_odom_transform(msg)
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
self.particle_cloud.append(Particle(xy_theta[0],xy_theta[1],xy_theta[2]))
# Initialize particle cloud with a decent amount of noise
for i in range (0,self.number_of_particles):
self.particle_cloud.append(Particle(xy_theta[0]+np.random.randn()*.5,xy_theta[1]+np.random.randn()*.5,xy_theta[2]+np.random.randn()*.5))
self.normalize_particles()
self.update_robot_pose()
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
particle_sum = 0
# Sum up particle weights to divide by for normalization
for particle in self.particle_cloud:
particle_sum += particle.w
# Make all particle weights add to 1
for particle in self.particle_cloud:
particle.w /= particle_sum
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not(self.initialized):
# wait for initialization to complete
return
if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
p = PoseStamped(header=Header(stamp=rospy.Time(0),
frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame,p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not(self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose() # update robot's pose
self.resample_particles() # resample particles to focus on areas of high density
self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def fix_map_to_odom_transform(self, msg):
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer """
(translation, rotation) = convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation),
header=Header(stamp=msg.header.stamp,frame_id=self.base_frame))
self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
(self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map
to odom transformation. This is necessary so things like
move_base can work properly. """
if not(hasattr(self,'translation') and hasattr(self,'rotation')):
return
self.tf_broadcaster.sendTransform(self.translation,
self.rotation,
rospy.get_rostime(),
self.odom_frame,
self.map_frame)
class RobotLocalizer(object):
'''
The class that represents a Particle Filter ROS Node
'''
def __init__(self):
print('Initializing')
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node('localizer')
self.pf = ParticleFilter()
self.base_frame = "base_link" # Robot base frame
self.map_frame = "map" # Map coord frame
self.odom_frame = "odom" # Odom coord frame
self.scan_topic = "scan" # Laser scan topic
self.linear_threshold = 0.1 # the amount of linear movement before performing an update
self.angular_threshold = math.pi / 10 # the amount of angular movement before performing an update
self.max_dist = 2.0 # maximum penalty to assess in the likelihood field model
self.odom_pose = PoseStamped()
self.robot_pose = Pose()
self.robot_pose = Pose()
self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.process_scan)
# init pf
# subscribers and publisher
self.odom_sub = rospy.Subscriber("/odom", Odometry,
self.odom_particle_updater)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.pose_updater)
# enable listening for and broadcasting coord transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.current_odom_xy_theta = []
print("initialization complete")
self.initialized = True
def robot_pose_updater(self):
''' Update the estimate of the robot's pose given the updated particles by computing the mean pose'''
self.pf.particle_normalizer()
# Calculate avg particle position based on pose
mean_particle = Particle(0, 0, 0, 0)
mean_particle_theta_x = 0
mean_particle_theta_y = 0
for particle in self.pf.particle_cloud:
mean_particle.x += particle.x * particle.w
mean_particle.y += particle.y * particle.w
# Using trig to calculate angle (@Paul I hate Trigonometry!)
distance_vector = np.sqrt(
np.square(particle.y) + np.square(particle.x))
mean_particle_theta_x += distance_vector * np.cos(
particle.theta) * particle.w
mean_particle_theta_y += distance_vector * np.sin(
particle.theta) * particle.w
mean_particle.theta = np.arctan2(float(mean_particle_theta_y),
float(mean_particle_theta_x))
self.robot_pose = mean_particle.particle_to_pose()
# Get the laser messages
def laserCallback(self, msg):
self.laserCallback = msg
def odom_particle_updater(self, msg):
''' Updates particles based on new odom pose using a delta value for x,y,theta'''
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
# compute the change (delta) in x,y,theta
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
odom_noise = .25 # noise level
'''
updates the particles based on angle1, dist, and angle2.
angle1: Angle by which the robot rotates to face new xy position
dist: Distance to move forward to xy position
angle2: Angle by which the robot rotates to face final direction
'''
for particle in self.pf.particle_cloud:
# calculates angle1, d, and angle2
angle1 = np.arctan2(float(delta[1]), float(
delta[0])) - old_odom_xy_theta[2]
dist = np.sqrt(np.square(delta[0]) + np.square(delta[1]))
angle2 = delta[2] - angle1
# updates the particles with the above variables, while also adding in some noise
#This is the part of class where Paul moved and we all updated based on that movement
particle.theta = particle.theta + angle1 * (
random_sample() * odom_noise + (1 - odom_noise / 2.0))
particle.x = particle.x + dist * np.cos(
particle.theta) * (random_sample() * odom_noise +
(1 - odom_noise / 2.0))
particle.y = particle.y + dist * np.sin(
particle.theta) * (random_sample() * odom_noise +
(1 - odom_noise / 2.0))
particle.theta = particle.theta + angle2 * (
random_sample() * odom_noise + (1 - odom_noise / 2.0))
def particle_resampler(self):
'''Resample the particles according to the new particle weights.'''
# make sure the distribution is normalized
self.pf.particle_normalizer()
particle_list = []
weights = []
num_samples = len(self.pf.particle_cloud)
for particle in self.pf.particle_cloud:
particle_list.append(particle)
weights.append(particle.w)
self.pf.particle_cloud = self.draw_random_sample(
particle_list, weights, num_samples)
def draw_random_sample(self, choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
# sets up an index list for the chosen particles, and makes bins for the probabilities
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(
random_sample(n), bins
)] # chooses the new particles based on the probabilities of the old ones
samples = []
for i in inds:
samples.append(
deepcopy(choices[int(i)])
) # makes the new particle cloud based on the chosen particles
return samples
def laser_particle_updater(self, msg):
'''Updates the particle weights in response to the scan contained in the msg'''
# Find the total error for each particle based on 36 laser measurements taken from the Neato's actual position
for particle in self.pf.particle_cloud:
error = []
for theta in range(0, 360, 10):
rad = np.radians(theta)
err = self.occupancy_field.get_closest_obstacle_distance(
particle.x +
msg.ranges[theta] * np.cos(particle.theta + rad),
particle.y +
msg.ranges[theta] * np.sin(particle.theta + rad))
if (
math.isnan(err)
): # if the get_closest_obstacle_distance method finds that a point is out of bounds, then the particle can't ever be it
particle.w = 0
break
if (
sum(error) == 0
): # if the particle is basically a perfect match, then we make the particle almost always enter the next iteration through resampling
particle.w = 1.0
else:
particle.w = 1.0 / sum(
error
) # the errors are inverted such that large errors become small and small errors become large
def pose_updater(self, msg):
''' Restart particle filter based on updated pose '''
xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
msg.pose.pose)
#self.fix_map_to_odom_transform(msg)
self.pf.particle_cloud_init(xy_theta)
self.fix_map_to_odom_transform(msg)
print("particle cloud initialized")
def process_scan(self, msg):
'''Handling laser data to update our understanding of the robot based on laser and odometry'''
if not (self.initialized):
print("first if not")
# wait for initialization to complete
return
if not (self.tf_listener.canTransform('base_link', msg.header.frame_id,
msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not (self.tf_listener.canTransform('base_link', 'odom',
msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
pose = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, pose)
# find out where the robot thinks it is based on its odometry
pose = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, pose)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
if not (self.pf.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.pf.particle_cloud_init()
# cache the last odometry pose so we can only update our particle filter if we move more than self.linear_threshold or self.angular_threshold
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
print("Trying to initialize!")
self.fix_map_to_odom_transform(msg)
print("Initialized finally!")
self.pf.particle_publisher(msg)
else:
# we have moved far enough to do an update!
#self.odom_particle_updater(msg) # update based on odometry
#print("map!")
#self.laser_particle_updater(msg) # update based on laser scan
#self.robot_pose_updater() # update robot's pose
self.particle_resampler(
) # resample particles to focus on areas of high density
self.fix_map_to_odom_transform(
msg
) # update map to odom transform now that we have new particles
self.pf.particle_publisher(msg)
def fix_map_to_odom_transform(self, msg):
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer """
(translation, rotation) = \
self.transform_helper.convert_pose_inverse_transform(self.robot_pose)
pose = PoseStamped(
pose=self.transform_helper.convert_translation_rotation_to_pose(
translation, rotation),
header=Header(stamp=msg.header.stamp, frame_id=self.base_frame))
self.tf_listener.waitForTransform(self.base_frame,
self.odom_frame, msg.header.stamp,
rospy.Duration(1.0))
self.odom_to_map = self.tf_listener.transformPose(
self.odom_frame, pose)
(self.translation,
self.rotation) = self.transform_helper.convert_pose_inverse_transform(
self.odom_to_map.pose)
def send_last_map_to_odom_transform(self):
if not (hasattr(self, 'translation') and hasattr(self, 'rotation')):
print("sup dude")
return
self.tf_broadcaster.sendTransform(self.translation, self.rotation,
rospy.get_rostime(), 'odom', 'map')
class ParticleFilter():
def __init__(self):
#
rospy.init_node("ParticleFilter")
self.lidar_sub = rospy.Subscriber("/scan",LaserScan, self.lidar_callback)
self.odom_sub = rospy.Subscriber("/odom",Odometry, self.odom_callback)
self.all_particles_pub = rospy.Publisher("/visualization_particles", MarkerArray, queue_size=10)
self.init_particles_pub = rospy.Publisher("/visualization_init",MarkerArray,queue_size=10)
self.initial_estimate_sub = rospy.Subscriber("/initialpose",PoseWithCovarianceStamped,self.pose_estimate_callback)
self.occupancy_field = OccupancyField()
self.number_of_particles = 20
self.particles = np.ones([3,self.number_of_particles], dtype=np.float)
self.weights = np.ones(self.number_of_particles, dtype=np.float)
pos_std_dev = 0.25
ori_std_dev = 25 * math.pi / 180
self.initial_std_dev = np.array([[pos_std_dev, pos_std_dev, ori_std_dev]]).T
self.lidar_std_dev = 0.05
self.resample_threshold = 0.2
self.scan = None
self.prev_pose = None
self.delta_pose = None
self.initial_pose_estimate = None
self.pose = None
rospy.loginfo("Initialized")
def sample_points(self, mean, std):
# samples a uniform distribution of points
stds = np.repeat(std, self.number_of_particles, 1)
self.particles = np.random.normal(mean, stds,
[3,self.number_of_particles])
def resample_points(self):
# Takes the weights of each particle and resamples them according to that weight
replace_inds = self.weights < self.resample_threshold
kept_weights = self.weights[~replace_idns]
probs = kept_weights / sum(kept_weights)
if sum(replace_inds) == 0:
rospy.logerr("No weights")
else:
for i in replace_inds:
choice = np.random.choice(kept_weights.size, p=probs)
weight = kept_weights[choice]
self.particles[:, i] = self.particles[:, choice]
self.weights[i] = weight
def apply_odom_transform(self):
# Takes rotation and translation from odom and transforms the particles accordingly. Also adds a bit of noise
self.delta_pose = self.pose - self.prev_pose
delta_std = abs(self.delta_pose) * np.transpose([[1.5, 1.5, 0.7]])
noisy_deltas = self.sample_points(self.delta_pose, delta_std)
self.particles += noisy_deltas
self.prev_pose = self.pose
def lidar_callback(self,msg):
# Lidar Subscriber callback function.
self.scan = msg.ranges;
def odom_callback(self,msg):
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
t = euler_from_quaternion([msg.pose.pose.orientation.w,msg.pose.pose.orientation.x,msg.pose.pose.orientation.y,msg.pose.pose.orientation.z])[0]
pose = -np.array([[x,y,t]]).T
if self.prev_pose == None:
self.prev_pose = pose
self.pose = pose
else:
self.pose = pose
self.apply_odom_transform()
self.plot_particles(self.particles, ColorRGBA(1, 0, 0.5, 0.5), self.all_particles_pub)
def pose_estimate_callback(self,msg):
rospy.logdebug("Callback Good")
position = msg.pose.pose.position
orientation = euler_from_quaternion([msg.pose.pose.orientation.w,msg.pose.pose.orientation.x,msg.pose.pose.orientation.y,msg.pose.pose.orientation.z])[0]
pose = [position.x,position.y,orientation - math.pi]
self.initial_pose_estimate = np.array([pose]).T
def calc_prob(self):
# Reweight particles based on compatibility with laser scan
for i, p in enumerate(self.particles.T):
weight_sum = 0
xs,ys = self.polar_to_cartesian(self.scan, np.radians(range(361)),p[2])
lidar_x = xs + p[0]
lidar_y = ys + p[1]
# Average the probability associated with each LIDAR reading
for x, y in zip(lidar_x,lidar_y):
dist = self.occupancy_field.get_closest_obstacle_distance(x,y)
prob = norm(0, self.lidar_std_dev).pdf(dist) / (0.4 / self.lidar_std_dev)
weight_sum += prob
self.weights[i] = weight_sum / 361
def update_transform(self, pose, target_frame='base_laser_link'):
# Updates the transform between the map frame and the odom frame
br = TransformBroadcaster()
br.sendTransform((pose[0], pose[1], 0),
quaternion_from_euler(0, 0, pose[2]+math.pi),
rospy.get_rostime(),
target_frame,
'map')
def polar_to_cartesian(self, rs, thetas, theta_offset):
# read the function name ok
np_rs = np.array(rs)
np_thetas = np.array(thetas)+theta_offset
xs = np_rs * np.cos(np_thetas)
ys = np_rs * np.sin(np_thetas)
return xs, ys
def plot_particles(self, particles, color, pub):
marker_array = MarkerArray()
for i, particle in enumerate(particles):
nextMarker = Marker()
x = particle[0]
y = particle[1]
t = particle[2]
nextMarker.header = Header(stamp = rospy.Time.now(), frame_id="map")
nextMarker.id = i;
nextMarker.ns="particle"
nextMarker.type = Marker.ARROW
nextMarker.points = [Point(x,y,0), Point(x+math.cos(t), y+math.sin(t), 0)]
# nextMarker.pose = Pose(Point(x,y,0), Quaternion(0,0,0,0))
nextMarker.scale = Vector3(0.1,0.3,0.2)
nextMarker.color = color
nextMarker.action = Marker.ADD
marker_array.markers.append(nextMarker)
pub.publish(marker_array)
def main(self):
r = rospy.Rate(5)
while(not rospy.is_shutdown() and (self.scan == None or self.initial_pose_estimate == None or self.pose == None)):
rospy.logwarn("Still Missing Something")
r.sleep()
# TODO: get inital pose estimate from RVIZ
self.sample_points(self.initial_pose_estimate, self.initial_std_dev)
counter = 0
while(not rospy.is_shutdown()):
max_weight = max(self.weights)
best_pose = self.particles[self.weights.index(max_weight)]
self.update_transform(best_pose)
if counter % 20 == 0:
rospy.loginfo("Calculating Probability")
self.calc_prob()
self.resample_points()
counter += 1
r.sleep()
class ParticleFilter:
""" The class that represents a Particle Filter ROS Node
Attributes list:
initialized: a Boolean flag to communicate to other class methods that initialization is complete
base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots)
map_frame: the name of the map coordinate frame (should be "map" in most cases)
odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases)
scan_topic: the name of the scan topic to listen to (should be "scan" in most cases)
n_particles: the number of particles in the filter
d_thresh: the amount of linear movement before triggering a filter update
a_thresh: the amount of angular movement before triggering a filter update
laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation
pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI)
particle_pub: a publisher for the particle cloud
laser_subscriber: listens for new scan data on topic self.scan_topic
tf_listener: listener for coordinate transforms
tf_broadcaster: broadcaster for coordinate transforms
particle_cloud: a list of particles representing a probability distribution over robot poses
current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed.
The pose is expressed as a list [x,y,theta] (where theta is the yaw)
map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid
"""
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node(
'pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.n_particles = 150 # the number of particles to use
self.d_thresh = 0.2 # the amount of linear movement before performing an update
self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
self.radius = 2 # ac 109_1
#self.radius = 1 # ac_109_2
self.num_best_particles = 5 # ac 109_1
# self.num_best_particles = 8 # ac 109_2
# standard deviation of random noise distribution (Gaussian) for updating particle with odom
# self.sigma_random_noise_update_odom = 0.01 # parameter for trying to not require a good initial estimate
self.sigma_random_noise_update_odom = 0.008
# standard deviation of p(z^k_t | x_t, map)
self.sigma_hit_update_scan = 0.01
#self.sigma_hit_update_scan = 0.05 # parameter for trying to not require a good initial estimate (did not work)
self.z_hit = 1
self.z_rand = 0
# Setup pubs and subs
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
# laser_subscriber listens for data from the lidar
rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.particle_cloud = []
# change use_projected_stable_scan to True to use point clouds instead of laser scans
self.use_projected_stable_scan = False
self.last_projected_stable_scan = None
if self.use_projected_stable_scan:
# subscriber to the odom point cloud
rospy.Subscriber("projected_stable_scan", PointCloud,
self.projected_scan_received)
self.current_odom_xy_theta = []
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.initialized = True
def update_robot_pose(self, timestamp):
""" Update the estimate of the robot's pose given the updated particles.
There are two logical methods for this:
(1): compute the mean pose
(2): compute the most likely pose (i.e. the mode of the distribution)
"""
# first make sure that the particle weights are normalized
self.normalize_particles()
# just to get started we will fix the robot's pose to always be at the origin
# self.robot_pose = Pose()
x_sum = 0
y_sum = 0
theta_sum = 0
# Take the average of the best particles to be the robot's pose estimate
particles_most_likely = sorted(self.particle_cloud,
key=lambda x: x.w,
reverse=True)
for p in particles_most_likely[0:self.num_best_particles]:
x_sum += p.x
y_sum += p.y
theta_sum += p.theta
# should not do this, for some reason messed up the yaw
# theta_sin_sum += math.sin(p.theta)
# theta_cos_sum += math.cos(p.theta)
x_avg = x_sum / self.num_best_particles
y_avg = y_sum / self.num_best_particles
theta_avg = theta_sum / self.num_best_particles
# theta_avg = math.atan2(theta_sin_sum, theta_cos_sum) / self.num_best_particles (this is bad)
mean_pose = Particle(x_avg, y_avg, theta_avg)
self.robot_pose = mean_pose.as_pose()
self.transform_helper.fix_map_to_odom_transform(
self.robot_pose, timestamp)
def projected_scan_received(self, msg):
self.last_projected_stable_scan = msg
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
# Update the odom of the particles accordingly
d = math.sqrt(delta[0]**2 + delta[1]**2)
for p in self.particle_cloud:
p.x += d * math.cos(p.theta) + normal(
0, self.sigma_random_noise_update_odom)
p.y += d * math.sin(p.theta) + normal(
0, self.sigma_random_noise_update_odom)
p.theta += delta[2] + normal(0,
self.sigma_random_noise_update_odom)
def resample_particles(self):
""" Resample the particles according to the new particle weights.
The weights stored with each particle should define the probability that a particular
particle is selected in the resampling step. You may want to make use of the given helper
function draw_random_sample.
"""
# make sure the distribution is normalized
self.normalize_particles()
# Sort the particles first
particles_most_likely = sorted(self.particle_cloud,
key=lambda x: x.w,
reverse=True)
new_particle_cloud = []
# Low variance resampler from Probabilistic Robotics p87
count_inv = 1.0 / self.n_particles # the case when particles have equal weights
r_num = random.uniform(
0, 1) * count_inv # draw a number in the interval [0,1/M]
for m in range(self.n_particles):
# Repeatedly add fixed amount to 1/M to random number r where 1/M represents the case where particles have
# equal weights
u = r_num + m * count_inv
c = 0 # cumulative weights of particles
for particle in particles_most_likely:
c += particle.w
# Add the first particle i such that the sum of weights of all particles from 0->i >= u
if c >= u:
new_particle_cloud.append(deepcopy(particle))
break
self.particle_cloud = new_particle_cloud
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
scans = msg.ranges
for particle in self.particle_cloud:
total_prob = 1
for angle, scan in enumerate(scans):
if not math.isinf(scan):
# convert scan measurement from view of the particle to map
x_scan = particle.x + scan * math.cos(particle.theta +
math.radians(angle))
y_scan = particle.y + scan * math.sin(particle.theta +
math.radians(angle))
d = self.occupancy_field.get_closest_obstacle_distance(
x_scan, y_scan)
# Compute p(z^k_t | x_t, map)
p_z_hit = self.z_hit * math.exp(
-d**2 / (2 * (self.sigma_hit_update_scan**2))
) / (self.sigma_hit_update_scan * math.sqrt(2 * math.pi))
p_z_rand = self.z_rand / self.laser_max_distance # z_random / z_max Probabilistic Robotics p143
p_z = p_z_hit + p_z_rand
# We sum the cube of the probability
# total_prob += p_z ** 3 # trying to make the model not require a good initial estimate
total_prob += p_z**6
# total_prob = total_prob/len(msg.ranges) # It works better not to average -> converge faster
# Reassign weight with newly computed p(z_t | x_t, map)
particle.w = total_prob
self.normalize_particles()
@staticmethod
def draw_random_sample(choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(random_sample(n), bins)]
samples = []
for i in inds:
samples.append(deepcopy(choices[int(i)]))
return samples
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
msg.pose.pose)
self.initialize_particle_cloud(msg.header.stamp, xy_theta)
def initialize_particle_cloud(self, timestamp, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is omitted, the odometry will be used """
if xy_theta is None:
xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
self.particle_cloud = []
self.particle_cloud.append(
Particle(xy_theta[0], xy_theta[1], xy_theta[2]))
for p in range(self.n_particles - 1):
p_yaw = random.uniform(0, 2 * math.pi)
radius = random.uniform(0, 1) * self.radius
theta_facing = random.uniform(0, 2 * math.pi)
# Forward x axis of Neato is x
p_x = radius * math.sin(theta_facing) + xy_theta[0]
p_y = radius * math.cos(theta_facing) + xy_theta[1]
self.particle_cloud.append(Particle(p_x, p_y, p_yaw))
self.normalize_particles()
self.update_robot_pose(timestamp)
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
total_weight = 0
for p in self.particle_cloud:
total_weight += p.w
for p in self.particle_cloud:
p.w = p.w / total_weight
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(
PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, we hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not (self.initialized):
# wait for initialization to complete
return
if not (self.tf_listener.canTransform(
self.base_frame, msg.header.frame_id, msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
# wait a little while to see if the transform becomes available. This fixes a race
# condition where the scan would arrive a little bit before the odom to base_link transform
# was updated.
self.tf_listener.waitForTransform(self.base_frame,
self.odom_frame, msg.header.stamp,
rospy.Duration(0.5))
if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame,
msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative to the robot base
p = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
if not self.current_odom_xy_theta:
self.current_odom_xy_theta = new_odom_xy_theta
return
if not (self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud(msg.header.stamp)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) >
self.d_thresh
or math.fabs(new_odom_xy_theta[1] -
self.current_odom_xy_theta[1]) > self.d_thresh
or math.fabs(new_odom_xy_theta[2] -
self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
if self.last_projected_stable_scan:
last_projected_scan_timeshift = deepcopy(
self.last_projected_stable_scan)
last_projected_scan_timeshift.header.stamp = msg.header.stamp
self.scan_in_base_link = self.tf_listener.transformPointCloud(
"base_link", last_projected_scan_timeshift)
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose(msg.header.stamp) # update robot's pose
self.resample_particles(
) # resample particles to focus on areas of high density
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
class ParticleFilter:
""" The class that represents a Particle Filter ROS Node
Attributes list:
initialized: a Boolean flag to communicate to other class methods that initializaiton is complete
base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots)
map_frame: the name of the map coordinate frame (should be "map" in most cases)
odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases)
scan_topic: the name of the scan topic to listen to (should be "scan" in most cases)
n_particles: the number of particles in the filter
d_thresh: the amount of linear movement before triggering a filter update
a_thresh: the amount of angular movement before triggering a filter update
laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation
pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI)
particle_pub: a publisher for the particle cloud
laser_subscriber: listens for new scan data on topic self.scan_topic
tf_listener: listener for coordinate transforms
tf_broadcaster: broadcaster for coordinate transforms
particle_cloud: a list of particles representing a probability distribution over robot poses
current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed.
The pose is expressed as a list [x,y,theta] (where theta is the yaw)
map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid
"""
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node(
'pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.n_particles = 1000 # the number of particles to use
self.n_angles = 20 # the number of angles to use from the range scan. Value contained in the interval [1,360]
self.d_thresh = 0.2 # the amount of linear movement before performing an update
self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
self.std_x = 1 # std of x
self.std_y = 1 # std of y
self.marker_multiplier = self.n_particles / 5
# Setup pubs and subs
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
self.pose_listener = rospy.Subscriber("initialpose",
PoseWithCovarianceStamped,
self.update_initial_pose)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
self.marker_pub = rospy.Publisher("markerpub",
MarkerArray,
queue_size=10)
# laser_subscriber listens for data from the lidar
self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan,
self.scan_received)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.particle_cloud = []
self.current_odom_xy_theta = []
# request the map from the map server, the map should be of type nav_msgs/OccupancyGrid
rospy.wait_for_service('static_map')
get_map = rospy.ServiceProxy('static_map', GetMap)
map = get_map().map
# for now we have commented out the occupancy field initialization until you can successfully fetch the map
self.occupancy_field = OccupancyField(map)
self.initialized = True
def update_robot_pose(self):
""" Update the estimate of the robot's pose given the updated particles.
There are two logical methods for this:
(1): compute the mean pose
(2): compute the most likely pose (i.e. the mode of the distribution)
"""
# first make sure that the particle weights are normalized
self.normalize_particles()
mean_x = sum(
[particle.w * particle.x for particle in self.particle_cloud])
mean_y = sum(
[particle.w * particle.y for particle in self.particle_cloud])
theta_x = np.mean([
particle.w * math.cos(particle.theta)
for particle in self.particle_cloud
])
theta_y = np.mean([
particle.w * math.sin(particle.theta)
for particle in self.particle_cloud
])
mean_theta = math.atan2(theta_y, theta_x)
self.robot_pose = Particle(mean_x, mean_y, mean_theta).as_pose()
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
print "delta: ", delta
x, y = delta[0], delta[1]
r = math.sqrt(x**2 + y**2)
theta = np.arctan2(float(y), float(x))
phi = theta - old_odom_xy_theta[2]
for particle in self.particle_cloud:
particle.x += r * math.cos(phi + particle.theta)
particle.y += r * math.sin(phi + particle.theta)
particle.theta += delta[2]
# particle.theta += phi + delta[2]
# TODO: modify particles using delta
# For added difficulty: Implement sample_motion_odometry (Prob Rob p 136)
#wzc!!!!!!!!!!!!!????????????????????????????
def map_calc_range(self, x, y, theta):
""" Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """
# TODO: nothing unless you want to try this alternate likelihood model
pass
def resample_particles(self):
""" Resample the particles according to the new particle weights.
The weights stored with each particle should define the probability that a particular
particle is selected in the resampling step. You may want to make use of the given helper
function draw_random_sample.
"""
# make sure the distribution is normalized
self.normalize_particles()
# draw_random_sample
self.particle_cloud = self.draw_random_sample(
self.particle_cloud,
[particle.w for particle in self.particle_cloud], self.n_particles)
self.normalize_particles()
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
test_angles = np.asarray(np.linspace(0, 360, self.n_angles + 1)[:-1],
dtype=int)
for angle in test_angles:
r_min_d = msg.ranges[angle]
# ignore when a scan finds nothing
if r_min_d == 0.0:
continue
for particle in self.particle_cloud:
ref_x = particle.x + r_min_d * math.cos(angle * np.pi / 180 +
particle.theta)
ref_y = particle.y + r_min_d * math.sin(angle * np.pi / 180 +
particle.theta)
p_min_d = self.occupancy_field.get_closest_obstacle_distance(
ref_x, ref_y)
particle.w *= math.exp(-p_min_d**2)
@staticmethod
def weighted_values(values, probabilities, size):
""" Return a random sample of size elements from the set values with the specified probabilities
values: the values to sample from (numpy.ndarray)
probabilities: the probability of selecting each element in values (numpy.ndarray)
size: the number of samples
"""
bins = np.add.accumulate(probabilities)
return values[np.digitize(random_sample(size), bins)]
@staticmethod
def draw_random_sample(choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the prob weighted_valuesability of selecting each element in choices represented as a list
n: the number of samples
"""
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(random_sample(n), bins)]
samples = []
for i in inds:
samples.append(deepcopy(choices[int(i)]))
return samples
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose)
self.initialize_particle_cloud(xy_theta)
self.fix_map_to_odom_transform(msg)
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
(x, y, theta) = xy_theta
# Create N particles centered around initial guess
for i in range(self.n_particles):
sample_x = x + randn() * self.std_x
sample_y = y + randn() * self.std_y
sample_theta = theta + uniform() * (2 * np.pi)
self.particle_cloud.append(
Particle(sample_x, sample_y, sample_theta))
self.normalize_particles()
self.update_robot_pose()
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
total = sum([particle.w for particle in self.particle_cloud])
for particle in self.particle_cloud:
particle.w /= total
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(
PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
marker_array = []
for index, particle in enumerate(self.particle_cloud):
marker = Marker(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
pose=particle.as_pose(),
type=0,
scale=Vector3(
x=particle.w * 2 * self.marker_multiplier,
y=particle.w * 1 * self.marker_multiplier,
z=particle.w * 4 * self.marker_multiplier),
id=index,
color=ColorRGBA(r=1, a=1))
marker_array.append(marker)
self.marker_pub.publish(MarkerArray(markers=marker_array))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not (self.initialized):
# wait for initialization to complete
return
if not (self.tf_listener.canTransform(
self.base_frame, msg.header.frame_id, msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame,
msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative to the robot base
p = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
#ZCW isnt laser in base frame alreay???
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# print new_odom_xy_theta
# print self.current_odom_xy_theta
if not (self.particle_cloud):
print "before initialize particle_cloud"
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
print "self.current_odom_xy_theta", self.current_odom_xy_theta
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) >
self.d_thresh
or math.fabs(new_odom_xy_theta[1] -
self.current_odom_xy_theta[1]) > self.d_thresh
or math.fabs(new_odom_xy_theta[2] -
self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose() # update robot's pose
self.resample_particles(
) # resample particles to focus on areas of high density
self.fix_map_to_odom_transform(
msg
) # update map to odom transform now that we have new particles
print "sum, length: ", (sum([
particle.w for particle in self.particle_cloud
]), len(self.particle_cloud))
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def fix_map_to_odom_transform(self, msg):
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer """
(translation,
rotation) = convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=convert_translation_rotation_to_pose(
translation, rotation),
header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame))
self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
(self.translation,
self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map
to odom transformation. This is necessary so things like
move_base can work properly. """
if not (hasattr(self, 'translation') and hasattr(self, 'rotation')):
return
self.tf_broadcaster.sendTransform(self.translation, self.rotation,
rospy.get_rostime(), self.odom_frame,
self.map_frame)
class ParticleFilter:
""" The class that represents a Particle Filter ROS Node
Attributes list:
initialized: a Boolean flag to communicate to other class methods that initializaiton is complete
base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots)
map_frame: the name of the map coordinate frame (should be "map" in most cases)
odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases)
scan_topic: the name of the scan topic to listen to (should be "scan" in most cases)
n_particles: the number of particles in the filter
d_thresh: the amount of linear movement before triggering a filter update
a_thresh: the amount of angular movement before triggering a filter update
laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation
pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI)
particle_pub: a publisher for the particle cloud
laser_subscriber: listens for new scan data on topic self.scan_topic
tf_listener: listener for coordinate transforms
tf_broadcaster: broadcaster for coordinate transforms
particle_cloud: a list of particles representing a probability distribution over robot poses
current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed.
The pose is expressed as a list [x,y,theta] (where theta is the yaw)
map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid
"""
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node(
'pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.n_particles = 300 # the number of particles to use
self.d_thresh = 0.2 # the amount of linear movement before performing an update
self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
# Setup pubs and subs
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
self.weight_pub = rospy.Publisher('visualization_marker',
MarkerArray,
queue_size=10)
# laser_subscriber listens for data from the lidar
rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
# Holds all particles
self.particle_cloud = []
# Holds pre-normalized probabilities for each particle
self.scan_probabilities = []
# change use_projected_stable_scan to True to use point clouds instead of laser scans
self.use_projected_stable_scan = False
self.last_projected_stable_scan = None
if self.use_projected_stable_scan:
# subscriber to the odom point cloud
rospy.Subscriber("projected_stable_scan", PointCloud,
self.projected_scan_received)
self.current_odom_xy_theta = []
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.initialized = True
def update_robot_pose(self, timestamp):
""" Update the estimate of the robot's pose given the updated particles.
There are two logical methods for this:
(1): compute the mean pose
(2): compute the most likely pose (i.e. the mode of the distribution)
"""
# first make sure that the particle weights are normalized
self.normalize_particles()
# Calculate the mean pose
if self.particle_cloud:
mean_x, mean_y, mean_theta = 0, 0, 0
for particle in self.particle_cloud:
mean_x += particle.x
mean_y += particle.y
mean_theta += particle.theta
mean_x /= len(self.particle_cloud)
mean_y /= len(self.particle_cloud)
mean_theta /= len(self.particle_cloud)
self.robot_pose = Particle(mean_x, mean_y, mean_theta).as_pose()
else:
self.robot_pose = Pose()
self.transform_helper.fix_map_to_odom_transform(
self.robot_pose, timestamp)
def projected_scan_received(self, msg):
self.last_projected_stable_scan = msg
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
# Modify particles using delta and inject noise.
for particle in self.particle_cloud:
# Step 1: turn particles in direction of translation
# Compute the unit vector of the desired heading to move in
heading_mag = math.sqrt(delta[0]**2 + delta[1]**2)
heading_uv = np.array(
[delta[0] / heading_mag, delta[1] / heading_mag])
# Compute the unit vector of the robot's current heading
robot_uv = np.array([
np.cos(self.current_odom_xy_theta[2]),
np.sin(self.current_odom_xy_theta[2])
])
# Calculate the angle r_1 that is between the current heading and target heading
r_1 = np.arccos(np.dot(robot_uv, heading_uv))
particle.theta += r_1 + np.random.normal(scale=.05)
# Step 2: move particles forward distance of translation
d = math.sqrt(delta[0]**2 +
delta[1]**2) + np.random.normal(scale=.05)
# Decompose the translation vector into x and y componenets
particle.x += d * np.cos(particle.theta)
particle.y += d * np.sin(particle.theta)
# Step 3: turn particles to final angle
r_2 = delta[2] - r_1
particle.theta += r_2
def map_calc_range(self, x, y, theta):
""" Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """
# TODO: nothing unless you want to try this alternate likelihood model
pass
def resample_particles(self):
""" Resample the particles according to the new particle weights.
The weights stored with each particle should define the probability that a particular
particle is selected in the resampling step. You may want to make use of the given helper
function draw_random_sample.
"""
# make sure the distribution is normalized
self.normalize_particles()
weights = []
for particle in self.particle_cloud:
weights.append(particle.w)
choices = self.draw_random_sample(self.particle_cloud, weights,
self.n_particles)
# Reset particle cloud
self.particle_cloud = []
# Populate particle cloud with sampled choices
for chosen_particle in choices:
self.particle_cloud.append(
Particle(chosen_particle.x, chosen_particle.y,
chosen_particle.theta, chosen_particle.w))
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
lidar_scan_angles = range(360)
# Populates lidar_scan list with (theta, distance) for each lidar scan angle
lidar_scan = []
for theta in lidar_scan_angles:
distance = msg.ranges[theta]
point = (theta, distance)
lidar_scan.append(point)
# Calculates the probability that each particle is the best estimate for the robot location
self.scan_probabilities = []
for p in self.particle_cloud:
particle_theta_prob = []
for point in lidar_scan:
x_vector = p.x + point[1] * math.cos(
math.radians(point[0]) + p.theta)
y_vector = p.y + point[1] * math.sin(
math.radians(point[0]) + p.theta)
closest_object = self.occupancy_field.get_closest_obstacle_distance(
x_vector, y_vector)
# Calculate probabilities using a tuned function
particle_theta_prob.append(1 / ((0.1 * closest_object)**2 + 1))
# Combine probability at every theta for every particle
self.scan_probabilities.append(
reduce(lambda a, b: a * b, particle_theta_prob))
@staticmethod
def draw_random_sample(choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(random_sample(n), bins)]
samples = []
for i in inds:
samples.append(deepcopy(choices[int(i)]))
return samples
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
msg.pose.pose)
self.initialize_particle_cloud(msg.header.stamp, xy_theta)
def initialize_particle_cloud(self, timestamp, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is omitted, the odometry will be used """
if xy_theta is None:
xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
# Create particles based on gaussian distribution centered around xy_theta
self.particle_cloud = []
for g in range(self.n_particles):
x = np.random.normal(xy_theta[0], scale=0.3)
y = np.random.normal(xy_theta[1], scale=0.3)
theta = np.random.normal(xy_theta[2], scale=0.1)
self.particle_cloud.append(Particle(x, y, theta, 1))
self.normalize_particles()
self.update_robot_pose(timestamp)
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
# Check if scan probabilities has been populated for each particle
if len(self.scan_probabilities) == len(self.particle_cloud):
sum_of_prob = sum(self.scan_probabilities)
for i, particle in enumerate(self.particle_cloud):
particle.w = self.scan_probabilities[i] / sum_of_prob
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(
PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
def publish_weights(self, msg):
# Visualize particle weights in rviz to get a better debug each particle
weight_markers = MarkerArray()
for i, particle in enumerate(self.particle_cloud):
weight_markers.markers.append(particle.as_marker(i))
self.weight_pub.publish(weight_markers)
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, we hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not (self.initialized):
# wait for initialization to complete
return
# wait a little while to see if the transform becomes available. This fixes a race
# condition where the scan would arrive a little bit before the odom to base_link transform
# was updated.
self.tf_listener.waitForTransform(self.base_frame, msg.header.frame_id,
msg.header.stamp,
rospy.Duration(0.5))
if not (self.tf_listener.canTransform(
self.base_frame, msg.header.frame_id, msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame,
msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative to the robot base
p = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
if not self.current_odom_xy_theta:
self.current_odom_xy_theta = new_odom_xy_theta
return
if not (self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud(msg.header.stamp)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) >
self.d_thresh
or math.fabs(new_odom_xy_theta[1] -
self.current_odom_xy_theta[1]) > self.d_thresh
or math.fabs(new_odom_xy_theta[2] -
self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
if self.last_projected_stable_scan:
last_projected_scan_timeshift = deepcopy(
self.last_projected_stable_scan)
last_projected_scan_timeshift.header.stamp = msg.header.stamp
self.scan_in_base_link = self.tf_listener.transformPointCloud(
"base_link", last_projected_scan_timeshift)
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose(msg.header.stamp) # update robot's pose
self.resample_particles(
) # resample particles to focus on areas of high density
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
self.publish_weights(msg)
class ParticleFilter:
""" The class that represents a Particle Filter ROS Node
Attributes list:
initialized: a Boolean flag to communicate to other class methods that initializaiton is complete
base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots)
map_frame: the name of the map coordinate frame (should be "map" in most cases)
odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases)
scan_topic: the name of the scan topic to listen to (should be "scan" in most cases)
n_particles: the number of particles in the filter
d_thresh: the amount of linear movement before triggering a filter update
a_thresh: the amount of angular movement before triggering a filter update
laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation
pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI)
particle_pub: a publisher for the particle cloud
laser_subscriber: listens for new scan data on topic self.scan_topic
tf_listener: listener for coordinate transforms
tf_broadcaster: broadcaster for coordinate transforms
particle_cloud: a list of particles representing a probability distribution over robot poses
current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed.
The pose is expressed as a list [x,y,theta] (where theta is the yaw)
map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid
"""
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node(
'pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.n_particles = 500 # the number of particles to use
self.d_thresh = 0.1 # the amount of linear movement before performing an update
self.a_thresh = math.pi / 12 # the amount of angular movement before performing an update
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
# Setup pubs and subs
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
# laser_subscriber listens for data from the lidar
rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.particle_cloud = []
# change use_projected_stable_scan to True to use point clouds instead of laser scans
self.use_projected_stable_scan = False
self.last_projected_stable_scan = None
if self.use_projected_stable_scan:
# subscriber to the odom point cloud
rospy.Subscriber("projected_stable_scan", PointCloud,
self.projected_scan_received)
self.current_odom_xy_theta = []
# request the map from the map server
rospy.wait_for_service('static_map')
try:
map_server = rospy.ServiceProxy('static_map', GetMap)
map = map_server().map
print map.info.resolution
except:
print "Service call failed!"
# initializes the occupancyfield which contains the map
self.occupancy_field = OccupancyField(map)
print "initialized"
self.initialized = True
def update_robot_pose(self):
""" Update the estimate of the robot's pose given the updated particles.
There are two logical methods for this:
(1): compute the mean pose
(2): compute the most likely pose (i.e. the mode of the distribution)
"""
# first make sure that the particle weights are normalized
self.normalize_particles()
# for the pose, calculate the particle's mean location
mean_particle = Particle(0, 0, 0, 0)
mean_particle_theta_x = 0
mean_particle_theta_y = 0
for particle in self.particle_cloud:
mean_particle.x += particle.x * particle.w
mean_particle.y += particle.y * particle.w
# angle is calculated using trig to account for angle runover
distance_vector = np.sqrt(
np.square(particle.y) + np.square(particle.x))
mean_particle_theta_x += distance_vector * np.cos(
particle.theta) * particle.w
mean_particle_theta_y += distance_vector * np.sin(
particle.theta) * particle.w
mean_particle.theta = np.arctan2(float(mean_particle_theta_y),
float(mean_particle_theta_x))
self.robot_pose = mean_particle.as_pose()
def projected_scan_received(self, msg):
self.last_projected_stable_scan = msg
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
odom_noise = .3 # level of noise put into particles after update from odom to introduce variability
# updates the particles based on r1, d, and r2. For more information on this, consult the website
for particle in self.particle_cloud:
# calculates r1, d, and r2
r1 = np.arctan2(float(delta[1]), float(
delta[0])) - old_odom_xy_theta[2]
d = np.sqrt(np.square(delta[0]) + np.square(delta[1]))
r2 = delta[2] - r1
# updates the particles with the above variables, while also adding in some noise
particle.theta = particle.theta + r1 * (
random_sample() * odom_noise + (1 - odom_noise / 2.0))
particle.x = particle.x + d * np.cos(
particle.theta) * (random_sample() * odom_noise +
(1 - odom_noise / 2.0))
particle.y = particle.y + d * np.sin(
particle.theta) * (random_sample() * odom_noise +
(1 - odom_noise / 2.0))
particle.theta = particle.theta + r2 * (
random_sample() * odom_noise + (1 - odom_noise / 2.0))
def resample_particles(self):
""" Resample the particles according to the new particle weights.
The weights stored with each particle should define the probability that a particular
particle is selected in the resampling step. You may want to make use of the given helper
function draw_random_sample.
"""
# make sure the distribution is normalized
self.normalize_particles()
# creates choices and probabilities lists, which are the particles and their respective weights
choices = []
probabilities = []
num_samples = len(self.particle_cloud)
for particle in self.particle_cloud:
choices.append(particle)
probabilities.append(particle.w)
# re-makes the particle cloud according to a random sample based on the probability distribution of the weights
self.particle_cloud = self.draw_random_sample(choices, probabilities,
num_samples)
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
# for each particle, find the total error based on 36 laser measurements taken from the Neato's actual position
for particle in self.particle_cloud:
error = []
for theta in range(0, 360, 10):
rad = np.radians(theta)
err = self.occupancy_field.get_closest_obstacle_distance(
particle.x +
msg.ranges[theta] * np.cos(particle.theta + rad),
particle.y +
msg.ranges[theta] * np.sin(particle.theta + rad))
if (
math.isnan(err)
): # if the get_closest_obstacle_distance method finds that a point is out of bounds, then the particle can't never be it
particle.w = 0
break
error.append(
err**5
) # each error is appended up to a power to make more likely particles have higher probability
if (
sum(error) == 0
): # if the particle is basically a perfect match, then we make the particle almost always enter the next iteration through resampling
particle.w = 1.0
else:
particle.w = 1.0 / sum(
error
) # the errors are inverted such that large errors become small and small errors become large
@staticmethod
def draw_random_sample(choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
# sets up an index list for the chosen particles, and makes bins for the probabilities
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(
random_sample(n), bins
)] # chooses the new particles based on the probabilities of the old ones
samples = []
for i in inds:
samples.append(
deepcopy(choices[int(i)])
) # makes the new particle cloud based on the chosen particles
return samples
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose)
self.initialize_particle_cloud(xy_theta)
self.fix_map_to_odom_transform(msg)
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
# levels of noise to introduce variability
lin_noise = 1
ang_noise = math.pi / 2.0
# if doesn't exist, use odom
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# make a new particle cloud, and then create a bunch of particles at the initial location with some added noise
self.particle_cloud = []
for x in range(self.n_particles):
x = xy_theta[0] + (random_sample() * lin_noise - (lin_noise / 2.0))
y = xy_theta[1] + (random_sample() * lin_noise - (lin_noise / 2.0))
theta = xy_theta[2] + (random_sample() * ang_noise -
(ang_noise / 2.0))
self.particle_cloud.append(Particle(x, y, theta))
# normalize particles because all weights were originall set to 1 on default
self.normalize_particles()
self.update_robot_pose()
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
# takes the sum, and then divides all weights by the sum
weights_sum = sum(particle.w for particle in self.particle_cloud)
for particle in self.particle_cloud:
particle.w /= weights_sum
def publish_particles(self, msg):
"""Publishes the particles out for visualization and other purposes"""
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(
PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not (self.initialized):
# wait for initialization to complete
return
if not (self.tf_listener.canTransform(
self.base_frame, msg.header.frame_id, msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame,
msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
p = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not (self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) >
self.d_thresh
or math.fabs(new_odom_xy_theta[1] -
self.current_odom_xy_theta[1]) > self.d_thresh
or math.fabs(new_odom_xy_theta[2] -
self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
if self.last_projected_stable_scan:
last_projected_scan_timeshift = deepcopy(
self.last_projected_stable_scan)
last_projected_scan_timeshift.header.stamp = msg.header.stamp
self.scan_in_base_link = self.tf_listener.transformPointCloud(
"base_link", last_projected_scan_timeshift)
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose() # update robot's pose
self.resample_particles(
) # resample particles to focus on areas of high density
self.fix_map_to_odom_transform(
msg
) # update map to odom transform now that we have new particles
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def fix_map_to_odom_transform(self, msg):
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer"""
(translation,
rotation) = convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=convert_translation_rotation_to_pose(
translation, rotation),
header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame))
self.tf_listener.waitForTransform(self.base_frame,
self.odom_frame, msg.header.stamp,
rospy.Duration(1.0))
self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
(self.translation,
self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map
to odom transformation. This is necessary so things like
move_base can work properly. """
if not (hasattr(self, 'translation') and hasattr(self, 'rotation')):
return
self.tf_broadcaster.sendTransform(self.translation, self.rotation,
rospy.get_rostime(), self.odom_frame,
self.map_frame)
class ParticleFilter():
def __init__(self):
# Initialize node and attributes
rospy.init_node("ParticleFilter")
# Subscribers
self.lidar_sub = rospy.Subscriber("/scan", LaserScan,
self.lidar_callback)
self.odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback)
self.initial_estimate_sub = rospy.Subscriber(
"/initialpose", PoseWithCovarianceStamped,
self.pose_estimate_callback)
# publishers
self.all_particles_pub = rospy.Publisher("/visualization_particles",
MarkerArray,
queue_size=10)
self.init_particles_pub = rospy.Publisher("/visualization_init",
MarkerArray,
queue_size=10)
self.new_particles_pub = rospy.Publisher("/particlecloud",
PoseArray,
queue_size=10)
# constants
self.number_of_particles = 30
pos_std_dev = 0.25
ori_std_dev = 25 * math.pi / 180
self.initial_std_dev = np.array(
[[pos_std_dev, pos_std_dev, ori_std_dev]]).T
self.lidar_std_dev = 0.02
self.resample_threshold = 0.1
# changing attributes
self.particles = np.ones([3, self.number_of_particles], dtype=np.float)
self.weights = np.ones(self.number_of_particles, dtype=np.float)
self.odom_tf_time = 0
self.base_tf_time = 0
self.scan = None
self.prev_pose = None
self.delta_pose = None
self.initial_pose_estimate = None
self.pose = None
# helper classes
self.occupancy_field = OccupancyField()
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.transform_helper = TFHelper()
rospy.loginfo("Initialized")
def lidar_callback(self, msg):
# Lidar Subscriber callback function.
self.scan = msg.ranges
def odom_callback(self, msg):
# Odometry Subscriber callback function.
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
t = euler_from_quaternion([
msg.pose.pose.orientation.w, msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y, msg.pose.pose.orientation.z
])[0]
pose = -np.array([[x, y, t]]).T
if self.prev_pose is None:
self.prev_pose = pose
self.pose = pose
self.apply_odom_transform()
self.plot_particles(self.particles)
self.transform_helper.send_last_map_to_odom_transform()
def pose_estimate_callback(self, msg):
# Rviz pose estimate callback function.
position = msg.pose.pose.position
orientation = euler_from_quaternion([
msg.pose.pose.orientation.w, msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y, msg.pose.pose.orientation.z
])[0]
pose = [position.x, position.y, -orientation - math.pi]
self.initial_pose_estimate = np.array([pose]).T
def sample_points(self, mean, std):
# samples a uniform distribution of points
stds = np.repeat(std, self.number_of_particles, 1)
return np.random.normal(mean, stds, [3, self.number_of_particles])
def resample_points(self):
# Takes the weights of each particle and resamples them according to that weight
replace = self.weights < self.resample_threshold
kept_weights = np.multiply(replace, self.weights)
probs = kept_weights / sum(kept_weights)
replace_inds = np.arange(self.weights.size)[replace]
if kept_weights.size == 0:
rospy.logerr("No particles meet threshold")
else:
for i in replace_inds:
choice = np.random.choice(kept_weights.size, p=probs)
weight = kept_weights[choice]
self.particles[:, i] = self.particles[:, choice]
self.weights[i] = weight
def apply_odom_transform(self):
# Takes rotation and translation from odom and transforms the particles accordingly. Also adds a bit of noise
self.delta_pose = self.pose - self.prev_pose
self.prev_pose = self.pose
if abs(
self.delta_pose[2]
) < 0.5: # this was added to get rid of the orientation wrap around effect
delta_std = abs(self.delta_pose) * np.transpose([[1.5, 1.5, 0.9]])
noisy_deltas = self.sample_points(self.delta_pose, delta_std)
self.particles = self.particles + noisy_deltas
def calc_prob(self):
# Reweight particles based on compatibility with laser scan
scan = self.scan
particles = self.particles
for i, p in enumerate(self.particles.T):
weight_sum = 0
xs, ys = self.polar_to_cartesian(self.scan, np.radians(range(361)),
p[2])
lidar_x = xs + p[0]
lidar_y = ys + p[1]
# Average the probability associated with each LIDAR reading
for x, y in zip(lidar_x[::2], lidar_y[::2]):
dist = self.occupancy_field.get_closest_obstacle_distance(x, y)
prob = norm(0, self.lidar_std_dev).pdf(dist) / (
0.4 / self.lidar_std_dev)
weight_sum += prob**3
self.weights[i] = weight_sum / len(lidar_x[::2])
def update_transform(self, pose, target_frame='base_laser_link'):
# Currently unused, Updates the transform between the map frame and the odom frame
if ((rospy.get_rostime() != self.odom_tf_time
and target_frame == 'odom')
or (rospy.get_rostime() != self.base_tf_time
and target_frame == 'base_laser_link')):
self.tf_broadcaster.sendTransform(
(pose[0], pose[1], 0),
quaternion_from_euler(0, 0, pose[2] + math.pi),
rospy.get_rostime(), target_frame, 'map')
if (target_frame == 'odom'):
self.odom_tf_time = rospy.get_rostime()
else:
self.base_tf_time = rospy.get_rostime()
def polar_to_cartesian(self, rs, thetas, theta_offset):
# read the function name ok
np_rs = np.array(rs)
np_thetas = np.array(thetas) + theta_offset
xs = np_rs * np.cos(np_thetas)
ys = np_rs * np.sin(np_thetas)
return xs, ys
def plot_particles(self, particles):
# plots the particles in the pose array particle cloud topic
pose_array = PoseArray()
pose_array.header = Header(stamp=rospy.Time.now(), frame_id="map")
for i, particle in enumerate(particles.T):
w = self.weights[i] * 10
nextPose = Pose()
nextPose.position = Point(x=particle[0], y=particle[1], z=0)
nextPose.orientation = Quaternion(
*quaternion_from_euler(0, 0, particle[2]))
pose_array.poses.append(nextPose)
self.new_particles_pub.publish(pose_array)
def calc_avg_particle(self):
# calculates the weighted average position of the particles
return np.sum(self.particles * self.weights, axis=1) / sum(
self.weights)
def main(self):
r = rospy.Rate(5)
while (not rospy.is_shutdown()
and (self.scan is None or self.initial_pose_estimate is None
or self.pose is None)):
rospy.logwarn(
"One of the necessary components has not yet been initialized")
r.sleep()
self.particles = self.sample_points(self.initial_pose_estimate,
self.initial_std_dev)
while (not rospy.is_shutdown()):
self.calc_prob()
best_pose = self.calc_avg_particle()
robot_pose = Pose(position=Point(x=best_pose[0], y=best_pose[1]),
orientation=Quaternion(
*quaternion_from_euler(0, 0, best_pose[2])))
self.transform_helper.fix_map_to_odom_transform(
robot_pose, rospy.Time.now())
self.resample_points()
r.sleep()
class ParticleFilter(object):
""" The class that represents a Particle Filter ROS Node
Attributes list:
initialized: a Boolean flag to communicate to other class methods that initializaiton is complete
base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots)
map_frame: the name of the map coordinate frame (should be "map" in most cases)
odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases)
scan_topic: the name of the scan topic to listen to (should be "scan" in most cases)
n_particles: the number of particles in the filter
d_thresh: the amount of linear movement before triggering a filter update
a_thresh: the amount of angular movement before triggering a filter update
laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation
pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI)
particle_pub: a publisher for the particle cloud
laser_subscriber: listens for new scan data on topic self.scan_topic
tf_listener: listener for coordinate transforms
tf_broadcaster: broadcaster for coordinate transforms
particle_cloud: a list of particles representing a probability distribution over robot poses
current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed.
The pose is expressed as a list [x,y,theta] (where theta is the yaw)
map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid
"""
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node('pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.n_particles = 300 # the number of particles to use
self.p_lost = .4 # The probability given to the robot being "lost" at any given time
self.outliers_to_keep = int(self.n_particles * self.p_lost * 0.5) # The number of outliers to keep around
self.d_thresh = 0.2 # the amount of linear movement before performing an update
self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
# TODO: define additional constants if needed
# Setup pubs and subs
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10)
# laser_subscriber listens for data from the lidar
self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.particle_cloud = []
self.current_odom_xy_theta = []
# Make a ros service call to the /static_map service to get a nav_msgs/OccupancyGrid map.
# Then use OccupancyField to make the map object
robotMap = rospy.ServiceProxy('/static_map', GetMap)().map
self.occupancy_field = OccupancyField(robotMap)
print "OccupancyField initialized", self.occupancy_field
self.initialized = True
def update_robot_pose(self):
""" Update the estimate of the robot's pose given the updated particles.
There are two logical methods for this:
(1): compute the mean pose
(2): compute the most likely pose (i.e. the mode of the distribution)
Our strategy is #2 to enable better tracking of unlikely particles in the future
"""
# first make sure that the particle weights are normalized
self.normalize_particles()
chosen_one = max(self.particle_cloud, key=lambda p: p.w)
# TODO: assign the lastest pose into self.robot_pose as a geometry_msgs.Pose object
# just to get started we will fix the robot's pose to always be at the origin
self.robot_pose = chosen_one.as_pose()
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
angle_diff(new_odom_xy_theta[2], self.current_odom_xy_theta[2]))
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
for i, particle in enumerate(self.particle_cloud):
# TODO: Change odometry uncertainty to be ROS param
# Calculate the angle difference between the old odometry position
# and the old particle position. Then create a rotation matrix between
# the two angles
rotationmatrix = self.make_rotation_matrix(particle.theta - old_odom_xy_theta[2])
# rotate the motion vector, add the result to the particle
rotated_delta = np.dot(rotationmatrix, delta[:2])
linear_randomness = np.random.normal(1, 0.2)
angular_randomness = np.random.uniform(particle.turn_multiplier, 0.3)
particle.x += rotated_delta[0] * linear_randomness
particle.y += rotated_delta[1] * linear_randomness
particle.theta += delta[2] * angular_randomness
# Make sure the particle's angle doesn't wrap
particle.theta = angle_diff(particle.theta, 0)
def make_rotation_matrix(self, theta):
""" make_rotation_matrix returns a rotation matrix given angle theta
Args:
theta (number): the angle of rotation in radians CCW
Returns:
ndarray: a two by two rotation matrix
"""
sinTheta = np.sin(theta)
cosTheta = np.cos(theta)
return np.array([[cosTheta, -sinTheta],
[sinTheta, cosTheta]])
def map_calc_range(self, x, y, theta):
""" Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """
# TODO: nothing unless you want to try this alternate likelihood model
pass
def lost_particles(self):
""" lost_particles predicts which paricles are "lost" using unsupervised outlier detection.
In this case, we choose to use Scikit Learn - OneClassSVM
Args:
Returns:
inliers = particles that are not lost
outlier = particles that are lost
"""
# First format training data
x = [p.x for p in self.particle_cloud]
y = [p.y for p in self.particle_cloud]
X_train = np.array(zip(x, y))
# Next make unsupervised outlier detection model
# We have chosen to use OneClassSVM
# Lower nu to detect fewer outliers
# Here, we use 1/2 of the lost probability : self.p_lost / 2.0
clf = OneClassSVM(nu=.3, kernel="rbf", gamma=0.1)
clf.fit(X_train)
# Predict inliers and outliers
y_pred_train = clf.predict(X_train)
# Create inlier and outlier particle lists
inliers = []
outliers = []
# Iterate through particles and predictions to populate lists
for p, pred in zip(self.particle_cloud, y_pred_train):
if pred == 1:
inliers.append(p)
elif pred == -1:
outliers.append(p)
return inliers, outliers
def resample_particles(self):
""" Resample the particles according to the new particle weights.
The weights stored with each particle should define the probability that a particular
particle is selected in the resampling step. You may want to make use of the given helper
function draw_random_sample.
"""
# TODO: Dynamically decide how many particles we need
# make sure the distribution is normalized
self.normalize_particles()
# Calculate inlaying and exploring particle sets
inliers, outliers = self.lost_particles()
desired_outliers = int(self.n_particles * self.p_lost)
desired_inliers = int(self.n_particles - desired_outliers)
# Calculate the average turn_multiplier of the inliers
mean_turn_multipler = np.mean([p.turn_multiplier for p in inliers])
print "Estimated turn multiplier:", mean_turn_multipler
# Recalculate inliers
probabilities = [p.w for p in self.particle_cloud]
new_inliers = self.draw_random_sample(self.particle_cloud, probabilities, desired_inliers)
# Recalculate outliers
# This keeps some number of outlying particles around unchanged, and spreads the rest randomly around the map.
if desired_outliers > min(len(outliers), self.outliers_to_keep):
outliers.sort(key=lambda p: p.w, reverse=True)
num_to_make = desired_outliers - min(len(outliers), self.outliers_to_keep)
new_outliers = outliers[:self.outliers_to_keep] + \
[Particle().generate_uniformly_on_map(self.occupancy_field.map) for _ in xrange(num_to_make)]
for p in new_outliers:
p.turn_multiplier = mean_turn_multipler
else:
new_outliers = outliers[:desired_outliers]
# Set all of the weights back to the same value. Concentration of particles now reflects weight.
new_particles = new_inliers + new_outliers
for p in new_particles:
p.w = 1.0
p.turn_multiplier = np.random.normal(p.turn_multiplier, 0.1)
self.normalize_particles()
self.particle_cloud = new_particles
@staticmethod
def laser_uncertainty_model(distErr):
"""
Computes the probability of the laser returning a point distance distErr from the wall.
Note that this uses an exponential distribution instead of anything reasonable for computational speed.
Args:
distErr (float): The distance between the point returned and the nearest
wall on the map (in meters)
Returns:
probability (float): A probability, in the range 0...1
"""
# TODO: make these into rosparams
k = 0.1 # meters of half-life of distance probability for main distribution
probMiss = 0.05 # Base probability that the laser scan is totally confused
distErr = abs(distErr)
return (1 / (1 + probMiss)) * (probMiss + 1 / (distErr / k + 1))
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg
Args:
msg (LaserScan): incoming message
"""
# Transform to cartesian coordinates
scan_points = PointCloud()
scan_points.header = msg.header
for i, range in enumerate(msg.ranges):
if range == 0:
continue
# Calculate point in laser coordinate frame
angle = msg.angle_min + i * msg.angle_increment
x = range * np.cos(angle)
y = range * np.sin(angle)
scan_points.points.append(Point32(x=x, y=y))
# Transform into base_link coordinates
scan_points = self.tf_listener.transformPointCloud('base_link', scan_points)
# For each particle...
for particle in self.particle_cloud:
# Create a 3x3 matrix that transforms points from the origin to the particle
rotmatrix = np.matrix([[np.cos(particle.theta), -np.sin(particle.theta), 0],
[np.sin(particle.theta), np.cos(particle.theta), 0],
[0, 0, 1]])
transmatrix = np.matrix([[1, 0, particle.x],
[0, 1, particle.y],
[0, 0, 1]])
mat33 = np.dot(transmatrix, rotmatrix)
# Iterate through the points in the laser scan
probabilities = []
for point in scan_points.points:
# Move the point onto the particle
xy = np.dot(mat33, np.array([point.x, point.y, 1]))
# Figure out the probability of that point
distToWall = self.occupancy_field.get_closest_obstacle_distance(xy.item(0), xy.item(1))
if np.isnan(distToWall):
continue
probabilities.append(self.laser_uncertainty_model(distToWall))
# Combine those into probability of this scan given hypothesized location
# This is the bullshit thing Paul showed
# TODO: exponent should be a rosparam
totalProb = np.sum([p ** 3 for p in probabilities]) / len(probabilities)
# Update the particle's probability with new info
particle.w *= totalProb
# Normalize particles
self.normalize_particles()
@staticmethod
def weighted_values(values, probabilities, size):
""" Return a random sample of size elements from the set values with the specified probabilities
values: the values to sample from (numpy.ndarray)
probabilities: the probability of selecting each element in values (numpy.ndarray)
size: the number of samples
"""
bins = np.add.accumulate(probabilities)
return values[np.digitize(random_sample(size), bins)]
@staticmethod
def draw_random_sample(choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
Args:
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
Returns:
samples (List): A list of n elements, deep-copied from choices
"""
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(random_sample(n), bins)]
samples = []
for i in inds:
samples.append(deepcopy(choices[int(i)]))
return samples
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose)
self.initialize_particle_cloud(xy_theta)
self.fix_map_to_odom_transform(msg)
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta is None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
linear_variance = 0.5 # meters
angular_variance = 4
xs = np.random.normal(xy_theta[0], linear_variance, size=self.n_particles)
ys = np.random.normal(xy_theta[1], linear_variance, size=self.n_particles)
thetas = np.random.vonmises(xy_theta[2], angular_variance, size=self.n_particles)
self.particle_cloud = [Particle(x=xs[i], y=ys[i], theta=thetas[i]) for i in xrange(self.n_particles)]
self.normalize_particles()
self.update_robot_pose()
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
total = sum([p.w for p in self.particle_cloud])
if total != 0:
for p in self.particle_cloud:
p.w /= total
# Plan: divide each by the sum of all
# TODO: implement this
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not (self.initialized):
# wait for initialization to complete
return
if not (self.tf_listener.canTransform(self.base_frame, msg.header.frame_id, msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
startTime = time.clock()
# calculate pose of laser relative ot the robot base
p = PoseStamped(header=Header(stamp=rospy.Time(0),
frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not (self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose() # update robot's pose
self.resample_particles() # resample particles to focus on areas of high density
self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles
print "Calculation time: {}ms".format((time.clock() - startTime) * 1000)
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def fix_map_to_odom_transform(self, msg):
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer
TODO: if you want to learn a lot about tf, reimplement this... I can provide
you with some hints as to what is going on here. """
(translation, rotation) = convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=convert_translation_rotation_to_pose(translation, rotation),
header=Header(stamp=msg.header.stamp, frame_id=self.base_frame))
self.tf_listener.waitForTransform(self.base_frame, self.odom_frame, msg.header.stamp, rospy.Duration(1.0))
self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
(self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map
to odom transformation. This is necessary so things like
move_base can work properly. """
if not (hasattr(self, 'translation') and hasattr(self, 'rotation')):
return
self.tf_broadcaster.sendTransform(self.translation,
self.rotation,
rospy.get_rostime(),
self.odom_frame,
self.map_frame)
class SensorLikelihoodTest(object):
def __init__(self):
rospy.init_node("sensor_likelihood_test")
self.occupancy_field = OccupancyField()
self.tf_helper = TFHelper()
self.latest_scan_ranges = []
rospy.Subscriber('/scan', LaserScan, self.read_sensor)
self.odom_poses = PoseArray()
self.odom_poses.header.frame_id = "odom"
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.marker_pub = rospy.Publisher('/visualization_marker_array',
MarkerArray,
queue_size=10)
self.p_distrib = ParticleDistribution()
self.init_particles()
# self.p = Particle(x=0, y=0, theta=0, weight=1)
self.particle_poses = PoseArray()
self.particle_poses.header.frame_id = "map"
self.last_odom_pose = PoseStamped()
self.last_odom_pose.header.frame_id = "odom"
self.last_odom_pose.header.stamp = rospy.Time(0)
self.base_link_pose = PoseStamped()
self.base_link_pose.header.frame_id = "base_link"
self.base_link_pose.header.stamp = rospy.Time(0)
self.counter = 0
self.is_first = True
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
def init_particles(self):
self.p_distrib.particle_list = []
# generate initial list of hypothesis (particles)
for i in range(self.p_distrib.num_particles):
# Find a random valid point on the map
x = random.uniform(-2, 2)
y = random.uniform(-2, 2)
theta = random.randint(0, 361)
weight = 1.0 / self.p_distrib.num_particles
# Add new particle to list
self.p_distrib.particle_list.append(
Particle(x=x, y=y, theta=theta, weight=weight))
# Normalize weights
# self.p_distrib.normalize_weights()
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter
based on a pose estimate. These pose estimates could be generated
by another ROS Node or could come from the rviz GUI """
xy_theta = \
self.tf_helper.convert_pose_to_xy_and_theta(msg.pose.pose)
self.tf_helper.fix_map_to_odom_transform(msg.pose.pose,
msg.header.stamp)
self.tf_helper.send_last_map_to_odom_transform()
# initialize your particle filter based on the xy_theta tuple
def read_sensor(self, scan_msg):
self.latest_scan_ranges = scan_msg.ranges
def run(self):
self.tf_helper.fix_map_to_odom_transform(Pose(), rospy.Time(0))
r = rospy.Rate(1)
while not rospy.is_shutdown():
try:
print("Trying")
(trans, rot) = self.tf_helper.tf_listener.lookupTransform(
'/odom', '/base_link', rospy.Time(0))
# print("Transform: Trans: {} \n Rot: {}".format(trans, rot))
if (self.latest_scan_ranges != []):
new_odom_pose = self.tf_helper.tf_listener.transformPose(
'odom', self.base_link_pose)
self.odom_poses.poses.append(new_odom_pose.pose)
self.odom_poses.header.stamp = rospy.Time.now()
self.odom_pose_pub.publish(self.odom_poses)
# new_odom_pose_map = self.tf_helper.tf_listener.transformPose('map', new_odom_pose)
new_odom_x, new_odom_y, new_odom_theta = self.tf_helper.convert_pose_to_xy_and_theta(
new_odom_pose.pose)
last_odom_x, last_odom_y, last_odom_theta = self.tf_helper.convert_pose_to_xy_and_theta(
self.last_odom_pose.pose)
x_change = new_odom_x - last_odom_x
y_change = new_odom_y - last_odom_y
angular_change = self.tf_helper.angle_diff(
new_odom_theta, last_odom_theta) # radians
print("x: {}, y: {}, theta: {}".format(
x_change, y_change, degrees(angular_change)))
self.is_first = False
if not self.is_first:
self.propagate(x_change, y_change, angular_change)
self.last_odom_pose = new_odom_pose
# # Update particle to be in odom for check
# x, y, theta = self.tf_helper.convert_pose_to_xy_and_theta(new_odom_pose_map.pose)
# print("x: {}, y: {}, theta: {}".format(x, y, theta))
#
# self.p.x = x
# self.p.y = y
# self.p.theta = degrees(theta)
self.get_how_likely()
self.p_distrib.normalize_weights()
self.p_distrib.resample()
self.p_distrib.normalize_weights()
# self.tf_helper.send_last_map_to_odom_transform()
# x = raw_input("Press Enter to continue")
r.sleep()
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
def propagate(self, dx, dy, dtheta):
for p in self.p_distrib.particle_list:
# Update the last odom pose in the process
r = sqrt(dx**2 + dy**2)
angle = radians(
p.theta) # Add p.theta to account for particle's rotation
# print("p.theta: {} + new_angle: {} = angle: {}".format(radians(self.p.theta), atan2(dy, dx), angle))
p.x += r * cos(angle)
p.y += r * sin(angle)
p.theta = (p.theta + degrees(dtheta) +
random.randint(1, 180)) % 360 # Wrap angle
self.particle_poses = self.p_distrib.get_particle_pose_array()
self.particle_poses.header.stamp = rospy.Time.now()
self.particle_pose_pub.publish(self.particle_poses)
def get_how_likely(self):
angles = [0, 45] # Use only some of the angles for now
num_angles = 0
marker_arr = MarkerArray()
for p in self.p_distrib.particle_list:
for angle in angles:
reading = self.latest_scan_ranges[angle]
if (reading > 0.0):
print("Have a valid reading: {} at angle: {}".format(
reading, angle))
num_angles += 1
# Take into account robot's yaw
yaw = p.theta
angle_in_map = yaw + angle
predicted_obstacle_x, predicted_obstacle_y = self.move_coordinate(
p.x, p.y, angle_in_map, reading)
my_marker = Marker()
my_marker.header.stamp = rospy.Time.now()
my_marker.header.frame_id = "map"
my_marker.color.a = 0.5
my_marker.type = Marker.SPHERE
my_marker.id = self.counter
self.counter += 1
my_marker.pose.position.x = predicted_obstacle_x
my_marker.pose.position.y = predicted_obstacle_y
my_marker.lifetime = rospy.Time(1)
marker_arr.markers.append(my_marker)
# error = self.get_predicted_obstacle_error(
# reading, pos[0], pos[1], angle_in_map)
predicted_reading = self.occupancy_field.get_closest_obstacle_distance(
predicted_obstacle_x, predicted_obstacle_y)
print("Predicted x: {}, y: {}, reading: {}".format(
predicted_obstacle_x, predicted_obstacle_y,
predicted_reading))
error = predicted_reading
if (predicted_reading !=
predicted_reading): # Check for nan
print("Got Nan")
my_marker.color.g = 1.0
my_marker.scale.x = 0.1
my_marker.scale.y = 0.1
my_marker.scale.z = 0.1
else:
my_marker.color.b = 1.0 - error
my_marker.color.r = error + 0.1
my_marker.scale.x = error + 0.1
my_marker.scale.y = error + 0.1
my_marker.scale.z = error + 0.1
# Gaussian probability
sigma = 0.1
p.weight += math.exp((-error**2) / (2 * sigma**2))
if (num_angles > 0):
p.weight = p.weight / num_angles
self.marker_pub.publish(marker_arr)
def get_predicted_obstacle_error(self, distance_reading, x, y, angle):
# Predict location of objects based on laser scan reading.
predicted_obstacle_x, predicted_obstacle_y = self.move_coordinate(
x, y, angle, distance_reading)
# Find the closest obstacle to the predicted obstacle position
predicted_reading = self.occupancy_field.get_closest_obstacle_distance(
predicted_obstacle_x, predicted_obstacle_y)
print("Predicted x: {}, y: {}, reading: {}".format(
predicted_obstacle_x, predicted_obstacle_y, predicted_reading))
return predicted_reading
def move_coordinate(self, x, y, angle, distance):
return (x + cos(radians(angle)) * distance,
y + sin(radians(angle)) * distance)
def get_uniform_probability(self, error):
# x axis is width
max_width = 1.0 * self.occupancy_field.map.info.width * map_model.occupancy_field.map.info.resolution / 2.0
# y axis is height
max_height = 1.0 * self.occupancy_field.map.info.height * map_model.occupancy_field.map.info.resolution / 2.0
max_distance = sqrt(max_width**2 + max_height**2)
return (max_distance - error) / max_distance
class ParticleFilter:
""" The class that represents a Particle Filter ROS Node
Attributes list:
initialized: a Boolean flag to communicate to other class methods that initializaiton is complete
base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots)
map_frame: the name of the map coordinate frame (should be "map" in most cases)
odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases)
scan_topic: the name of the scan topic to listen to (should be "scan" in most cases)
n_particles: the number of particles in the filter
d_thresh: the amount of linear movement before triggering a filter update
a_thresh: the amount of angular movement before triggering a filter update
laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation
pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI)
particle_pub: a publisher for the particle cloud
laser_subscriber: listens for new scan data on topic self.scan_topic
tf_listener: listener for coordinate transforms
tf_broadcaster: broadcaster for coordinate transforms
particle_cloud: a list of particles representing a probability distribution over robot poses
current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed.
The pose is expressed as a list [x,y,theta] (where theta is the yaw)
map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid
"""
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node(
'pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.n_particles = 600 # the number of particles to use
self.particle_init_options = ParticleInitOptions.UNIFORM_DISTRIBUTION
self.d_thresh = 0.1 # the amount of linear movement before performing an update
self.a_thresh = math.pi / 12.0 # the amount of angular movement before performing an update
self.num_lidar_points = 180 # int from 1 to 360
# Note: self.laser_max_distance is NOT implemented
# TODO: Experiment with setting a max acceptable distance for lidar scans
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
# Setup pubs and subs
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
# laser_subscriber listens for data from the lidar
rospy.Subscriber(self.scan_topic,
LaserScan,
self.scan_received,
queue_size=1)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
# publish our hypotheses points
self.hypothesis_pub = rospy.Publisher("hypotheses",
MarkerArray,
queue_size=10)
# Publish our hypothesis points right before they get udpated through odom
self.before_odom_hypothesis_pub = rospy.Publisher(
"before_odom_hypotheses", MarkerArray, queue_size=10)
# Publish where the hypothesis points will be once they're updated with the odometry
self.future_hypothesis_pub = rospy.Publisher("future_hypotheses",
MarkerArray,
queue_size=10)
# Publish the lidar scan that pf.py sees
self.lidar_pub = rospy.Publisher("lidar_visualization",
MarkerArray,
queue_size=1)
# Publish the lidar scan projected from the first hypothesis
self.projected_lidar_pub = rospy.Publisher(
"projected_lidar_visualization", MarkerArray, queue_size=1)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.particle_cloud = []
# change use_projected_stable_scan to True to use point clouds instead of laser scans
self.use_projected_stable_scan = False
self.last_projected_stable_scan = None
if self.use_projected_stable_scan:
# subscriber to the odom point cloud
rospy.Subscriber("projected_stable_scan", PointCloud,
self.projected_scan_received)
self.current_odom_xy_theta = []
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.initialized = True
def update_robot_pose(self, timestamp):
""" Update the estimate of the robot's pose given the updated particles.
There are two logical methods for this:
(1): compute the mean pose
(2): compute the most likely pose (i.e. the mode of the distribution)
"""
# assign the best particle's pose to self.robot_pose as a geometry_msgs.Pose object
best_particle = self.particle_cloud[0]
for particle in self.particle_cloud[1:]:
if particle.w > best_particle.w:
best_particle = particle
self.robot_pose = best_particle.as_pose()
self.transform_helper.fix_map_to_odom_transform(
self.robot_pose, timestamp)
def projected_scan_received(self, msg):
self.last_projected_stable_scan = msg
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
# Publish a visualization of all our particles before they get updated
timestamp = rospy.Time.now()
particle_color = (1.0, 0.0, 0.0)
particle_markers = [
particle.as_marker(timestamp, count, "before_odom_hypotheses",
particle_color)
for count, particle in enumerate(self.particle_cloud)
]
# Publish the visualization of all the particles in Rviz
self.before_odom_hypothesis_pub.publish(
MarkerArray(markers=particle_markers))
# delta xy_theta is relative to the odom frame, which is a global frame
# Global Frame -> Robot Frame
# Delta also works for relative to robot _> need to rotate it properly
# Robot Frame - Rotate it so that it's projected from the particle in the particle frame
# Need the difference between the particle theta and the robot theta
# That's how much to rotate it by
# diff_theta = self.current_odom_xy_theta[2] -
# Particle Frame -> Global Frame
for index, particle in enumerate(self.particle_cloud):
diff_theta = self.current_odom_xy_theta[2] - (particle.theta -
math.pi)
partRotMtrx = np.array([[np.cos(diff_theta), -np.sin(diff_theta)],
[np.sin(diff_theta),
np.cos(diff_theta)]])
translationMtrx = np.array([[delta[0]], [delta[1]]])
partTranslationOp = partRotMtrx.dot(translationMtrx)
# update particle position to move with delta
self.particle_cloud[index].x -= partTranslationOp[0, 0]
self.particle_cloud[index].y -= partTranslationOp[1, 0]
self.particle_cloud[index].theta += delta[2]
if len(self.particle_cloud) == 1: # For debugging purposes
print("")
print("Robot Theta: ", self.current_odom_xy_theta[2])
print("Particle Theta:", particle.theta)
print("Diff Theta: ", diff_theta)
print("Deltas before transformations:\nDelta x: ", delta[0],
" | Delta y: ", delta[1], " | Delta theta: ", delta[2])
print("Deltas after transformations:\nDelta x: ",
partTranslationOp[0, 0], " | Delta y: ",
partTranslationOp[1, 0])
# Build up a list of all the just moved particles as Rviz Markers
timestamp = rospy.Time.now()
particle_color = (0.0, 0.0, 1.0)
particle_markers = [
particle.as_marker(timestamp, count, "future_hypotheses",
particle_color)
for count, particle in enumerate(self.particle_cloud)
]
# Publish the visualization of all the particles in Rviz
self.future_hypothesis_pub.publish(
MarkerArray(markers=particle_markers))
def map_calc_range(self, x, y, theta):
""" Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """
# TODO: nothing unless you want to try this alternate likelihood model
pass
def resample_particles(self):
""" Resample the particles according to the new particle weights.
The weights stored with each particle should define the probability that a particular
particle is selected in the resampling step. You may want to make use of the given helper
function draw_random_sample.
"""
# cull particles
# set looping variable values and initalize array to store significant points
def returnFunc(part):
return part.w
self.particle_cloud.sort(key=returnFunc, reverse=True)
numResamplingNodes = 500
resamplingNodes = self.particle_cloud[0:numResamplingNodes]
# Calculate the number of particles to cluster around each resamplingNode
cluster_size = math.ceil(
(self.n_particles - numResamplingNodes) / numResamplingNodes)
# Uniformly cluster the lowest weighted particles around the highest weighted particles (resamplingNodes)
num_cluster = 0
cluster_radius = 0.25
cluster_theta_range = math.pi / 2.0
for resamplingNode in resamplingNodes:
start_cluster_index = numResamplingNodes + num_cluster * cluster_size
end_cluster_index = start_cluster_index + cluster_size
if end_cluster_index > len(self.particle_cloud):
end_cluster_index = len(self.particle_cloud)
for particle_index in range(start_cluster_index,
end_cluster_index):
self.particle_cloud[particle_index].x = uniform(
(resamplingNode.x - cluster_radius),
(resamplingNode.x + cluster_radius))
self.particle_cloud[particle_index].y = uniform(
(resamplingNode.y - cluster_radius),
(resamplingNode.y + cluster_radius))
self.particle_cloud[particle_index].theta = uniform(
(resamplingNode.w - cluster_theta_range),
(resamplingNode.w + cluster_theta_range))
self.particle_cloud[particle_index].w = resamplingNode.w
# self.particle_cloud[particle_index].w = uniform((resamplingNode.w - cluster_theta_range),(resamplingNode.w + cluster_theta_range))
num_cluster += 1
# TODO: Experiment with clustering points dependending on weight of the resamplingNode
# #repopulate field
# #loop through all the significant weighted particles (or nodes in the probability field)
# nodeIndex = 0
# particleIndex = 0
# while nodeIndex < len(resamplingNodes):
# #place points around nodes
# placePointIndex = 0
# #loop through the number of points that need to be placed given the weight of the particle
# while placePointIndex < self.n_particles * resamplingNodes[nodeIndex].w:
# #place point in circular area around node
# radiusRepopCircle = resamplingNodes[nodeIndex].w*10.0
# #create a point in the circular area
# self.particle_cloud[particleIndex] = Particle(uniform((resamplingNodes[nodeIndex].x - radiusRepopCircle),(resamplingNodes[nodeIndex].x + radiusRepopCircle)),uniform((resamplingNodes[nodeIndex].y - radiusRepopCircle),(resamplingNodes[nodeIndex].y + radiusRepopCircle)),resamplingNodes[nodeIndex].theta)
# #update iteration variables
# particleIndex += 1
# placePointIndex += 1
# nodeIndex += 1
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
# Note: This only updates the weights. This does not move the particles themselves
# Only get the specified number of lidar points at regular slices
downsampled_angle_range_list = []
downsampled_angles = np.linspace(0, 360, self.num_lidar_points, False)
downsampled_angles_int = downsampled_angles.astype(int)
for angle, range_ in enumerate(msg.ranges[0:360]):
if angle in downsampled_angles_int:
downsampled_angle_range_list.append((angle, range_))
# Filter out invalid ranges
filtered_angle_range_list = []
for angle, range_ in downsampled_angle_range_list:
if range_ != 0.0:
filtered_angle_range_list.append((angle, range_))
# Transform ranges into numpy array of xs and ys
relative_to_robot = np.zeros((len(filtered_angle_range_list), 2))
for index, (angle, range_) in enumerate(filtered_angle_range_list):
relative_to_robot[index,
0] = range_ * np.cos(angle * np.pi / 180.0) # xs
relative_to_robot[index,
1] = range_ * np.sin(angle * np.pi / 180.0) # ys
# Build up an array of lidar markers for visualization
lidar_markers = []
for index, xy_point in enumerate(relative_to_robot):
lidar_markers.append(
build_lidar_marker(msg.header.stamp, xy_point[0], xy_point[1],
index, "base_link", "lidar_visualization",
(1.0, 0.0, 0.0)))
# Make sure to delete any old markers
num_deletion_markers = 360 - len(lidar_markers)
for _ in range(num_deletion_markers):
marker_id = len(lidar_markers)
lidar_markers.append(
build_deletion_marker(msg.header.stamp, marker_id,
"lidar_visualization"))
# Publish lidar points for visualization
self.lidar_pub.publish(MarkerArray(markers=lidar_markers))
# For every particle (hypothesis) we have
for particle in self.particle_cloud:
# Combine the xy positions of the scan with the xy w of the hypothesis
# Rotation matrix could be helpful here (https://en.wikipedia.org/wiki/Rotation_matrix)
# Build our rotation matrix
R = np.array([[np.cos(particle.theta), -np.sin(particle.theta)],
[np.sin(particle.theta),
np.cos(particle.theta)]])
# Rotate the points according to particle orientation
relative_to_particle = (R.dot(relative_to_robot.T)).T
# relative_to_particle = relative_to_robot.dot(R)
# Translate points to be relative to map origin
relative_to_map = deepcopy(relative_to_particle)
relative_to_map[:,
0:1] = relative_to_map[:,
0:1] + particle.x * np.ones(
(relative_to_map.
shape[0], 1))
relative_to_map[:,
1:2] = relative_to_map[:,
1:2] + particle.y * np.ones(
(relative_to_map.
shape[0], 1))
# Get the distances of each projected point to nearest obstacle
distance_list = []
for xy_projected_point in relative_to_map:
distance = self.occupancy_field.get_closest_obstacle_distance(
xy_projected_point[0], xy_projected_point[1])
if not np.isfinite(distance):
# Note: ac109 map has approximately a 10x10 bounding box
# Hardcode 1m as the default distance in case the projected point is off the map
distance = 1.0
distance_list.append(distance)
# Calculate a weight for for this particle
# Note: The further away a projected point is from an obstacle point,
# the lower its weight should be
weight = 1.0 / sum(distance_list)
particle.w = weight
# Normalize the weights
self.normalize_particles()
# Grab the first particle
particle = self.particle_cloud[0]
# Visualize the projected points around that particle
projected_lidar_markers = []
for index, xy_point in enumerate(relative_to_map):
projected_lidar_markers.append(
build_lidar_marker(msg.header.stamp, xy_point[0], xy_point[1],
index, "map",
"projected_lidar_visualization"))
# Make sure to delete any old markers
num_deletion_markers = 360 - len(projected_lidar_markers)
for _ in range(num_deletion_markers):
marker_id = len(projected_lidar_markers)
projected_lidar_markers.append(
build_deletion_marker(msg.header.stamp, marker_id,
"projected_lidar_visualization"))
# Publish the projection visualization to rviz
self.projected_lidar_pub.publish(
MarkerArray(markers=projected_lidar_markers))
# Build up a list of all the particles as Rviz Markers
timestamp = rospy.Time.now()
particle_markers = [
particle.as_marker(timestamp, count)
for count, particle in enumerate(n.particle_cloud)
]
# Publish the visualization of all the particles in Rviz
self.hypothesis_pub.publish(MarkerArray(markers=particle_markers))
@staticmethod
def draw_random_sample(choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(random_sample(n), bins)]
samples = []
for i in inds:
samples.append(deepcopy(choices[int(i)]))
return samples
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
msg.pose.pose)
self.initialize_particle_cloud(msg.header.stamp, xy_theta)
def initialize_particle_cloud(self, timestamp, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is omitted, the odometry will be used """
# TODO: Check if moving the xy_theta stuff to where the robot initializes around a given set of points is helpful
# if xy_theta is None:
# xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(self.odom_pose.pose)
# Check how the algorithm should initialize its particles
# Distribute particles uniformly with parameters defining the number of particles and bounding box
if self.particle_init_options == ParticleInitOptions.UNIFORM_DISTRIBUTION:
#create an index to track the x cordinate of the particles being created
#calculate the number of particles to place widthwize vs hightwize along the map based on the number of particles and the dimensions of the map
num_particles_x = math.sqrt(self.n_particles)
num_particles_y = num_particles_x
index_x = -3
#iterate over the map to place points in a uniform grid
while index_x < 4:
index_y = -4
while index_y < 3:
#create a particle at the location with a random orientation
new_particle = Particle(index_x, index_y,
uniform(0, 2 * math.pi))
#add the particle to the particle array
self.particle_cloud.append(new_particle)
#increment the index to place the next particle
index_y += 7 / (num_particles_y)
#increment index to place next column of particles
index_x += 7 / num_particles_x
# Distribute particles uniformly, but hard-coded (mainly for quick tests)
elif self.particle_init_options == ParticleInitOptions.UNIFORM_DISTRIBUTION_HARDCODED:
# Make a list of hypotheses that can update based on values
xs = np.linspace(-3, 4, 21)
ys = np.linspace(-4, 3, 21)
for y in ys:
for x in xs:
for i in range(5):
new_particle = Particle(
x, y, np.random.uniform(0, 2 * math.pi))
self.particle_cloud.append(new_particle)
# Create a single arbitrary particle (For debugging)
elif self.particle_init_options == ParticleInitOptions.SINGLE_PARTICLE:
new_particle = Particle(3.1, 0.0, -0.38802401685700466 + math.pi)
self.particle_cloud.append(new_particle)
# TODO: Set up robot pose on particle cloud initialization
# self.update_robot_pose(timestamp)
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
#set variable inital values
index = 0
weightSum = 0
# calulate the total particle weight
while index < len(self.particle_cloud):
weightSum += self.particle_cloud[index].w
index += 1
index = 0
#normalize the weight for each particle by divifdng by the total weight
while index < len(self.particle_cloud):
self.particle_cloud[
index].w = self.particle_cloud[index].w / weightSum
index += 1
pass
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(
PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, we hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not (self.initialized):
# wait for initialization to complete
return
if not (self.tf_listener.canTransform(
self.base_frame, msg.header.frame_id, msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame,
msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative to the robot base
p = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
if not self.current_odom_xy_theta:
self.current_odom_xy_theta = new_odom_xy_theta
return
if not (self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud(msg.header.stamp)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) >
self.d_thresh
or math.fabs(new_odom_xy_theta[1] -
self.current_odom_xy_theta[1]) > self.d_thresh
or math.fabs(new_odom_xy_theta[2] -
self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
if self.last_projected_stable_scan:
last_projected_scan_timeshift = deepcopy(
self.last_projected_stable_scan)
last_projected_scan_timeshift.header.stamp = msg.header.stamp
self.scan_in_base_link = self.tf_listener.transformPointCloud(
"base_link", last_projected_scan_timeshift)
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose(msg.header.stamp) # update robot's pose
self.resample_particles(
) # resample particles to focus on areas of high density
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
class ParticleFilter:
""" The class that represents a Particle Filter ROS Node
Attributes list:
initialized: a Boolean flag to communicate to other class methods that initializaiton is complete
base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots)
map_frame: the name of the map coordinate frame (should be "map" in most cases)
odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases)
scan_topic: the name of the scan topic to listen to (should be "scan" in most cases)
n_particles: the number of particles in the filter
d_thresh: the amount of linear movement before triggering a filter update
a_thresh: the amount of angular movement before triggering a filter update
laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation
pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI)
particle_pub: a publisher for the particle cloud
laser_subscriber: listens for new scan data on topic self.scan_topic
tf_listener: listener for coordinate transforms
tf_broadcaster: broadcaster for coordinate transforms
particle_cloud: a list of particles representing a probability distribution over robot poses
current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed.
The pose is expressed as a list [x,y,theta] (where theta is the yaw)
map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid
"""
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node('pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.n_particles = 300 # the number of particles to use
self.initial_uncertainty_xy = 1 # Amplitute factor of initial x and y uncertainty
self.initial_uncertainty_theta = 0.5 # Amplitude factor of initial theta uncertainty
self.variance_scale = 0.15 # Scaling term for variance effect on resampling
self.n_particles_average = 20 # Number of particles to average for pose update
self.linear_var_thresh = 0.05 # Maximum confidence along x/y (meters)
self.angular_var_thresh = 0.2 # Maximum confidence along theta (radians)
# self.resample_noise_xy = 0.1 # Amplitude factor of resample x and y noise
# self.resample_noise_theta = 0.1 # Amplitude factor of resample theta noise
self.d_thresh = 0.2 # the amount of linear movement before performing an update
self.a_thresh = math.pi/6 # the amount of angular movement before performing an update
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
# TODO: define additional constants if needed
# Setup pubs and subs
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10)
# publish custom particle array messge type
self.particle_viz_pub = rospy.Publisher("weighted_particlecloud", ParticleArray, queue_size=10)
# laser_subscriber listens for data from the lidar
rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.particle_cloud = []
# change use_projected_stable_scan to True to use point clouds instead of laser scans
self.use_projected_stable_scan = False
self.last_projected_stable_scan = None
if self.use_projected_stable_scan:
# subscriber to the odom point cloud
rospy.Subscriber("projected_stable_scan", PointCloud, self.projected_scan_received)
self.current_odom_xy_theta = []
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.initialized = True
def update_robot_pose(self, timestamp):
""" Update the estimate of the robot's pose given the updated particles.
There are two logical methods for this:
(1): compute the mean pose
(2): compute the most likely pose (i.e. the mode of the distribution)
Our implementation averages a couple of the best particles to update the
pose
"""
# first make sure that the particle weights are normalized
self.normalize_particles()
sum_x, sum_y, sum_theta = 0, 0, 0
# sort our particles by weight
best_particles = sorted(self.particle_cloud)
# take the top particles with the highest weights
best_particles = best_particles[-self.n_particles_average:]
# find the average of this subset
for p in best_particles:
sum_x += p.x
sum_y += p.y
sum_theta += p.theta
# Assign the latest pose into self.robot_pose as a Pose object
self.robot_pose = self.transform_helper.convert_xy_and_theta_to_pose(
sum_x/self.n_particles_average,
sum_y/self.n_particles_average,
sum_theta/self.n_particles_average)
self.transform_helper.fix_map_to_odom_transform(self.robot_pose, timestamp)
def projected_scan_received(self, msg):
self.last_projected_stable_scan = msg
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])# COUNTERCLOCKWISE
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
# update particles
r = math.sqrt(delta[0]**2 + delta[1]**2)
for particle in self.particle_cloud:
particle.x += r*math.cos(particle.theta)
particle.y += r*math.sin(particle.theta)
particle.theta += delta[2]
def map_calc_range(self,x,y,theta):
""" Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """
# TODO: nothing unless you want to try this alternate likelihood model
pass
def resample_particles(self):
""" Resample the particles according to the new particle weights.
The weights stored with each particle should define the probability that a particular
particle is selected in the resampling step. You may want to make use of the given helper
function draw_random_sample.
"""
xs, ys, thetas, weights = [],[],[],[]
# Make a list of particle stats for draw_random_sample to use
for p in self.particle_cloud:
xs.append(p.x)
ys.append(p.y)
thetas.append(p.theta)
weights.append(p.w)
# Throw out some particles
self.particle_cloud = self.draw_random_sample(self.particle_cloud,
weights, self.n_particles)
# Compute variance of particles
x_var = np.var(xs)
y_var = np.var(ys)
theta_var = np.var(weights)
# Set a threshold for minimum linear and angular variance
# This prevents our filter from becoming "overconfident" in the estimate
if x_var < self.linear_var_thresh:
x_var = self.linear_var_thresh
if y_var < self.linear_var_thresh:
y_var = self.linear_var_thresh
if theta_var < self.angular_var_thresh:
theta_var = self.angular_var_thresh
# Inject some noise into the new cloud based on current variance
for p in self.particle_cloud:
noise = np.random.randn(3)
p.x += noise[0] * x_var * self.variance_scale
p.y += noise[1] * y_var * self.variance_scale
p.theta += noise[2] * theta_var * self.variance_scale
self.normalize_particles()
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
for p in self.particle_cloud:
# Compute delta to x,y coords in map frame of each lidar point assuming
# lidar is centered at the base_link
# TODO: Account for the offset between the lidar and the base_link
angles = np.linspace(0, 2*math.pi, num=361)
dxs = np.array(msg.ranges) * np.cos(angles + p.theta)
dys = np.array(msg.ranges) * np.sin(angles + p.theta)
# Initialize total distance to 0
d = 0
# Initialize number of valid points
valid_pts = len(dxs)
for dx,dy in zip(dxs, dys):
# Ignore points with invalid ranges
if dx == 0 and dy == 0:
continue
# Apply delta
x = p.x + dx
y = p.y + dy
# Find nearest point to each lidar point according to map
dist = self.occupancy_field.get_closest_obstacle_distance(x, y)
# Check to make sure lidar point is actually on the map
if not np.isnan(dist):
d += dist
else:
valid_pts -= 1
# If there aren't enough valid points for the particle, assume that it's
# not good
# TODO: Add a ROS param threshold for this
if valid_pts < 10:
p.w = 0
else:
# Update particle weight based on inverse of average squared difference
if d != 0:
p.w = 1 / ((d ** 2)/valid_pts)
else:
# If difference is exactly 0, something's likely wrong
rospy.logwarn("Computed difference between particle projection and lidar scan is exactly 0")
self.normalize_particles()
@staticmethod
def draw_random_sample(choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(random_sample(n), bins)]
samples = []
for i in inds:
samples.append(deepcopy(choices[int(i)]))
return samples
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(msg.pose.pose)
self.initialize_particle_cloud(msg.header.stamp, xy_theta)
def initialize_particle_cloud(self, timestamp, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is omitted, the odometry will be used """
if xy_theta is None:
xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
for i in range(self.n_particles):
noise = (np.random.randn(3)*self.initial_uncertainty_xy)
noise[2] = np.random.randn()*self.initial_uncertainty_theta
new_pose = np.array(xy_theta) + noise
new_particle = Particle(*new_pose)
self.particle_cloud.append(new_particle)
self.normalize_particles()
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
cumulative_weight = 0
# Compute cumulative weight
for p in self.particle_cloud:
cumulative_weight += p.w
# Normalize weights
for p in self.particle_cloud:
p.w = p.w/cumulative_weight
def publish_particles(self, msg):
particles_conv = []
custom_particle_msgs = []
for p in self.particle_cloud:
particle_pose = p.as_pose()
particles_conv.append(particle_pose)
# Create new particle message
new_particle = Particle()
new_particle.pose = particle_pose
new_particle.weight = p.w
custom_particle_msgs.append(new_particle)
# actually send the message so that we can view it in rviz
self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
# send our custom message to visualize weights in rviz
self.particle_viz_pub.publish(ParticleArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
particles=custom_particle_msgs))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, we hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not(self.initialized):
# wait for initialization to complete
rospy.loginfo_once("Waiting for initial pose estimate...")
return
else:
rospy.loginfo_once("Initial pose estimate found!")
# wait a little while to see if the transform becomes available. This fixes a race
# condition where the scan would arrive a little bit before the odom to base_link transform
# was updated.
self.tf_listener.waitForTransform(self.base_frame, msg.header.frame_id, msg.header.stamp, rospy.Duration(0.5))
if not(self.tf_listener.canTransform(self.base_frame, msg.header.frame_id, msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not(self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative to the robot base
p = PoseStamped(header=Header(stamp=rospy.Time(0),
frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not self.current_odom_xy_theta:
self.current_odom_xy_theta = new_odom_xy_theta
return
if not(self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud(msg.header.stamp)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
if self.last_projected_stable_scan:
last_projected_scan_timeshift = deepcopy(self.last_projected_stable_scan)
last_projected_scan_timeshift.header.stamp = msg.header.stamp
self.scan_in_base_link = self.tf_listener.transformPointCloud("base_link", last_projected_scan_timeshift)
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose(msg.header.stamp) # update robot's pose
self.resample_particles() # resample particles to focus on areas of high density
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
class ParticleFilter:
""" The class that represents a Particle Filter ROS Node
Attributes list:
initialized: a Boolean flag to communicate to other class methods that initializaiton is complete
base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots)
map_frame: the name of the map coordinate frame (should be "map" in most cases)
odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases)
scan_topic: the name of the scan topic to listen to (should be "scan" in most cases)
n_particles: the number of particles in the filter
linear_mov: the amount of linear movement before triggering a filter update
angular_mov: the amount of angular movement before triggering a filter update
laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation
pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI)
particle_pub: a publisher for the particle cloud
laser_subscriber: listens for new scan data on topic self.scan_topic
tf_listener: listener for coordinate transforms
particle_cloud: a list of particles representing a probability distribution over robot poses
current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed.
The pose is expressed as a list [x,y,theta] (where theta is the yaw)
map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid
"""
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node('RMI_pf')
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.n_particles = 20
self.linear_mov = 0.1
self.angular_mov = math.pi / 10
self.laser_max_distance = 2.0
self.sigma = 0.05
# Descomentar essa linha caso /initialpose seja publicada
# self.pose_listener = rospy.Subscriber("initialpose",
# PoseWithCovarianceStamped,
# self.update_initial_pose)
self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan,
self.scan_received)
self.particle_pub = rospy.Publisher("particlecloud_rmi",
PoseArray,
queue_size=1)
self.tf_listener = TransformListener()
self.particle_cloud = []
self.current_odom_xy_theta = []
self.map_server = rospy.ServiceProxy('static_map', GetMap)
self.map = self.map_server().map
self.occupancy_field = OccupancyField(self.map)
self.tf_listener.waitForTransform(self.odom_frame, self.base_frame,
rospy.Time(), rospy.Duration(1.0))
self.initialized = True
def update_particles_with_odom(self, msg):
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# print 'new_odom_xy_theta', new_odom_xy_theta
# Pega a posicao da odom (x,y,tehta)
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
# print 'delta', delta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
for particle in self.particle_cloud:
d = math.sqrt((delta[0]**2) + (delta[1]**2))
# print 'particle_theta_1', particle.theta
particle.x += d * (math.cos(particle.theta) + normal(0, 0.01))
particle.y += d * (math.sin(particle.theta) + normal(0, 0.01))
particle.theta = self.current_odom_xy_theta[2] #+ normal(0,0.05)
# Systematic Resample
def resample_particles(self):
self.normalize_particles()
# for particle in self.particle_cloud:
# print 'TODAS PART', particle.w, particle.x, particle.y
weights = []
for particle in self.particle_cloud:
weights.append(particle.w)
newParticles = []
N = len(weights)
positions = (np.arange(N) + random.random()) / N
cumulative_sum = np.cumsum(weights)
i, j = 0, 0
while i < N:
if positions[i] < cumulative_sum[j]:
newParticles.append(deepcopy(self.particle_cloud[j]))
i += 1
else:
j += 1
self.particle_cloud = newParticles
def update_particles_with_laser(self, msg):
depths = []
for dist in msg.ranges:
if not np.isnan(dist):
depths.append(dist)
fullDepthsArray = msg.ranges[:]
# Verifica se ha objetos proximos ao robot
if len(depths) == 0:
self.closest = 0
self.position = 0
else:
self.closest = min(depths)
self.position = fullDepthsArray.index(self.closest)
# print 'self.position, self.closest', self.position, self.closest, self.xy_theta_aux
# print msg, '/scan'
for index, particle in enumerate(self.particle_cloud):
tot_prob = 0.0
for index, scan in enumerate(depths):
x, y = self.transform_scan(particle, scan, index)
# print 'x,y, scan', x, y, scan
# usa o metodo get_closest_obstacle_distance para buscar o obstaculo mais proximo dentro do range x,y do grid map
d = self.occupancy_field.get_closest_obstacle_distance(x, y)
# quanto mais proximo de zero mais relevante
tot_prob += math.exp((-d**2) / (2 * self.sigma**2))
tot_prob = tot_prob / len(depths)
if math.isnan(tot_prob):
particle.w = 1.0
else:
particle.w = tot_prob
# print 'LASER', particle.x, particle.y, particle.w
def transform_scan(self, particle, distance, theta):
return (particle.x +
distance * math.cos(math.radians(particle.theta + theta)),
particle.y +
distance * math.sin(math.radians(particle.theta + theta)))
def update_initial_pose(self, msg):
xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose)
self.initialize_particle_cloud(xy_theta)
def initialize_particle_cloud(self, xy_theta=None):
print 'Cria o set inicial de particulas'
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
x, y, theta = xy_theta
# Altere este parametro para aumentar a circunferencia do filtro de particulas
# Na VM ate 1 e suportado
rad = 0.5
self.particle_cloud = []
self.particle_cloud.append(
Particle(xy_theta[0], xy_theta[1], xy_theta[2]))
# print 'particle_values_W', self.particle_cloud[0].w
# print 'particle_values_X', self.particle_cloud[0].x
# print 'particle_values_Y', self.particle_cloud[0].y
# print 'particle_values_THETA', self.particle_cloud[0].theta
for i in range(self.n_particles - 1):
# initial facing of the particle
theta = random.random() * 360
# compute params to generate x,y in a circle
other_theta = random.random() * 360
radius = random.random() * rad
# x => straight ahead
x = radius * math.sin(other_theta) + xy_theta[0]
y = radius * math.cos(other_theta) + xy_theta[1]
particle = Particle(x, y, theta)
self.particle_cloud.append(particle)
self.normalize_particles()
def normalize_particles(self):
tot_weight = sum([particle.w
for particle in self.particle_cloud]) or 1.0
for particle in self.particle_cloud:
particle.w = particle.w / tot_weight
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose(
)) # transforma a particula em POSE para ser entendida pelo ROS
# print 'PARTII', [particles.x for particles in self.particle_cloud]
# Publica as particulas no rviz (particloud_rmi)
self.particle_pub.publish(
PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
def scan_received(self, msg):
# print msg
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not (self.initialized):
# wait for initialization to complete
return
if not (self.tf_listener.canTransform(
self.base_frame, msg.header.frame_id, msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame,
msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# print 'msg.header.frame_id', msg.header.frame_id
# calculate pose of laser relative ot the robot base
p = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
# listener.getLatestCommonTime("/base_link",object_pose_in.header.frame_id)
# p = PoseStamped(header=Header(stamp=msg.header.stamp,
p = PoseStamped(
header=Header(
stamp=self.tf_listener.getLatestCommonTime(
self.base_frame, self.map_frame),
# p = PoseStamped(header=Header(stamp=rospy.Time.now(),
frame_id=self.base_frame),
pose=Pose())
# p_aux = PoseStamped(header=Header(stamp=self.tf_listener.getLatestCommonTime("/base_link","/map"),
p_aux = PoseStamped(
header=Header(
stamp=self.tf_listener.getLatestCommonTime(
self.odom_frame, self.map_frame),
# p_aux = PoseStamped(header=Header(stamp=rospy.Time.now(),
frame_id=self.odom_frame),
pose=Pose())
odom_aux = self.tf_listener.transformPose(self.map_frame, p_aux)
odom_aux_xy_theta = convert_pose_to_xy_and_theta(odom_aux.pose)
# print 'odom_aux_xy_theta', odom_aux_xy_theta
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# print 'self.odom_pose', self.odom_pose
# (trans, root) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
# self.odom_pose = trans
# print trans, root
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# new_odom_xy_theta = convert_pose_to_xy_and_theta(self.laser_pose.pose)
xy_theta_aux = (new_odom_xy_theta[0] + odom_aux_xy_theta[0],
new_odom_xy_theta[1] + odom_aux_xy_theta[1],
new_odom_xy_theta[2])
self.xy_theta_aux = xy_theta_aux
if not (self.particle_cloud):
self.initialize_particle_cloud(xy_theta_aux)
self.current_odom_xy_theta = new_odom_xy_theta
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) >
self.linear_mov
or math.fabs(new_odom_xy_theta[1] -
self.current_odom_xy_theta[1]) > self.linear_mov
or math.fabs(new_odom_xy_theta[2] -
self.current_odom_xy_theta[2]) > self.angular_mov):
self.update_particles_with_odom(msg)
self.update_particles_with_laser(msg)
self.resample_particles()
self.publish_particles(msg)
class ParticleFilter:
""" The class that represents a Particle Filter ROS Node
Attributes list:
initialized: a Boolean flag to communicate to other class methods that initializaiton is complete
base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots)
map_frame: the name of the map coordinate frame (should be "map" in most cases)
odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases)
scan_topic: the name of the scan topic to listen to (should be "scan" in most cases)
n_particles: the number of particles in the filter
d_thresh: the amount of linear movement before triggering a filter update
a_thresh: the amount of angular movement before triggering a filter update
laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation
pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI)
particle_pub: a publisher for the particle cloud
laser_subscriber: listens for new scan data on topic self.scan_topic
tf_listener: listener for coordinate transforms
tf_broadcaster: broadcaster for coordinate transforms
particle_cloud: a list of particles representing a probability distribution over robot poses
current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed.
The pose is expressed as a list [x,y,theta] (where theta is the yaw)
map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid
"""
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node('pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "base_scan" # the topic where we will get laser scans from
self.n_particles = 500 # the number of particles to use
self.d_thresh = 0.2 # the amount of linear movement before performing an update
self.a_thresh = math.pi/6 # the amount of angular movement before performing an update
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
self.sigma = 0.08 # guess for how inaccurate lidar readings are in meters
# Setup pubs and subs
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10)
self.marker_pub = rospy.Publisher("markers", MarkerArray, queue_size=10)
# laser_subscriber listens for data from the lidar
self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.particle_cloud = []
self.current_odom_xy_theta = []
# request the map from the map server, the map should be of type nav_msgs/OccupancyGrid
self.map_server = rospy.ServiceProxy('static_map', GetMap)
self.map = self.map_server().map
# for now we have commented out the occupancy field initialization until you can successfully fetch the map
self.occupancy_field = OccupancyField(self.map)
self.initialized = True
def update_robot_pose(self):
""" Update the estimate of the robot's pose given the updated particles.
Computed by taking the weighted average of poses.
"""
# first make sure that the particle weights are normalized
self.normalize_particles()
x = 0
y = 0
theta = 0
angles = []
for particle in self.particle_cloud:
x += particle.x * particle.w
y += particle.y * particle.w
v = [particle.w * math.cos(math.radians(particle.theta)), particle.w * math.sin(math.radians(particle.theta))]
angles.append(v)
theta = sum_vectors(angles)
orientation_tuple = tf.transformations.quaternion_from_euler(0,0,theta)
self.robot_pose = Pose(position=Point(x=x,y=y),orientation=Quaternion(x=orientation_tuple[0], y=orientation_tuple[1], z=orientation_tuple[2], w=orientation_tuple[3]))
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
for particle in self.particle_cloud:
r1 = math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2]
d = math.sqrt((delta[0]**2) + (delta[1]**2))
particle.theta += r1 % 360
particle.x += d * math.cos(particle.theta) + normal(0,0.1)
particle.y += d * math.sin(particle.theta) + normal(0,0.1)
particle.theta += (delta[2] - r1 + normal(0,0.1)) % 360
# For added difficulty: Implement sample_motion_odometry (Prob Rob p 136)
def map_calc_range(self,x,y,theta):
""" Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """
pass
def resample_particles(self):
""" Resample the particles according to the new particle weights.
The weights stored with each particle should define the probability that a particular
particle is selected in the resampling step. You may want to make use of the given helper
function draw_random_sample.
"""
# make sure the distribution is normalized
self.normalize_particles()
newParticles = []
for i in range(len(self.particle_cloud)):
# resample the same # of particles
choice = random_sample()
# all the particle weights sum to 1
csum = 0 # cumulative sum
for particle in self.particle_cloud:
csum += particle.w
if csum >= choice:
# if the random choice fell within the particle's weight
newParticles.append(deepcopy(particle))
break
self.particle_cloud = newParticles
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
for particle in self.particle_cloud:
tot_prob = 0
for index, scan in enumerate(msg.ranges):
x,y = self.transform_scan(particle,scan,index)
# transform scan to view of the particle
d = self.occupancy_field.get_closest_obstacle_distance(x,y)
# calculate nearest distance to particle's scan (should be near 0 if it's on robot)
tot_prob += math.exp((-d**2)/(2*self.sigma**2))
# add probability (0 to 1) to total probability
tot_prob = tot_prob/len(msg.ranges)
# normalize total probability back to 0-1
particle.w = tot_prob
# assign particles weight
def transform_scan(self, particle, distance, theta):
""" Calculates the x and y of a scan from a given particle
particle: Particle object
distance: scan distance (from ranges)
theta: scan angle (range index)
"""
return (particle.x + distance * math.cos(math.radians(particle.theta + theta)),
particle.y + distance * math.sin(math.radians(particle.theta + theta)))
@staticmethod
def weighted_values(values, probabilities, size):
""" Return a random sample of size elements from the set values with the specified probabilities
values: the values to sample from (numpy.ndarray)
probabilities: the probability of selecting each element in values (numpy.ndarray)
size: the number of samples
"""
bins = np.add.accumulate(probabilities)
return values[np.digitize(random_sample(size), bins)]
@staticmethod
def draw_random_sample(choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(random_sample(n), bins)]
samples = []
for i in inds:
samples.append(deepcopy(choices[int(i)]))
return samples
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose)
self.initialize_particle_cloud(xy_theta)
self.fix_map_to_odom_transform(msg)
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
rad = 1 # meters
self.particle_cloud = []
self.particle_cloud.append(Particle(xy_theta[0], xy_theta[1], xy_theta[2]))
for i in range(self.n_particles-1):
# initial facing of the particle
theta = random.random() * 360
# compute params to generate x,y in a circle
other_theta = random.random() * 360
radius = random.random() * rad
# x => straight ahead
x = radius * math.sin(other_theta) + xy_theta[0]
y = radius * math.cos(other_theta) + xy_theta[1]
particle = Particle(x,y,theta)
self.particle_cloud.append(particle)
self.normalize_particles()
self.update_robot_pose()
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
tot_weight = sum([particle.w for particle in self.particle_cloud]) or 1
for particle in self.particle_cloud:
particle.w = particle.w/tot_weight;
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
marker_array = []
for index, particle in enumerate(self.particle_cloud):
marker = Marker(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
pose=particle.as_pose(),
type=0,
scale=Vector3(x=particle.w*2,y=particle.w*1,z=particle.w*5),
id=index,
color=ColorRGBA(r=1,a=1))
marker_array.append(marker)
self.marker_pub.publish(MarkerArray(markers=marker_array))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not(self.initialized):
# wait for initialization to complete
return
if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
p = PoseStamped(header=Header(stamp=rospy.Time(0),
frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame,p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not(self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose() # update robot's pose
self.resample_particles() # resample particles to focus on areas of high density
self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def fix_map_to_odom_transform(self, msg):
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer """
(translation, rotation) = convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation),
header=Header(stamp=msg.header.stamp,frame_id=self.base_frame))
self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
(self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map
to odom transformation. This is necessary so things like
move_base can work properly. """
if not(hasattr(self,'translation') and hasattr(self,'rotation')):
return
self.tf_broadcaster.sendTransform(self.translation,
self.rotation,
rospy.get_rostime(),
self.odom_frame,
self.map_frame)
class ParticleFilter:
""" The class that represents a Particle Filter ROS Node
Attributes list:
initialized: a Boolean flag to communicate to other class methods that initializaiton is complete
base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots)
map_frame: the name of the map coordinate frame (should be "map" in most cases)
odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases)
scan_topic: the name of the scan topic to listen to (should be "scan" in most cases)
start_particles: the number of particles first initalized
end_particles: the number of particles which end in the filter
middle_step: the step at which the number of particles has decayed about 50%
d_thresh: the amount of linear movement before triggering a filter update
a_thresh: the amount of angular movement before triggering a filter update
laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation
pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI)
particle_pub: a publisher for the particle cloud
laser_subscriber: listens for new scan data on topic self.scan_topic
tf_listener: listener for coordinate transforms
tf_broadcaster: broadcaster for coordinate transforms
particle_cloud: a list of particles representing a probability distribution over robot poses
current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed.
The pose is expressed as a list [x,y,theta] (where theta is the yaw)
map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid
"""
def __init__(self):
""" Define a new particle filter
"""
print("RUNNING")
self.initialized = False # make sure we don't perform updates before everything is setup
self.kidnap = False
rospy.init_node(
'pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.start_particles = 1000 # the number of particles to use
self.end_particles = 200
self.resample_count = 10
self.middle_step = 10
self.d_thresh = 0.2 # the amount of linear movement before performing an update
self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
# Setup pubs and subs
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
# publish weights for live graph node
self.weight_pub = rospy.Publisher("/graph_data",
Float64MultiArray,
queue_size=10)
# laser_subscriber listens for data from the lidar
rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.particle_cloud = []
# change use_projected_stable_scan to True to use point clouds instead of laser scans
self.use_projected_stable_scan = False
self.last_projected_stable_scan = None
if self.use_projected_stable_scan:
# subscriber to the odom point cloud
rospy.Subscriber("projected_stable_scan", PointCloud,
self.projected_scan_received)
self.current_odom_xy_theta = []
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
# publish the marker array
# self.viz = rospy.Publisher('/particle_marker', Marker, queue_size=10)
# self.marker = Marker()
self.viz = rospy.Publisher('/particle_marker',
MarkerArray,
queue_size=10)
self.markerArray = MarkerArray()
self.initialized = True
# assigns robot pose. used only a visual debugger, the real data comes from the bag file.
def update_robot_pose(self, timestamp):
#print("Guessing Robot Position")
self.normalize_particles(self.particle_cloud)
weights = [p.w for p in self.particle_cloud]
index_best = weights.index(max(weights))
best_particle = self.particle_cloud[index_best]
self.robot_pose = self.transform_helper.covert_xy_and_theta_to_pose(
best_particle.x, best_particle.y, best_particle.theta)
self.transform_helper.fix_map_to_odom_transform(
self.robot_pose, timestamp)
def projected_scan_received(self, msg):
self.last_projected_stable_scan = msg
# deadreckons particles with respect to robot motion.
def update_particles_with_odom(self, msg):
""" To apply the robot transformations to a particle, it can be broken down into a rotations, a linear movement, and another rotation (which could equal 0)
"""
#print("Deadreckoning")
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
delta_x = delta[0]
delta_y = delta[1]
delta_theta = delta[2]
direction = math.atan2(delta_y, delta_x)
theta_1 = self.transform_helper.angle_diff(
direction, self.current_odom_xy_theta[2])
for p in self.particle_cloud:
distance = math.sqrt((delta_x**2) +
(delta_y**2)) + np.random.normal(
0, 0.001)
dx = distance * np.cos(p.theta + theta_1)
dy = distance * np.sin(p.theta + theta_1)
p.x += dx
p.y += dy
p.theta += delta_theta + np.random.normal(0, 0.005)
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
def resample_particles(self):
""" Resample the particles according to the new particle weights.
The weights stored with each particle should define the probability that a particular
particle is selected in the resampling step. You may want to make use of the given helper
function draw_random_sample.
"""
# print("Resampling Particles")
# make sure the distribution is normalized
self.normalize_particles(self.particle_cloud)
particle_cloud_length = len(self.particle_cloud)
n_particles = ParticleFilter.sigmoid_function(self.resample_count,
self.start_particles,
self.end_particles,
self.middle_step, 1)
print("Number of Particles Reassigned: " + str(n_particles))
norm_weights = [p.w for p in self.particle_cloud]
# print("Weights: "+ str(norm_weights))
top_percent = 0.20
ordered_indexes = np.argsort(norm_weights)
ordered_particles = [
self.particle_cloud[index] for index in ordered_indexes
]
best_particles = ordered_particles[int(particle_cloud_length *
(1 - top_percent)):]
new_particles = ParticleFilter.draw_random_sample(
self.particle_cloud, norm_weights,
n_particles - int(particle_cloud_length * top_percent))
dist = 0.001 # adding a square meter of noise around each ideal particle
self.particle_cloud = []
self.particle_cloud += best_particles
for p in new_particles:
x_pos, y_pos, angle = p.x, p.y, p.theta
x_particle = np.random.normal(x_pos, dist)
y_particle = np.random.normal(y_pos, dist)
theta_particle = np.random.normal(angle, 0.05)
self.particle_cloud.append(
Particle(x_particle, y_particle, theta_particle))
self.normalize_particles(self.particle_cloud)
self.resample_count += 1
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
#transform laser data from particle's perspective to map coords
#print("Assigning Weights")
scan = msg.ranges
num_particles = len(self.particle_cloud)
num_scans = 361
step = 2
angles = np.arange(num_scans) # will be scan indices (0-361)
distances = np.array(scan) # will be scan values (scan)
angles_rad = np.deg2rad(angles)
for p in self.particle_cloud:
sin_values = np.sin(angles_rad + p.theta)
cos_values = np.cos(angles_rad + p.theta)
d_angles_sin = np.multiply(distances, sin_values)
d_angles_cos = np.multiply(distances, cos_values)
d_angles_sin = d_angles_sin[0:361:step]
d_angles_cos = d_angles_cos[0:361:step]
total_beam_x = np.add(p.x, d_angles_cos)
total_beam_y = np.add(p.y, d_angles_sin)
particle_distances = self.occupancy_field.get_closest_obstacle_distance(
total_beam_x, total_beam_y)
cleaned_particle_distances = [
2 * np.exp(-(dist**2)) for dist in particle_distances
if (math.isnan(dist) != True)
]
p_d_cubed = np.power(cleaned_particle_distances, 3)
p.w = np.sum(p_d_cubed)
self.normalize_particles(self.particle_cloud)
@staticmethod
def draw_random_sample(choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(random_sample(n), bins)]
samples = []
for i in inds:
samples.append(deepcopy(choices[int(i)]))
return samples
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
#print("Initial Pose Set")
xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
msg.pose.pose)
self.initialize_particle_cloud(msg.header.stamp, xy_theta)
def initialize_particle_cloud(self, timestamp, xy_theta=None):
"""
Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is omitted, the odometry will be used
Also check to see if we are attempting the robot kidnapping problem or are given an initial 2D pose
"""
if self.kidnap:
print("Kidnap Problem")
x_bound, y_bound = self.occupancy_field.get_obstacle_bounding_box()
x_particle = np.random.uniform(x_bound[0],
x_bound[1],
size=self.start_particles)
y_particle = np.random.uniform(y_bound[0],
y_bound[1],
size=self.start_particles)
theta_particle = np.deg2rad(
np.random.randint(0, 360, size=self.start_particles))
else:
print("Starting with Inital Position")
if xy_theta is None:
print("No Position Definied")
xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
x, y, theta = xy_theta
x_particle = np.random.normal(x, 0.25, size=self.start_particles)
y_particle = np.random.normal(y, 0.25, size=self.start_particles)
theta_particle = np.random.normal(theta,
0.001,
size=self.start_particles)
self.particle_cloud = [Particle(x_particle[i],\
y_particle[i],\
theta_particle[i]) \
for i in range(self.start_particles)]
def normalize_particles(self, particle_list):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
#print("Normalize Particles")
old_weights = [p.w for p in particle_list]
new_weights = []
for p in particle_list:
p.w = float(p.w) / sum(old_weights)
new_weights.append(p.w)
float_array = Float64MultiArray()
float_array.data = new_weights
self.weight_pub.publish(float_array)
def publish_particles(self, msg):
"""
Publishes particle poses on the map.
Uses Paul's default code at the moment, maybe later attempt to publish a visualization/MarkerArray
"""
particles_conv = []
for num, p in enumerate(self.particle_cloud):
particles_conv.append(p.as_pose())
self.particle_pub.publish(
PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
# self.marker_update("map", self.particle_cloud, False)
# self.viz.publish()
def scan_received(self, msg):
"""
All control flow happens here!
Special init case then goes into loop
"""
if not (self.initialized):
# wait for initialization to complete
return
# wait a little while to see if the transform becomes available. This fixes a race
# condition where the scan would arrive a little bit before the odom to base_link transform
# was updated.
# self.tf_listener.waitForTransform(self.base_frame, msg.header.frame_id, msg.header.stamp, rospy.Duration(0.5))
if not (self.tf_listener.canTransform(
self.base_frame, msg.header.frame_id, msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame,
msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative to the robot base
p = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
if not self.current_odom_xy_theta:
self.current_odom_xy_theta = new_odom_xy_theta
return
if not (self.particle_cloud):
print("Particle Cloud Empty")
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud(msg.header.stamp)
self.update_particles_with_laser(msg)
self.normalize_particles(self.particle_cloud)
self.update_robot_pose(msg.header.stamp)
self.resample_particles()
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) >
self.d_thresh
or math.fabs(new_odom_xy_theta[1] -
self.current_odom_xy_theta[1]) > self.d_thresh
or math.fabs(new_odom_xy_theta[2] -
self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
print("UPDATING PARTICLES")
self.update_particles_with_odom(msg) # update based on odometry
if self.last_projected_stable_scan:
last_projected_scan_timeshift = deepcopy(
self.last_projected_stable_scan)
last_projected_scan_timeshift.header.stamp = msg.header.stamp
self.scan_in_base_link = self.tf_listener.transformPointCloud(
"base_link", last_projected_scan_timeshift)
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose(msg.header.stamp) # update robot's pose
self.resample_particles(
) # resample particles to focus on areas of high density
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def marker_update(self, frame_id, p_cloud, delete):
num = 0
if (delete):
self.markerArray.markers = []
else:
for p in p_cloud:
marker = Marker()
marker.header.frame_id = frame_id
marker.header.stamp = rospy.Time.now()
marker.ns = "my_namespace"
marker.id = num
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.pose = p.as_pose()
marker.pose.position.z = 0.5
marker.scale.x = 1.0
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1.0 # Don't forget to set the alpha!
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
num += 1
self.markerArray.markers.append(marker)
@staticmethod
def sigmoid_function(value, max_output, min_output, middle, inc=1):
particle_difference = max_output - min_output
exponent = inc * (value - (middle / 2))
return int(particle_difference / (1 + np.exp(exponent)) + min_output)
class ParticleFilter(object):
"""
Particle Filter ROS Node.
"""
def __init__(self):
"""
Initialize node and necessary helper functions and p
"""
rospy.init_node('pf')
# Helper functions and debugging.
# Occupancy field used to get closest obstacle distance.
self.occupancy_field = OccupancyField()
# Helper functions for coordinate transformations and operations.
self.transform_helper = TFHelper()
# Set debug to true to print robot state information to the terminal.
self.debug = True
# Particle filter attributes.
# List of each particle in the filter.
self.particle_cloud = []
# Config attributes:
# n = Number of particles in the particle cloud.
# xy_spread_size: Scale factor for the spread of the x and y
# coordinates of the initial particle cloud.
# theta_spread_size: Scale factor for the spread of the angles
# in the initial particle cloud.
# xy_update_thresh: Change in x and y coordinates of the robot
# position (as determined by odometry data) at
# which to re-estimate robot position and
# resample the particle cloud.
# theta_update_thresh: Change (in degrees) of the robot's
# orientation (as determined by odometry data) at
# which to re-estimate robot position and
# resample the particle cloud.
self.particle_cloud_config = {
"n": 100,
"xy_spread_size": 1,
"theta_spread_size": 30,
"xy_update_thresh": 0.005,
"theta_update_thresh": 0.001
}
# The mininum weight of a particle, used to ensure non weights are NaN.
self.minimum_weight = 0.0000001
# Robot location attributes.
# Initial pose estimate, stored as a triple (x, y, theta).
# Used to create particle cloud.
self.xy_theta = None
# Pose estimate, stored as a pose message type.
# Used to track changes in pose and update pose markers.
self.current_pose_estimate = Pose()
# The overall change in the pose of the robot.
self.pose_delta = [0, 0, 0]
# Whether or not there is an initial pose value.
self.pose_set = False
# The frame of the robot base.
self.base_frame = "base_link"
# The name of the map coordinate frame.
self.map_frame = "map"
# The name of the odom coordinate frame.
self.odom_frame = "odom"
# The number of the most highly-weighted particles to incorporate
# in the mean value used to update the robot position estimate.
self.particles_to_incoporate_in_mean = 100
# Adjustment factor for the magnitude of noise added to the cloud
# during the resampling step.
self.noise_adjustment_factor = 0.001
# ROS Publishers/Subscribers
# Listen for new approximate initial robot location.
# Selected in rviz through the "2D Pose Estimate" button.
rospy.Subscriber("initialpose",
PoseWithCovarianceStamped,
self.initialize_pose_estimate)
# Get input data from laser scan.
rospy.Subscriber("scan", LaserScan, self.laser_scan_callback)
# Publish particle cloud for rviz.
self.particle_pub = rospy.Publisher("/particlecloud",
PoseArray,
queue_size=10)
def initialize_pose_estimate(self, msg):
"""
Initialize new pose estimate and particle cloud. Store the pose estimate as it as a
triple with the format (x, y, theta).
msg: PoseWithCovarianceStamped message received on the initialpose topic
after selection of the "2D Pose Estimate" button in rviz.
"""
if self.debug:
print("Got initial pose.")
self.xy_theta = \
self.transform_helper.convert_pose_to_xy_and_theta(msg.pose.pose)
self.create_particle_cloud(msg.header.stamp)
self.pose_set = True
def update_pose_estimate(self, timestamp):
"""
Update robot's pose estimate given the current particle cloud.
Calculate the pose estimate using the mean of the most highly weighted
particles, and then convert the mean coordinates and angle to a pose
stored in self.current_pose_estimate. Call fix_map_to_odom transform
to update the displayed current pose estimate.
timestamp: The timestamp of the current particle cloud in type time.
"""
self.normalize_particles()
mean_x = 0
mean_y = 0
mean_theta = 0
# Calculate the mean of the top se
particle_cloud_majority = sorted(self.particle_cloud, key=lambda x: x.w, reverse=True)
for particle in particle_cloud_majority[self.particles_to_incoporate_in_mean:]:
mean_x += particle.x * particle.w
mean_y += particle.y * particle.w
mean_theta = particle.theta * particle.w
mean_x /= self.particles_to_incoporate_in_mean
mean_y /= self.particles_to_incoporate_in_mean
mean_theta /= self.particles_to_incoporate_in_mean
# Use particle methods to convert particle to pose.
current_pose_particle = Particle(mean_x, mean_y, mean_theta)
self.current_pose_estimate = current_pose_particle.as_pose()
# Send out next map to odom transform with updated pose estimate.
self.transform_helper.fix_map_to_odom_transform(self.current_pose_estimate, timestamp)
def normalize_particles(self):
"""
Ensures particle weights sum to 1 by finding the sum of the particle
weights and then dividing each value by this sum.
Modifies self.particle_cloud directly.
"""
# Ensure that none of the particle weights are NaN.
self.set_minimum_weight()
total_w = sum(p.w for p in self.particle_cloud)
if total_w != 1.0:
for i in range(len(self.particle_cloud)):
self.particle_cloud[i].w /= total_w
def set_minimum_weight(self):
"""
Change any NaN weights in self.particle_cloud to the minimum weight
value instead.
Modifies self.particle_cloud directly.
"""
for i in range(len(self.particle_cloud)):
if math.isnan(self.particle_cloud[i].w):
self.particle_cloud[i].w = self.minimum_weight
def create_particle_cloud(self, timestamp):
"""
Generate a new particle cloud using the parameters stored in
self.particle_cloud_config, and then normalize the particles and
update the current pose estimate based on the state of the created
particle cloud.
timestamp: The timestamp of the current particle cloud in type time.
"""
if (self.debug):
print("Creating particle cloud.")
if self.xy_theta is None:
self.xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
# Sample particle values from normal distributions
x_spread = np.random.normal(
self.xy_theta[0],
self.particle_cloud_config["xy_spread_size"],
self.particle_cloud_config["n"])
y_spread = np.random.normal(
self.xy_theta[1],
self.particle_cloud_config["xy_spread_size"],
self.particle_cloud_config["n"])
theta_spread = np.random.normal(
self.xy_theta[2], np.deg2rad(self.particle_cloud_config["theta_spread_size"]), self.particle_cloud_config["n"])
self.particle_cloud = []
for (x, y, theta) in zip(x_spread, y_spread, theta_spread):
self.particle_cloud.append(Particle(x, y, theta, 1))
self.normalize_particles()
self.update_pose_estimate(timestamp)
def resample(self):
"""
Select new distribution of particles, weighted by each particle's
weight w. Then add some noise to the system to aid in visualization and
increase system robustness.
Modifies self.particle_cloud instance directly.
"""
self.set_minimum_weight()
if len(self.particle_cloud):
self.normalize_particles()
weights = [particle.w if not math.isnan(particle.w) else self.minimum_weight for particle in self.particle_cloud]
# Resample points based on their weights.
self.particle_cloud = [deepcopy(particle) for particle in list(np.random.choice(
self.particle_cloud,
size=len(self.particle_cloud),
replace=True,
p=weights,
))]
# Add noise to each particle.
for p in self.particle_cloud:
particle_noise = np.random.randn(3)
p.x += particle_noise[0] * self.noise_adjustment_factor
p.y += particle_noise[1] * self.noise_adjustment_factor
p.theta += particle_noise[2] * self.noise_adjustment_factor
if self.debug:
print("Resampling executed.")
def publish_particle_viz(self):
"""
Publish a visualization of self.particle_cloud for use in rviz.
"""
self.particle_pub.publish(
PoseArray(
header=Header(
stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=[
p.as_pose() for p in self.particle_cloud]))
if self.debug:
print("Publishing new visualization.")
def update_pose_delta(self, pose1, pose2):
"""
Calculate floating point distance between pose triples.
pose1: The first pose, stored as a triple (x, y, theta).
pose2: The second pose, stored as a triple (x, y, theta).
Returns a new triple with the difference between the poses in the
form of (x, y, theta) and also updates self.pose_delta.
"""
self.pose_delta[0] = pose2[0] - pose1[0]
self.pose_delta[1] = pose2[1] - pose1[1]
self.pose_delta[2] = self.transform_helper.angle_diff(pose2[2], pose1[2])
return self.pose_delta
def update_thresholds_met(self, msg):
"""
Calculate the estimated laser scan and robot pose, and then update the
current pose. Return if the difference between the previous and current
pose exceeds a given threshold.
msg: Incoming laser scan data of message type LaserScan.
Returns a boolean indicating whether the change in the robot's position
exceeds the given movement threshold.
"""
# Calculate pose of laser relative to the robot base.
p = PoseStamped(header=Header(stamp=rospy.Time(0),
frame_id=msg.header.frame_id))
self.laser_pose = self.transform_helper.tf_listener.transformPose(self.base_frame, p)
# Find out where the robot thinks it is based on its odometry.
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.transform_helper.tf_listener.transformPose(self.odom_frame, p)
# Store the the odometry pose into (x,y,theta).
current_pose = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
# If the old pose has not definition, the robot needs to move more.
if not hasattr(self, "old_pose"):
self.old_pose = current_pose
return False
# Otherwise, update self.pose_delta and return whether its magnitude
# exceeds the threshold.
x_d, y_d, theta_d = self.update_pose_delta(self.old_pose, current_pose)
self.old_pose = current_pose
return math.fabs(x_d) > self.particle_cloud_config["xy_update_thresh"] or \
math.fabs(y_d) > self.particle_cloud_config["xy_update_thresh"] or \
math.fabs(theta_d) > self.particle_cloud_config["theta_update_thresh"]
def odom_update(self):
"""
Use self.pose_delta to update particle locations to reflect the change
in the robot's position.
Modifies self.particle_cloud directly.
"""
x_d, y_d, theta_d = self.pose_delta
for i in range(len(self.particle_cloud)):
self.particle_cloud[i].x -= x_d
self.particle_cloud[i].y -= y_d
self.particle_cloud[i].theta += theta_d
def laser_update(self, msg):
"""
Use scan data in msg to update particle weights by using the occupancy
field to determine the distance from the closest obstacle and
adjusting the particle weights accordingly.
msg: Incoming laser scan data of message type LaserScan.
Modifies self.particle_cloud in place.
"""
for particle in self.particle_cloud:
# Get total distances for each angle for each particle.
total_distance = 0
x_values = msg.ranges * np.cos(np.degrees(np.arange(0, 361, dtype=float) + particle.theta))
y_values = msg.ranges * np.sin(np.degrees(np.arange(0, 361, dtype=float) + particle.theta))
for x, y in zip(x_values, y_values):
# Disregard any invalid distances.
if x == 0 and y == 0:
continue
o_d = self.occupancy_field.get_closest_obstacle_distance(particle.x + x, particle.y + y)
if not(math.isnan(o_d)) and o_d != 0:
# Cube the distance to weight closer objects more highly.
total_distance += o_d**3
# Set particle weight to the inverse of the total distance.
if total_distance > 0:
particle.w = 1.0/total_distance
# Ensure that particle weights sum to 1.
self.normalize_particles()
def laser_scan_callback(self, msg):
"""
Process incoming laser scan data. If the change in position exceeds
the thresholds, update the pose estimate and resample.
msg: Incoming laser scan data of message type LaserScan.
"""
if not self.pose_set:
return
self.transform_helper.tf_listener.waitForTransform(self.base_frame, msg.header.frame_id, msg.header.stamp, rospy.Duration(0.5))
if not(self.transform_helper.tf_listener.canTransform(self.base_frame, msg.header.frame_id, msg.header.stamp)) or \
not(self.transform_helper.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)):
return
# Regardless of update, publish particle cloud visualization.
self.publish_particle_viz()
if self.update_thresholds_met(msg):
self.odom_update()
self.laser_update(msg)
# Update the self.current_pose_estimate with the mean particle and resample.
self.update_pose_estimate(msg.header.stamp)
self.resample()
else:
if self.debug:
print("Update thresholds not met!")
def run(self):
"""
As most of the processing happens through callback functions for laser
scan and odometry data, simply publish the most recent transform for as
long as the node runs.
"""
r = rospy.Rate(2)
while not(rospy.is_shutdown()):
# in the main loop all we do is continuously broadcast the latest
# map to odom transform
self.transform_helper.send_last_map_to_odom_transform()
r.sleep()
