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):
"""
The class that represents a Particle Filter ROS Node
"""
def __init__(self):
rospy.init_node('pf')
# create instances of two helper objects that are provided to you
# as part of the project
self.occupancy_field = OccupancyField()
self.tfh = TFHelper()
self.ros_boss = RosBoss()
# publisher for the particle cloud for visualizing in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("/odom", Odometry, self.update_initial_pose)
self.current_particles = {}
self.max_particle_number = 100
self.map_width = self.occupancy_field.map.info.width
self.map_height = self.occupancy_field.map.info.height
self.map_origin = (self.occupancy_field.map.info.origin.position.x,
self.occupancy_field.map.info.origin.position.y)
self.map_resolution = self.occupancy_field.map.info.resolution
self.particle_viz = ParticlesMarker()
self.last_time = time.time()
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 """
self.tfh.fix_map_to_odom_transform(
msg.pose.pose,
msg.header.stamp) # Update transform between map and odom.
if self.ros_boss.poseGiven():
xy_theta = self.tfh.convert_pose_to_xy_and_theta(
self.ros_boss.stamped_pose
) # Get position and orientation from pose
if len(self.current_particles
) == 0: #Initial generation of particles
self.generate_initial_particles(xy_theta)
else:
# self.current_particles = {(Particle : probability}
threshold = self.thresholdWeight(
) # Calculate threshold weight
self.sample_best_particles(
threshold
) # self.current_particles = selected best particles.
self.generate_probability_dist(
) # "Refills" any missing particles by placing them around the most likely current particles.
for particle in self.current_particles.keys():
self.calc_new_weights(
particle
) # Calculates new probabilities for all particles (Evaluation)
timesince = time.time() - self.last_time
self.estimate_future_particles(
particle, timesince) # Move particles forward in time
self.last_time = time.time()
self.normalize_weights(
particle, max(self.current_particles.values()))
# Update RViz map and particles
self.particle_viz.updateParticles(
self.current_particles, self.map_origin,
self.map_resolution) # Update rviz visualization
def generate_initial_particles(self, xy_theta):
""" Generate a number of initial particles and add them to the dict. x & y are in meters."""
raw_x, raw_y, theta = xy_theta
timenow = rospy.get_rostime()
# self.transform_helper.tf_listener.transformPoint("odom", Point, base_point);
# + self.transform_helper.translation.y
for num in range(self.max_particle_number):
# Generate random x,y, theta coordinate for particle, and add as key-value pair to dict of particles
[x, y] = np.random.multivariate_normal([raw_x, raw_y],
[[0.0, 0.0], [0.0, 0.0]])
theta = np.random.uniform(0.0, 2 * math.pi)
# pose = self.tfh.transform(Pose(Point(raw_x, raw_y, 0.0), self.tfh.euler_to_quat(raw_theta)))
# x, y, theta = self.tfh.convert_pose_to_xy_and_theta(Pose(pose.pose.position, pose.pose.orientation))
self.current_particles[Particle(
x, y, theta)] = (1.0 / self.max_particle_number)
self.particle_viz.bestParticle = (list(
self.current_particles.items()))[0]
return
def sample_best_particles(self, threshold):
""" Iterates through list of particles and recycles the ones below the threshold. """
# Two step process to avoid changing the dict while iterating through
best_particles = {
particle: probability
for particle, probability in self.current_particles.iteritems()
if self.current_particles[particle] <= threshold
}
self.current_particles = best_particles
if len(self.current_particles) == 0:
return None
return
def thresholdWeight(self):
""" Picks the threshold value below which particles will be recycled. """
return sum(self.current_particles.values()) / len(
self.current_particles.values()) # Average value
def generate_probability_dist(self):
""" Generates more particles around the existing most likely particles. """
if len(self.current_particles.values()) < self.max_particle_number:
num_created_particles = self.max_particle_number - len(
self.current_particles.values())
# Iterate through sorted list of particles to get list of best options
topProbabilities = dict(
sorted(self.current_particles.iteritems(),
key=lambda (particle, probability):
(particle, probability),
reverse=True)[:(len(self.current_particles) %
num_created_particles)])
if len(topProbabilities) > 0:
# Get best particle (first in sorted dict)
best_particle, probability = (list(
topProbabilities.items()))[0]
# Set value in particle_viz
self.particle_viz.bestParticlePose = Pose(
Point(best_particle.x, best_particle.y, 0.0),
self.tfh.euler_to_quat(best_particle.theta))
# Create new particles.
preload = math.floor(
len(self.current_particles) / num_created_particles)
new_particles = {}
index = 0
for particle in self.current_particles.keys():
# if num desired particles is larger than current list of particles, we must create triplicates, etc.
for num in range(int(preload)):
new_particles[Particle(
particle.x, particle.y, particle.theta,
particle.distance)] = self.current_particles[particle]
# These are the ones that get an extra first, if the number of new particles is not evenly divisible.
if (index <=
(len(self.current_particles) % num_created_particles)):
new_particles[Particle(
particle.x, particle.y, particle.theta,
particle.distance)] = self.current_particles[particle]
index += 1
# Add new dict entries to self.current_particles
self.current_particles.update(new_particles)
return
def calc_new_weights(self, particle):
""" Iterates through list of particles and calculates new weights. """
angle_probability = self.tfh.angle_diff(self.ros_boss.minAngle,
particle.theta) / (2 * math.pi)
distance_probability = abs(
self.ros_boss.minDistance - particle.distance
) / self.ros_boss.minDistance # Shortest laser scan - shortest laser scan for current particle.
self.current_particles[
particle] = angle_probability + distance_probability
return
def estimate_future_particles(self, particle, timesince):
""" Estimate future particle locations given noise and current speed. Particles are in the map frame. """
particle.distance = math.sqrt(
(self.ros_boss.linear_vel.x * timesince)**2 +
(self.ros_boss.linear_vel.y * timesince)**2) + np.random.uniform(
-0.1, 0.1)
particle.x = particle.x + self.ros_boss.linear_vel.x * timesince + np.random.uniform(
-0.1, 0.1) # DeltaX = Velx * time + noise
particle.y = particle.y + self.ros_boss.linear_vel.y * timesince + np.random.uniform(
-0.1, 0.1)
return
def normalize_weights(self, particle, max_val):
""" Normalizes all weights to be between 0 and 1. """
self.current_particles[
particle] = self.current_particles[particle] / max_val
return
def run(self):
r = rospy.Rate(5)
while not (rospy.is_shutdown()):
# in the main loop all we do is continuously broadcast the latest
# map to odom transform
self.tfh.send_last_map_to_odom_transform()
r.sleep()
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 PropagationTest(object):
def __init__(self):
rospy.init_node("propagation_test_node")
self.tf_helper = TFHelper()
self.base_link_pose = PoseStamped()
self.base_link_pose.header.frame_id = "base_link"
self.base_link_pose.header.stamp = rospy.Time(0)
self.last_odom_pose = PoseStamped()
self.last_odom_pose.header.frame_id = "odom"
self.last_odom_pose.header.stamp = rospy.Time(0)
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.pose_array = PoseArray()
self.pose_array.header.stamp = rospy.Time(0)
self.pose_array.header.frame_id = "odom"
self.p = Particle(x=0, y=0, theta=180, weight=0)
self.p_array = PoseArray()
self.p_array.header.stamp = rospy.Time(0)
self.p_array.header.frame_id = "map"
self.is_first = True
def run(self):
r = rospy.Rate(1)
while not rospy.is_shutdown():
try:
(trans, rot) = self.tf_helper.tf_listener.lookupTransform(
'/odom', '/base_link', rospy.Time(0))
# print("Transform: Trans: {} \n Rot: {}".format(trans, rot))
# Get odom change.
new_odom_pose = self.tf_helper.tf_listener.transformPose(
'odom', self.base_link_pose)
self.pose_array.poses.append(new_odom_pose.pose)
self.odom_pose_pub.publish(self.pose_array)
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)))
if not self.is_first:
self.propagate(x_change, y_change, angular_change)
self.last_odom_pose = new_odom_pose
self.is_first = False
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):
# Update the last odom pose in the process
r = sqrt(dx**2 + dy**2)
angle = radians(
self.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))
self.p.x += r * cos(angle)
self.p.y += r * sin(angle)
self.p.theta = (self.p.theta + degrees(dtheta)) % 360 # Wrap angle
self.p_array.poses.append(self.p.get_pose())
self.particle_pose_pub.publish(self.p_array)
class RobotLocalizer(object):
"""
doc
"""
def __init__(self):
print("init RobotLocalizer")
rospy.init_node('localizer')
self.tfHelper = TFHelper()
self.xs = None
self.ys = None
self.ranges = [] # Lidar scan
self.last_odom_msg = None
print(self.last_odom_msg)
self.diff_transform = {
'translation': None,
'rotation': None,
}
self.odom_changed = False # Toggles to True when the odom frame has changed enough
# subscribers and publisher
self.laser_sub = rospy.Subscriber('/scan', LaserScan,
self.process_scan)
self.odom_sub = rospy.Subscriber("/odom", Odometry, self.update_odom)
### Used for the particle filter
# publisher for the particle cloud for visualizing in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
# publisher for the top weighted particle
self.topparticle_pub = rospy.Publisher("topparticle",
PoseArray,
queue_size=10)
# publisher for rviz markers
self.pub_markers = rospy.Publisher('/visualization_markerarray',
MarkerArray,
queue_size=10)
self.particle_filter = ParticleFilter(self.topparticle_pub,
self.particle_pub,
self.pub_markers)
print("ParticleFilter initialized")
print("RobotLocalizer initialized")
def update_odom(self, msg):
MIN_TRAVEL_DISANCE = 0.1
MIN_TRAVEL_ANGLE = math.radians(5)
if self.last_odom_msg is None:
self.last_odom_msg = msg
return
last_xyt = self.tfHelper.convert_pose_to_xy_and_theta(
self.last_odom_msg.pose.pose)
current_xyt = self.tfHelper.convert_pose_to_xy_and_theta(msg.pose.pose)
# Get translation in odom
translation = [
current_xyt[0] - last_xyt[0], # x
current_xyt[1] - last_xyt[1], # y
]
# rotate to vehicle frame
translation = self.tfHelper.rotate_2d_vector(
translation,
-1 * last_xyt[2]) # negative angle to counter rotation
# get orientation diff
theta = self.tfHelper.angle_diff(current_xyt[2], last_xyt[2])
# Schedule to update particle filter if there's enough change
distance_travelled = math.sqrt(translation[0]**2 + translation[1]**2)
if distance_travelled > MIN_TRAVEL_DISANCE or theta > MIN_TRAVEL_ANGLE:
# TODO(matt): consider using actual transform
# last_to_current_transform = self.tfHelper.convert_translation_rotation_to_pose(
# translation, self.tfHelper.convert_theta_to_quaternion(theta)
# )
print("travelled: {}".format(distance_travelled))
last_to_current_transform = {
'translation': translation,
'rotation': theta,
}
print("transform\n x: {}\n y: {}".format(
last_to_current_transform['translation'][0],
last_to_current_transform['translation'][1]))
self.diff_transform = last_to_current_transform
self.last_odom_msg = msg
self.odom_changed = True
def process_scan(self, m):
"""Storing lidar data
"""
#TODO:
self.ranges = m.ranges
xs = []
ys = []
for i in range(len(self.ranges)):
if self.ranges[i] != 0:
theta = math.radians(i)
r = self.ranges[i]
xf = math.cos(theta) * r
yf = math.sin(theta) * r
xs.append(xf)
ys.append(yf)
self.xs = xs
self.ys = ys
def get_x_directions(self, x):
interval = 360 / x
angle = 0
directions = []
for i in range(x):
dist = self.ranges[angle]
directions.append((math.radians(angle), dist))
angle = angle + interval
return directions
def gen_neighbor_particles(self):
"""Generates particles around given points"""
#TODO:
pass
def find_all_nearest_objects(self):
"""Determines nearest non-zero point of all point (loops through)"""
#TODO:
pass
def get_encoder_value(self):
"""Records odom movement, translate to x, y, and theta"""
#TODO:
pass
"""
Functions to write or figure out where they are:
Order of particle filter:
1. DONE generate initial 500 random particles
2. DONE get ranges from robot
-determine 8 values for directions
-find lowest distance to obstacle
3. Process particles
- project lowest distance from robot onto each particle
-DONE for each particle get nearest object -> error distance
-DONE 1/error distance = particle.weight
4. DONE publish particle with highest weight
5. DONE resample particles based on weight
6. DONE move robot - get transform
7. DONE transform resampled points with randomness
"""
def run(self):
# save odom position (Odom or TF Module)
# self.generate_random_points()
NUM_DIRECTIONS = 8
self.particle_filter.gen_init_particles()
# # Get lidar readings in x directions
# robo_pts = self.get_x_directions(NUM_DIRECTIONS)
# # For each particle compare lidar scan with map
# self.particle_filter.compare_points(robo_pts)
#
# # Publish best guessself.particle_filter.gen_init_particles()
# self.particle_filter.publish_top_particle(self.topparticle_pub)
#
# # Resample particles
# self.particle_filter.resample_particles()
#
# # Publish cloud
# self.particle_filter.publish_particle_cloud(self.particle_pub)
r = rospy.Rate(5)
while not (rospy.is_shutdown()):
self.particle_filter.gauge_particle_position()
if (self.odom_changed):
print("Odom changed, let's do some stuff")
# Get lidar readings in x directions
robo_pts = self.get_x_directions(NUM_DIRECTIONS)
# Update particle poses
self.particle_filter.update_all_particles(self.diff_transform)
# Display new markers
self.particle_filter.draw_markerArray()
# For each particle compare lidar scan with map
self.particle_filter.compare_points(robo_pts)
# Publish best guess self.particle_filter.gen_init_particles()
top_particle_pose = self.particle_filter.publish_top_particle()
self.tfHelper.fix_map_to_odom_transform(
top_particle_pose, rospy.Time(0)) # TODO: Move?
# Resample particles
self.particle_filter.resample_particles()
# Publish cloud
self.particle_filter.publish_particle_cloud()
# Wait until robot moves enough again
self.odom_changed = False
self.tfHelper.send_last_map_to_odom_transform()
r.sleep()
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(object):
""" The class that represents a Particle Filter ROS Node
"""
def __init__(self):
rospy.init_node('pf')
self.particle_publisher = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.particle_manager = ParticleManager()
self.sensor_manager = SensorManager()
self.particle_manager.init_particles(self.occupancy_field)
self.scanDistance = 0.2
self.scanAngle = 0.5
self.moved = (0, 0)
def run(self):
r = rospy.Rate(5)
while not (rospy.is_shutdown()):
#Create a pose array with all of the current particles for easy viewing in Rviz
poseArray = PoseArray(header=Header(
seq=10, stamp=rospy.get_rostime(), frame_id='map'))
for particle in self.particle_manager.particles:
poseArray.poses.append(
self.transform_helper.convert_xy_and_theta_to_pose(
particle.x, particle.y, particle.theta))
#Publish the pose array of the current particles
self.particle_publisher.publish(poseArray)
#If we are not already looking for a new laser scan but have moved a moderate distance, set the flag to look for a new laser scan
if not self.sensor_manager.newLaserScan and (
(getDistance(self.sensor_manager.lastScan[0],
self.sensor_manager.lastScan[1],
self.sensor_manager.pose[0],
self.sensor_manager.pose[1]) > self.scanDistance)
or self.transform_helper.angle_diff(
math.radians(self.sensor_manager.pose[2]),
math.radians(self.sensor_manager.lastScan[2])) >
self.scanAngle):
#Set the flag to look for a new laser scan and mark where we last took laser scan data so we know how far we can move before taking another
self.sensor_manager.newLaserScan = True
self.moved = (getDistance(self.sensor_manager.lastScan[0],
self.sensor_manager.lastScan[1],
self.sensor_manager.pose[0],
self.sensor_manager.pose[1]),
math.degrees(
self.transform_helper.angle_diff(
self.sensor_manager.pose[2],
self.sensor_manager.lastScan[2])))
#Here, we are currently looking for a new laser scan and haven't moved sufficient distance to warrant a new one
elif self.sensor_manager.newLaserScan:
#While we are looking for a new laser scan, wait for the data to come in so that we don't process the particles on old data
while self.sensor_manager.newLaserScan:
continue
#Tranform all particles to match the transform the robot has done since the last laser scan and particle trimming
self.particle_manager.transform_particles(
self.moved[0], self.moved[1])
#Update the particle probabilities so we know how likely each particle is based on current laser scan data
self.particle_manager.update_probabilities(
self.sensor_manager.minRange, self.occupancy_field,
self.sensor_manager.closestAngles)
#Do a random weighted selection of all of the current particles to choose particles to be passed on to the next round of laser scan data
self.particle_manager.trim_particles()
#Generate new particles around the kept particles with some added noise
self.particle_manager.generate_particles()
#Create and publish the pose array with the new particles for viewing in Rviz
poseArray = PoseArray(header=Header(
seq=10, stamp=rospy.get_rostime(), frame_id='map'))
for particle in self.particle_manager.particles:
poseArray.poses.append(
self.transform_helper.convert_xy_and_theta_to_pose(
particle.x, particle.y, particle.theta))
self.particle_publisher.publish(poseArray)
self.transform_helper.send_last_map_to_odom_transform()
class NeatoMDP(object):
def __init__(self, num_positions=500, num_orientations=10):
# TODO: Interface with SLAM algorithm's published map
# Initialize map.
rospy.init_node(
"neato_mdp") # May break if markov_model is also subscribed...?
rospy.wait_for_service("static_map")
static_map = rospy.ServiceProxy("static_map", GetMap)
# Initialize MDP
self.mdp = MDP(num_positions=num_positions,
num_orientations=num_orientations,
map=static_map().map)
self.state_idx = None # Current state idx is unknown.
self.curr_odom_pose = Pose()
self.tf_helper = TFHelper()
# Velocity publisher
self.cmd_vel_publisher = rospy.Publisher("/cmd_vel",
Twist,
queue_size=10,
latch=True)
self.odom_subscriber = rospy.Subscriber('/odom', Odometry,
self.set_odom)
self.goal_state = None
# Visualize robot
self.robot_state_pub = rospy.Publisher('/robot_state_marker',
Marker,
queue_size=10)
self.robot_state_pose_pub = rospy.Publisher('/robot_state_pose',
PoseArray,
queue_size=10)
self.goal_state_pub = rospy.Publisher('/goal_state_marker',
Marker,
queue_size=10)
# # pose_listener responds to selection of a new approximate robot location (for instance using rviz)
#
self.odom_pose = PoseStamped()
self.odom_pose.header.stamp = rospy.Time(0)
self.odom_pose.header.frame_id = 'odom'
#
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
rospy.Subscriber("move_base_simple/goal", PoseStamped,
self.update_goal_state)
def set_odom(self, msg):
self.curr_odom_pose = msg.pose.pose
def set_goal_idxs(self):
idxs = []
for i, reward in enumerate(self.mdp.rewards):
if reward > 0:
idxs.append(i)
return idxs
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("Got an initial pose estimate...?")
self.tf_helper.fix_map_to_odom_transform(msg.pose.pose,
msg.header.stamp)
self.tf_helper.send_last_map_to_odom_transform()
# Wait for the transform to really get sent. Otherwise, it complains that it does not exist.
rospy.sleep(2.)
map_pose = self.tf_helper.tf_listener.transformPose(
'map', self.odom_pose)
x, y, theta = self.tf_helper.convert_pose_to_xy_and_theta(
map_pose.pose)
self.state_idx = self.mdp.markov_model.get_closest_state_idx(
State(x=x, y=y, theta=theta))
def update_goal_state(self, msg):
map_pose = self.tf_helper.tf_listener.transformPose('map', msg)
x, y, theta = self.tf_helper.convert_pose_to_xy_and_theta(
map_pose.pose)
theta = theta % (2 * math.pi)
self.goal_state = State(x=x, y=y, theta=theta)
self.goal_state_pub.publish(self.goal_state.get_marker())
def execute_action(self, policy):
# Identify which state you are in
#TODO: Interface with SLAM algorithm to get predicted position
action = Action.get_all_actions()[policy[self.state_idx]]
print("Taking action: ", Action.to_str(action))
new_state = self.move(action)
# print(new_state)
# Update state idx.
self.state_idx = self.mdp.markov_model.get_closest_state_idx(
new_state, start_state_idx=self.state_idx)
def move(self, action):
linear, angular = Action.get_pose_change(action)
print("linear: {}, angular: {}".format(linear, angular))
twist_msg = Twist()
twist_msg.linear.x = 0.25 * linear
twist_msg.angular.z = 0.3 * angular
# publish this twist message
self.cmd_vel_publisher.publish(twist_msg)
start_odom_x, start_odom_y, start_odom_theta = self.tf_helper.convert_pose_to_xy_and_theta(
self.curr_odom_pose)
start_odom_theta = start_odom_theta % (2 * math.pi)
linear_change, angular_change = self.get_change_in_motion(
start_odom_x, start_odom_y, start_odom_theta)
r = rospy.Rate(1)
while (action == Action.FORWARD and abs(linear_change) < abs(linear)
) or (action != Action.FORWARD
and abs(angular_change) < abs(angular)):
map_pose = self.tf_helper.tf_listener.transformPose(
'map', self.odom_pose)
x, y, theta = self.tf_helper.convert_pose_to_xy_and_theta(
map_pose.pose)
theta = theta % (2 * math.pi)
new_state = State(x=x, y=y, theta=theta)
self.publish_robot_odom(new_state)
r.sleep() # Wait a little.
# Check change again.
linear_change, angular_change = self.get_change_in_motion(
start_odom_x, start_odom_y, start_odom_theta)
print("Lin change: {} Ang change: {}".format(
linear_change, angular_change))
# Convert odom state to map frame.
self.odom_pose.pose = self.curr_odom_pose
# self.odom_pose.header.stamp = rospy.Time.now()
map_pose = self.tf_helper.tf_listener.transformPose(
'map', self.odom_pose)
x, y, theta = self.tf_helper.convert_pose_to_xy_and_theta(
map_pose.pose)
theta = theta % (2 * math.pi)
new_state = State(x=x, y=y, theta=theta)
self.publish_robot_odom(new_state)
print("New state is: ")
print(new_state)
return new_state
def stop(self):
twist_msg = Twist()
self.cmd_vel_publisher.publish(twist_msg)
def publish_robot_odom(self, curr_state):
self.robot_state_pub.publish(
curr_state.get_marker(r=0.0, g=0.0, b=1.0, scale=0.15))
robot_pose = PoseArray()
robot_pose.header.frame_id = "map"
robot_pose.poses = [curr_state.get_pose()]
self.robot_state_pose_pub.publish(robot_pose)
def get_change_in_motion(self, start_odom_x, start_odom_y,
start_odom_theta):
new_odom_x, new_odom_y, new_odom_theta = self.tf_helper.convert_pose_to_xy_and_theta(
self.curr_odom_pose)
new_odom_theta = new_odom_theta % (2 * math.pi)
return (new_odom_x - start_odom_x,
self.tf_helper.angle_diff(new_odom_theta, start_odom_theta))
def run(self):
# TODO: Parametrize goal state, and be able to dynamically update it?
# Set an initial goal state.
# goal_state = State(x=1, y=1, theta=math.radians(40))
# Solve the MDP
while self.goal_state == None:
continue
policy, iter, time = self.mdp.get_policy(self.goal_state)
self.mdp.visualize_policy(policy, self.goal_state)
goal_idxs = self.set_goal_idxs()
print(goal_idxs)
r = rospy.Rate(0.5)
while not rospy.is_shutdown():
if (self.state_idx == None):
# print("Current state is unknown. Cannot execute policy.")
continue
else:
# Keep checking whether you are in a different state and should
# execute a different action.
if self.state_idx in goal_idxs:
self.stop()
print(
"Finished executing policy. Would you like to go again?"
)
if (raw_input() in ['n', 'NO', 'N', 'no']):
print("Exiting")
break
else:
print("Enter goal state: ")
goal_state_idx = input()
self.goal_state = self.mdp.markov_model.model_states[
goal_state_idx]
# Visualize the goal state as sphere.
self.goal_state_pub.publish(
self.mdp.markov_model.model_states[goal_state_idx].
get_marker())
# Solve the MDP
policy, iter, time = self.mdp.get_policy(
self.goal_state)
goal_idxs = self.set_goal_idxs()
else:
self.execute_action(policy)
class ParticleFilterNode(object):
""" The class that represents a Particle Filter ROS Node
"""
def __init__(self):
rospy.init_node('pf')
real_robot = False
# create instances of two helper objects that are provided to you
# as part of the project
self.particle_filter = ParticleFilter()
self.occupancy_field = OccupancyField()
self.TFHelper = TFHelper()
self.sensor_model = sensor_model = SensorModel(
model_noise_rate=0.5,
odometry_noise_rate=0.15,
world_model=self.occupancy_field,
TFHelper=self.TFHelper)
self.position_delta = None # Pose, delta from current to previous odometry reading
self.last_scan = None # list of ranges
self.odom = None # Pose, current odometry reading
self.x_y_spread = 0.3 # Spread constant for x-y initialization of particles
self.z_spread = 0.2 # Spread constant for z initialization of particles
self.n_particles = 150 # number of particles
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
rospy.Subscriber("odom", Odometry, self.update_position)
rospy.Subscriber("stable_scan", LaserScan, self.update_scan)
# publisher for the particle cloud for visualizing in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
def update_scan(self, msg):
"""Updates the scan to the most recent reading"""
self.last_scan = [(i, msg.ranges[i]) for i in range(len(msg.ranges))]
def update_position(self, msg):
"""Calculate delta in position since last odometry reading, update current odometry reading"""
self.position_delta = Vector3()
this_pos = self.TFHelper.convert_pose_to_xy_and_theta(msg.pose.pose)
if self.odom is not None:
prev_pos = self.TFHelper.convert_pose_to_xy_and_theta(self.odom)
self.position_delta.x = this_pos.x - prev_pos.x
self.position_delta.y = this_pos.y - prev_pos.y
self.position_delta.z = self.TFHelper.angle_diff(
this_pos.z, prev_pos.z)
else:
self.position_delta = this_pos
self.odom = msg.pose.pose
self.particle_filter.predict(self.position_delta)
def reinitialize_particles(self, initial_pose):
"""Reinitialize particles when a new initial pose is given."""
self.particle_filter.particles = []
for i in range(self.n_particles):
pos = Vector3()
initial_pose_trans = self.TFHelper.convert_pose_to_xy_and_theta(
initial_pose)
pos.x = initial_pose_trans.x + (2 * randn() - 1) * self.x_y_spread
pos.y = initial_pose_trans.y + (2 * randn() - 1) * self.x_y_spread
pos.z = initial_pose_trans.z + (2 * randn() - 1) * self.z_spread
new_particle = Particle(position=pos,
weight=1 / float(self.n_particles),
sensor_model=self.sensor_model)
self.particle_filter.add_particle(new_particle)
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 """
self.reinitialize_particles(msg.pose.pose)
self.TFHelper.fix_map_to_odom_transform(msg.pose.pose,
msg.header.stamp)
def publish_particles(self):
""" Extract position from each particle, transform into pose, and publish them as PoseArray"""
pose_array = PoseArray()
for p in self.particle_filter.particles:
pose_array.poses.append(
self.TFHelper.convert_vector3_to_pose(p.position))
pose_array.header.frame_id = "map"
self.particle_pub.publish(pose_array)
def run(self):
r = rospy.Rate(5)
while not (rospy.is_shutdown()):
# in the main loop all we do is continuously broadcast the latest
# map to odom transform
self.TFHelper.send_last_map_to_odom_transform()
if len(self.particle_filter.particles) > 0:
if self.last_scan != None:
self.particle_filter.integrate_observation(self.last_scan)
self.last_scan = None
self.particle_filter.normalize()
self.publish_particles()
self.particle_filter.resample()
r.sleep()
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()