class TfNode:
def __init__(self, *args):
self.tf = TransformListener()
self.sub = rospy.Subscriber(
"/detectedObjs_primitive",
thesis_visualization.msg.objectLocalization,
self.__subCb,
queue_size=1)
self.msg = thesis_visualization.msg.objectLocalization()
self.thread_sub = Thread(target=self.start_sub)
def __subCb(self, msg):
self.msg = msg
print self.msg.modelList
def start_sub(self):
rospy.spin()
def subscribe(self):
self.thread_sub.start()
def unregister_sub(self):
self.sub.unregister()
rospy.signal_shutdown("close subcriber")
def example_function(self):
if self.tf.frameExists("/world") and self.tf.frameExists(
"/kinect2_depth"):
t = self.tf.getLatestCommonTime("/world", "/kinect2_depth")
p1 = geometry_msgs.msg.PoseStamped()
p1.header.frame_id = "kinect2_depth"
p1.pose.orientation.w = 1.0 # Neutral orientation
p_in_base = self.tf.transformPose("/world", p1_)
print "Position of the fingertip in the robot base:"
print p_in_base
def pcd2worldFrame(self, pcd):
self.tf.transformPointCloud("world", pcd)
class Trajectory_generator(object):
# Generate the object's trajectory from the central point of the detected box, then project
def __init__(self, node_name="pointcloud_reproject"):
self.pointcloud = PointCloud()
self.pointcloud2 = PointCloud()
self.node_name = node_name
#self.listener = tf.TransformListener()
rospy.init_node(self.node_name)
self.tf = TransformListener()
#self.tf = TransformerROS()
self.pointcloud_sub = rospy.Subscriber("/output_cloud_husky2",
PointCloud,
self.Pointcloud_callback)
self.pointcloud_pub = rospy.Publisher("~pointcloud_reproject",
PointCloud,
queue_size=2)
def Pointcloud_callback(self, human_centers):
#self.listener.waitForTransform("/map","/velodyne",rospy.Time(),rospy.Duration(0.5))
#try:
# now = rospy.Time.now()
# self.listener.waitForTransform("/map","/velodyne",now,rospy.Duration(0.5))
# (trans,rot) = listener.lookupTransform("/map","/velodyne",now)
#time = rospy.Time.now()
#rospy.logerr(time)
#self.tf.waitForTransform("map","thermal_camera",rospy.Time.now(),rospy.Duration(2.0))
#self.tf.waitForTransform("map","husky2/left_camera",rospy.Time.now(),rospy.Duration(4))
try:
#rospy.loginfo("Here!")
header = human_centers.header
human_centers.header.frame_id = "husky2/left_camera"
human_centers.header.stamp = rospy.Time.now()
self.tf.waitForTransform("map", "husky2/left_camera",
rospy.Time.now(), rospy.Duration(1.0))
self.pointcloud = self.tf.transformPointCloud("map", human_centers)
#rospy.loginfo(self.pointcloud.header)
self.pointcloud.header.stamp = header.stamp
self.pointcloud_pub.publish(self.pointcloud)
except (tf.LookupException, tf.ConnectivityException):
rospy.loginfo(
"There are any exception happened!"
) #,tf.ExtrapolationException,rospy.ROSTimeMovedBackwardsException):
rospy.loginfo("Husky2:The point has been published!")
class MockCamera(object):
"""A MockCamera reads saved point clouds.
"""
def __init__(self):
self.tf = TransformListener()
pass
def read_cloud(self, path):
"""Returns the sensor_msgs/PointCloud2 in the given bag file.
Args:
path: string, the path to a bag file with a single
sensor_msgs/PointCloud2 in it.
Returns: A sensor_msgs/PointCloud2 message, or None if there were no
PointCloud2 messages in the bag file.
"""
bag = rosbag.Bag(path)
for topic, msg, t in bag.read_messages():
if msg is not None:
return msg
return None
def read_cloud_odom(self, path):
"""Returns the sensor_msgs/PointCloud2 in the given bag file.
Args:
path: string, the path to a bag file with a single
sensor_msgs/PointCloud2 in it.
Returns: A sensor_msgs/PointCloud2 message, or None if there were no
PointCloud2 messages in the bag file.
"""
bag = rosbag.Bag(path)
for topic, msg, t in bag.read_messages():
if msg is not None:
return self.tf.transformPointCloud("odom", msg)
return None
class Trajectory_generator(object):
# Generate the object's trajectory from the central point of the detected box, then project
def __init__(self, node_name="pointcloud_reproject"):
self.pointcloud = PointCloud()
self.node_name = node_name
#self.listener = tf.TransformListener()
rospy.init_node(self.node_name)
self.tf = TransformListener()
self.pointcloud_sub = rospy.Subscriber("output_cloud_husky1_thermal",
PointCloud,
self.Pointcloud_callback)
self.pointcloud_pub = rospy.Publisher("~pointcloud_reproject",
PointCloud,
queue_size=2)
def Pointcloud_callback(self, human_centers):
#self.listener.waitForTransform("/map","/velodyne",rospy.Time(),rospy.Duration(0.5))
#try:
# now = rospy.Time.now()
# self.listener.waitForTransform("/map","/velodyne",now,rospy.Duration(0.5))
# (trans,rot) = listener.lookupTransform("/map","/velodyne",now)
#time = rospy.Time.now()
#rospy.logerr(time)
#self.tf.waitForTransform("map","thermal_camera",rospy.Time.now(),rospy.Duration(2.0))
while not rospy.is_shutdown():
try:
#self.tf.waitForTransform("map","thermal_camera",rospy.Time.now(),rospy.Duration(4.0))
self.pointcloud = self.tf.transformPointCloud(
"map", human_centers)
self.pointcloud_pub.publish(self.pointcloud)
rospy.loginfo("The point has been published!")
except (tf.ConnectivityException, tf.LookupException,
tf.ExtrapolationException,
rospy.ROSTimeMovedBackwardsException):
rospy.loginfo("Some Exceptions happend!")
rospy.spin()
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 OccupancyGrid(object):
'''
'''
def __init__(self,
width,
height,
resolution,
alpha=0.5,
origin_x=0,
origin_y=0):
'''
Constructor for OccupancyGrid class
width: disired width in meters of occupancy grid (x axis)
height: desired height in meters of occupancy grid (y axis)
resolution: desired meter representation of each cell in grid (each cell with be rxr)
Grid is indexed as follows: [y][x] ([height][width])
[ [ ................. ]
[ ................. ]
[ ................. ] ]
Each grid cell contains the probability of
'''
# Creates the ROS node.
rospy.init_node("map_pointcloud")
self.tf = TransformListener()
# Inits the pointcloud Subscriber
rospy.Subscriber("/aries/filtered_front_pointcloud", PointCloud,
self.pointcloud_callback)
# Initialize service that gets the current angle of the lidar
self._service = rospy.Service("/aries/get_occupancy_map",
occupancy_map,
self.handle_get_occupancy_map)
self.width = width
self.height = height
self.resolution = resolution
self.alpha = alpha
self.origin_x = origin_x
self.origin_y = origin_y
self.grid = None
self._construct_grid(self.width, self.height, self.resolution)
def _construct_grid(self, width, height, resolution):
'''
Private helper function. Uses width, height, and resolution variables to
construct and set self.grid
'''
self.grid = np.zeros((height / resolution, width / resolution))
# self.grid.fill(-1)
def _loc_to_indices(self, loc):
'''
Transforms global location coordinates (given in meters as x, y position tuple) to grid indices
'''
x = int(round(loc[0] / self.resolution))
y = int(round(loc[1] / self.resolution))
return (x, y)
def set_cell(self, loc, value):
'''
Given a tuple location (x, y) and a value, set that location in grid to value.
- Location is given in meters (global x, y location from localization routine)
- This function transforms given location to grid indices
- Value should be a probability given as a number between 0 and 1
'''
i = self._loc_to_indices(loc)
self.grid[i[1]][i[0]] = value
def get_cell(self, loc):
'''
Given global location in meters, return value at that location in occupancy grid
'''
i = self._loc_to_indices(loc)
return self.grid[i[1]][i[0]]
def handle_get_occupancy_map(self, req):
""" Return a nav_msgs/OccupancyGrid representation of this map. """
grid_msg = OGmsg()
# Set up the header.
grid_msg.header.stamp = rospy.Time.now()
grid_msg.header.frame_id = "map"
# .info is a nav_msgs/MapMetaData message.
grid_msg.info.resolution = self.resolution
grid_msg.info.width = self.width // self.resolution + 1
grid_msg.info.height = self.height // self.resolution + 1
# Rotated maps are not supported... quaternion represents no
# rotation.
grid_msg.info.origin = Pose(Point(self.origin_x, self.origin_y, 0),
Quaternion(0, 0, 0, 1))
# Flatten the numpy array into a list of integers from 0-100.
# This assumes that the grid entries are probalities in the
# range 0-1. This code will need to be modified if the grid
# entries are given a different interpretation (like
# log-odds).
flat_grid = self.grid.reshape(self.grid.size) * 100
grid_msg.data = [int(x) for x in np.round(flat_grid).tolist()]
print grid_msg.data
return occupancy_mapResponse(grid_msg)
def pointcloud_callback(self, cloud):
# Learning Rate: should be between 0 and 1
alpha = self.alpha
# Transforms the point cloud into the /map frame for mapping
self.tf.waitForTransform("front_laser", "map", rospy.Time(0),
rospy.Duration(4.0))
cloud = self.tf.transformPointCloud("map", cloud)
for point in cloud.points:
if (0 < point.x < self.width and 0 < point.y < self.height):
probability = min(abs(3 * point.z),
1.0) * alpha + self.get_cell(
(point.x, point.y)) * (1 - alpha)
self.set_cell((point.x, point.y), probability)
def run(self):
# Runs while shut down message is not recieved.
rate = rospy.Rate(10)
# Due to differences in startup time, the node needs to wait or it will raise errors
# by calling for tf transforms at times before startup of the tf broadcaster.
rospy.sleep(5)
# Waits until a transform is available
self.tf.waitForTransform("front_laser", "map", rospy.Time(0),
rospy.Duration(4.0))
# Main message processing loop
while not rospy.is_shutdown():
rate.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 = 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 Filter_PointCloud(object):
"""
The Filter_PointCloud class filters erroneous cloud data while it's still in
2D space.
"""
def __init__(self):
""" Start the mapper. """
rospy.init_node('filter_pointcloud')
self.tf = TransformListener()
# Setting the queue_size to 1 will prevent the subscriber from
# buffering cloud messages. This is important because the
# callback is likely to be too slow to keep up with the cloud
# messages. If we buffer those messages we will fall behind
# and end up processing really old clouds. Better to just drop
# old clouds and always work with the most recent available.
rospy.Subscriber('/aries/front_pointcloud',
PointCloud,
self.cloud_callback,
queue_size=1)
self.current_cloud = PointCloud() # current cloud message
self.received_cloud = False # True if we've received a new cloud, false if not
# Creates publisher for filtered point cloud topic
self._cloud_pub = rospy.Publisher('/aries/filtered_front_pointcloud',
PointCloud,
queue_size=10)
def cloud_callback(self, cloud):
'''
This function is called everytime a message is transmitted over /aries/front_pointcloud topic
'''
# Update current cloud
self.current_cloud = cloud
# Set received cloud flag to True
self.received_cloud = True
def process_cloud(self, cloud):
self.received_cloud = False
# Extracts coordinates into numpy arrays
y = np.array([v.x for v in cloud.points])
x = np.array([v.y for v in cloud.points])
# Set number of standard deviations to allow coordinates to vary within
nStd = 4
N = len(x)
stop = False
# Containers for discarded points
#outlierX = np.array([])
#outlierY = np.array([])
# Iteratively removes outliers and fine tunes the regression line.
while not stop and N > 0:
A = np.vstack([x, np.ones(len(x))]).T
# Performs linear regression
slope, intercept = np.linalg.lstsq(A, y)[0]
# Generates best-fit line
yLinear = slope * x + intercept
# Calculate errors of the points
errors = y - yLinear
# Determines standard deviation of the errors
sigErr = np.std(errors)
# Converts errors into multiples of standard deviations
magError = (np.absolute(errors) / sigErr)
# Finds the largest outlier and its index
val = np.amax(magError)
ind = np.argmax(magError)
# Checks if the largest outlier is outside of the specified bounds
if (val > nStd):
# Removes the outlier point
N -= 1
#outlierX = np.append(outlierX, x[ind])
#outlierY = np.append(outlierY, y[ind])
x = np.delete(x, ind)
y = np.delete(y, ind)
# print str(val) + " " + str(nStd)
else:
# All remaining points lie within boundaries, exit the loop
stop = True
# Updates the cloud message with the new coordinates, minus the outliers/noise
cloud.points = [
Point32(x=outY, y=outX, z=0) for outX, outY in zip(x, y)
]
# Transforms the point cloud into the /map frame for mapping
self.tf.waitForTransform("/front_laser", "/map", rospy.Time(0),
rospy.Duration(4.0))
cloud = self.tf.transformPointCloud("/map", cloud)
# Only points with potential obstacles need to be mapped.
# This removes points from the point cloud within safe z-height ranges
newPoints = []
for i, point in enumerate(cloud.points):
if (abs(point.z) < 0.1):
cloud.points[i].z = 0
# Publishes the new cloud of mapping points
self._cloud_pub.publish(cloud)
def run(self):
'''
Main work loop.
'''
rate = rospy.Rate(10)
# Due to differences in startup time, the node needs to wait or it will raise errors
# by calling for tf transforms at times before startup of the tf broadcaster.
rospy.sleep(5)
# Waits until a transform is available
self.tf.waitForTransform("/front_laser", "/map", rospy.Time(0),
rospy.Duration(4.0))
# Main message processing loop
while not rospy.is_shutdown():
if self.received_cloud:
self.process_cloud(self.current_cloud)
rate.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 = "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:
""" 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():
"""Sets up a particle filter
some parameters are specified at runtime.
---Parameters---
n_particles: the number of pose hypotheses
map_frame; str, name of the map frame
odometry_frame: str, name of the coordinate frame for relative updates
base_frame = str, name of a body-centered coordinate frame
d_thresh: float, smallest distance the robot moves before we do an update
theta_thresh: float, smallest rotation before we do an update
"""
def __init__(self):
self.n_particles = rospy.get_param('~n_particles', 500)
self.map_frame = "map"
self.odometry_frame = "odom"
self.d_thresh = rospy.get_param('~d_thresh', 0.1)
self.theta_thresh = rospy.get_param('~theta_thresh', 0.1)
self.resample_conf = rospy.get_param('~resample', 0.5)
self.xy_cauchy = rospy.get_param('~xy_cauchy', 0.5)
self.last_odom_update_pose = PoseStamped(
header=Header(stamp=rospy.get_rostime(), frame_id="odom"),
pose=Pose(position=Point(0, 0, 0),
orientation=Quaternion(*quaternion_from_euler(0, 0, 0))))
#We need a map
rospy.wait_for_service('static_map')
try:
map_pxy = rospy.ServiceProxy('static_map', GetMap)
self.map = map_pxy().map
except rospy.ServiceException:
rospy.logfatal("No map aquired")
#a sprinkle of tf
self.tf = TransformListener()
self.tfb = TransformBroadcaster()
#also some publishers
#particle cloud
self.beta_pub = rospy.Publisher('/alpha_pose/beta_list',
PoseArray,
queue_size=10)
#best guess
self.alpha_prime_pub = rospy.Publisher('/alpha_pose/prime',
PoseStamped,
queue_size=10)
#Two inputs from the robot need to be handled by the particle filter
#Position and laser scans
self.tf.waitForTransform('/base_link', '/base_laser_link',
rospy.Time(0), rospy.Duration(4.0))
rospy.Subscriber("/odom", Odometry, self.update_particles_with_odom)
rospy.Subscriber("/stable_scan", LaserScan,
self.update_particles_with_laser)
#list of hypothesised poses
self.betas = []
self.alpha_prime = None
#init particle filter
self.occupancy_field = OccupancyField(self.map)
self.make_transform()
self.initialize_betas()
self.most_likely_particle()
def beta_in_hull(self):
while True:
#square sampling
x, y = np.random.uniform(low=np.min(self.hull_pts, axis=0),
high=np.max(self.hull_pts, axis=0),
size=2)
#inside convex hull
if not self.poly.contains_point((x, y)):
continue
theta = np.random.uniform(low=0, high=360 - 1)
return Particle(x, y, theta, w=1) #normalize later
def initialize_betas(self):
"""Builds the initial beta particle list.
Box-samples around the convex hull of the map."""
#builds a closed polygon from convex hull vertices
#vertices are indexes
cvx_hull = self.occupancy_field.convex_hull
hull_pts = np.array(cvx_hull.points[cvx_hull.vertices])
#close the convex hull polygon with the first vertex
hull_pts = np.vstack((hull_pts, hull_pts[0]))
poly = Path(hull_pts)
self.hull_pts = hull_pts
self.poly = poly
while len(self.betas) <= self.n_particles:
b = self.beta_in_hull()
self.betas.append(b)
self.normalize_betas()
def normalize_betas(self):
"""Normalizes the w attribute of all Particles in self.betas"""
betas = deepcopy(self.betas)
weights = np.array([p.w for p in betas])
weights = weights / np.sum(weights)
mod_beta = []
for i, p in enumerate(betas):
p.w = weights[i]
mod_beta.append(p)
self.betas = mod_beta
@staticmethod
def unwrap(obj, attrs):
"""helper getattr wrapper for data structure unwrap"""
return [getattr(obj, x) for x in attrs]
@staticmethod
def rot(x):
"""returns a 2D rotation matrix that rotates anticlockwise in the xy-plane"""
return np.array([[np.cos(x), -np.sin(x)], [np.sin(x), np.cos(x)]])
def update_particles_with_odom(self, msg):
"""When the robot moves some relative distance bounded
by some parameters, update the particles for that motion
This is relative motion, and for small distances, holds (mostly) true.
It is based off wheel encoders, so there are several failure modes"""
betas = deepcopy(self.betas)
msg_loc = np.array(self.unwrap(msg.pose.pose.position, 'xy'))
last_loc = np.array(
self.unwrap(self.last_odom_update_pose.pose.position, 'xy'))
distance = msg_loc - last_loc
if np.linalg.norm(distance) > self.d_thresh:
self.last_odom_update_pose = PoseStamped(
header=Header(stamp=rospy.get_rostime(), frame_id='odom'),
pose=Pose(
position=msg.pose.pose.position,
orientation=self.last_odom_update_pose.pose.orientation))
new_betas = []
for p in betas:
attrs = np.array(self.unwrap(p, 'xy'))
msg_angle = np.arctan(distance[0] / distance[1]) * 180 / np.pi
attrs = attrs + np.dot(
self.rot((p.theta - msg_angle) * np.pi / 180), distance)
new_betas.append(Particle(*attrs, theta=p.theta, w=p.w))
self.betas = new_betas
msg_theta = np.array(self.unwrap(msg.pose.pose.orientation, 'xyzw'))
last_theta = np.array(
self.unwrap(self.last_odom_update_pose.pose.orientation, 'xyzw'))
#corresponds to 'z' axis rotation
angle_delta = angle_diff(msg_theta[2], last_theta[2])
if abs(angle_delta) > self.theta_thresh:
self.last_odom_update_pose = PoseStamped(
header=Header(stamp=rospy.get_rostime(), frame_id='odom'),
pose=Pose(position=self.last_odom_update_pose.pose.position,
orientation=msg.pose.pose.orientation))
new_betas = []
for p in betas:
new_betas.append(
Particle(p.x, p.y, (p.theta + euler_from_quaternion(
(0, 0, angle_delta, 0))[2]) % 360, p.w))
self.betas = new_betas
self.resample_particles()
@staticmethod
def laser_to_cloud(msg):
scan = msg.ranges[:-1] #the last value is a repeated first value
angles = np.array(range(len(scan))) * 180 / np.pi
xs = np.cos(angles) * scan
ys = np.sin(angles) * scan
points = [
Point32(x, y, 0) for x, y in zip(xs, ys)
if not np.linalg.norm([x, y]) == 0.0
] #drop all zero-distance readings
cloud = PointCloud(header=Header(frame_id="base_laser_link",
stamp=rospy.Time(0)),
points=points,
channels=ChannelFloat32(
name="distance",
values=[d for d in scan if not d == 0.0]))
return cloud
def update_particles_with_laser(self, msg):
#return
#get scan points in cartesian
pts = self.tf.transformPointCloud("base_link",
self.laser_to_cloud(msg)).points
#coulumn vector
pts = np.array([(p.x, p.y) for p in pts])
weights = []
betas = deepcopy(self.betas)
scan_cost = []
for p in betas:
tformed_pts = np.dot(pts, self.rot(
p.theta * 180 / np.pi)) + np.array([p.x, p.y])
xs, ys = tformed_pts.T
scan_cost.append(
np.sum([
self.occupancy_field.get_closest_obstacle_distance(x, y)
for x, y, in zip(xs, ys)
]))
for i, p in enumerate(betas):
p.w = p.w / scan_cost[i]
self.betas = betas
self.resample_particles()
def normalize_particle_weights(self):
betas = copy(self.betas)
ws = np.array([p.w for p in betas])
ws = ws / np.sum(ws)
for i, p in enumerate(betas):
p.w = ws[i]
self.betas = betas
def resample_particles(self):
"""deletes invalid or unlikely particles, and samples new ones"""
if not hasattr(self, 'alpha_pose'):
return
self.normalize_particle_weights()
betas = deepcopy(self.betas)
#if it's likely and within the hull
good_particles = []
max_w = np.max(np.array([p.w for p in betas]))
for p in betas:
if p.w > (max_w * self.resample_conf):
if self.poly.contains_point((p.x, p.y)):
good_particles.append(p)
#number of leftover particles / whatever's left in probability
if len(good_particles) < self.n_particles:
leftover_prob = 1 - np.sum(np.array([p.w for p in good_particles]))
p_prob = leftover_prob / (self.n_particles - len(good_particles))
else:
return
while len(good_particles) <= self.n_particles:
x, y = self.unwrap(self.alpha_pose.position, 'xy')
theta = euler_from_quaternion(
self.unwrap(self.alpha_pose.orientation, 'xyzw'))[2]
orig_x, orig_y = x, y
while True:
x = cauchy.rvs(loc=orig_x, scale=self.xy_cauchy)
y = cauchy.rvs(loc=orig_y, scale=self.xy_cauchy)
if self.poly.contains_point((x, y)):
break
theta = np.random.uniform(low=0, high=360 - 1)
#b = self.beta_in_hull()
#good_particles.append(Particle(*self.unwrap(b, ['x', 'y', 'theta']), w=p_prob))
good_particles.append(Particle(x, y, theta, p_prob))
self.betas = good_particles
self.most_likely_particle()
def most_likely_particle(self):
#mode of the distribution
betas = deepcopy(self.betas)
self.alpha_pose = betas[np.argmax([p.w for p in betas])].as_pose()
self.alpha_prime_pub.publish(
PoseStamped(header=Header(stamp=rospy.get_rostime(),
frame_id=self.map_frame),
pose=self.alpha_pose))
def make_transform(self, trans=None, rot=None):
if trans == None:
trans = self.unwrap(self.map.info.origin.position, 'xyz')
if rot == None:
rot = self.unwrap(self.map.info.origin.orientation, 'xyzw')
self.tfb.sendTransform(trans, rot, rospy.Time.now(), self.map_frame,
self.odometry_frame)
def run(self):
rate = rospy.Rate(5)
while not rospy.is_shutdown():
self.beta_pub.publish(self.beta_as_posearray(self.betas))
self.make_transform()
rate.sleep()
def beta_as_posearray(self, betas):
return PoseArray(header=Header(stamp=rospy.get_rostime(),
frame_id=self.map_frame),
poses=[p.as_pose() for p in betas])
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
# 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)
# 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()
# TODO: assign the latest 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 = 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
# TODO: modify particles using delta
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()
# TODO: fill out the rest of the implementation
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
# TODO: implement this
pass
@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 = []
# TODO create particles
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) """
# TODO: implement this
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
# 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 = 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)
collision_operation.object2 = coll_map_res.collision_support_surface_name
collision_operation.operation = CollisionOperation.DISABLE
ordered_collision_operations.collision_operations = [collision_operation]
if not move_arm_to_grasping_joint_pose(joint_names, joint_positions, allowed_contacts, gripper_paddings, ordered_collision_operations):
exit(1)
rospy.loginfo('Pickup stage has successfully finished. Will place the object now')
############################################################################
####################### PLACE STAGE START HERE #############################
listener = TransformListener()
# move grasped object and find a good grasp do we can approach and place
obj_pcluster = listener.transformPointCloud('base_link', coll_map_res.graspable_objects[0].cluster)
x = [point.x for point in obj_pcluster.points]
y = [point.y for point in obj_pcluster.points]
z = [point.z for point in obj_pcluster.points]
offset = [0.0, -0.2, 0.0]
req = GraspPlanningRequest()
req.arm_name = 'left_arm'
req.target.cluster.header.frame_id = 'base_link'
req.target.cluster.points = [Point32(x[i]+offset[0], y[i]+offset[1], z[i]+offset[2]) for i in range(len(x))]
req.target.type = GraspableObject.POINT_CLUSTER
req.collision_object_name = coll_map_res.collision_object_names[0]
req.collision_support_surface_name = coll_map_res.collision_support_surface_name
rospy.loginfo('trying to find a good grasp for graspable object %s' % coll_map_res.collision_object_names[0])
grasping_result = grasp_planning_srv(req)
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 Filter_PointCloud(object):
"""
The Filter_PointCloud class filters erroneous cloud data while it's still in
2D space.
"""
def __init__(self):
""" Start the mapper. """
rospy.init_node('filter_pointcloud')
self.tf = TransformListener()
# Setting the queue_size to 1 will prevent the subscriber from
# buffering cloud messages. This is important because the
# callback is likely to be too slow to keep up with the cloud
# messages. If we buffer those messages we will fall behind
# and end up processing really old clouds. Better to just drop
# old clouds and always work with the most recent available.
rospy.Subscriber('/aries/front_pointcloud',
PointCloud, self.cloud_callback, queue_size=1)
self.current_cloud = PointCloud() # current cloud message
self.received_cloud = False # True if we've received a new cloud, false if not
# Creates publisher for filtered point cloud topic
self._cloud_pub = rospy.Publisher('/aries/filtered_front_pointcloud', PointCloud, queue_size=10)
def cloud_callback(self, cloud):
'''
This function is called everytime a message is transmitted over /aries/front_pointcloud topic
'''
# Update current cloud
self.current_cloud = cloud
# Set received cloud flag to True
self.received_cloud = True
def process_cloud(self, cloud):
self.received_cloud = False
# Extracts coordinates into numpy arrays
y = np.array([v.x for v in cloud.points])
x = np.array([v.y for v in cloud.points])
# Set number of standard deviations to allow coordinates to vary within
nStd = 4
N = len(x)
stop = False
# Containers for discarded points
#outlierX = np.array([])
#outlierY = np.array([])
# Iteratively removes outliers and fine tunes the regression line.
while not stop and N > 0:
A = np.vstack([x, np.ones(len(x))]).T
# Performs linear regression
slope, intercept = np.linalg.lstsq(A, y)[0]
# Generates best-fit line
yLinear = slope*x + intercept
# Calculate errors of the points
errors = y - yLinear
# Determines standard deviation of the errors
sigErr = np.std(errors)
# Converts errors into multiples of standard deviations
magError = (np.absolute(errors)/sigErr)
# Finds the largest outlier and its index
val = np.amax(magError)
ind = np.argmax(magError)
# Checks if the largest outlier is outside of the specified bounds
if(val > nStd):
# Removes the outlier point
N -= 1
#outlierX = np.append(outlierX, x[ind])
#outlierY = np.append(outlierY, y[ind])
x = np.delete(x, ind)
y = np.delete(y, ind)
# print str(val) + " " + str(nStd)
else:
# All remaining points lie within boundaries, exit the loop
stop = True
# Updates the cloud message with the new coordinates, minus the outliers/noise
cloud.points = [Point32(x=outY, y=outX, z=0) for outX, outY in zip(x, y)]
# Transforms the point cloud into the /map frame for mapping
self.tf.waitForTransform("/front_laser", "/map", rospy.Time(0), rospy.Duration(4.0))
cloud = self.tf.transformPointCloud("/map", cloud)
# Only points with potential obstacles need to be mapped.
# This removes points from the point cloud within safe z-height ranges
newPoints = []
for i, point in enumerate(cloud.points):
if (abs(point.z) < 0.1):
cloud.points[i].z = 0
# Publishes the new cloud of mapping points
self._cloud_pub.publish(cloud)
def run(self):
'''
Main work loop.
'''
rate = rospy.Rate(10)
# Due to differences in startup time, the node needs to wait or it will raise errors
# by calling for tf transforms at times before startup of the tf broadcaster.
rospy.sleep(5)
# Waits until a transform is available
self.tf.waitForTransform("/front_laser", "/map", rospy.Time(0), rospy.Duration(4.0))
# Main message processing loop
while not rospy.is_shutdown():
if self.received_cloud:
self.process_cloud(self.current_cloud)
rate.sleep()
class GetGraspFromEntity(EventState):
'''
State taking an entity and returning the best grasp based on the pointcloud entity
@author Jeffrey Cousineau
@license Apache2.0
># Entity object entity to grasp
#> GraspingPose object pose to grasp the given entity
-- ApproachDistance float distance from object at which the gripper shall approach
-- DistanceScoringMultiplier float how much a meter distance affects the score (Higher = closer poses)
-- OrientationScoringMultiplier float how much radians difference from ideal pose affects the score (Higher = more correctly orientated poses)
-- GraspScoringMultiplier float how much scores from gpd affects the final score (Higher = more weight in final score from gpd)
<= done grasp was found and returned
<= failed grasp was not found
'''
def graspCallback(self, msg):
self.graspList = msg.grasps
def __init__(self, approachDistance, distanceScoringMultiplier,
orientationScoringMultiplier, graspScoringMultiplier):
super(GetGraspFromEntity,
self).__init__(outcomes=['done', 'failed'],
input_keys=['Entity'],
output_keys=['ApproachPose', 'GraspingPose'])
self.approachDistance = approachDistance
self.distanceScoringMultiplier = distanceScoringMultiplier
self.orientationScoringMultiplier = orientationScoringMultiplier
self.graspScoringMultiplier = graspScoringMultiplier
self.graspList = None
self.grasps_sub = rospy.Subscriber('/detect_grasps/clustered_grasps',
GraspConfigList, self.graspCallback)
self.listener = TransformListener(20)
self.idealRoll = 0.0
self.idealPitch = 0.0
self.idealYaw = 0.07 # 4 degrees to the right relatively to the robot POV
self.maxgraspScore = 0.0
self.pub = rospy.Publisher('cloud_indexed', CloudIndexed, queue_size=1)
self.marker_pub = rospy.Publisher('grasp_pose',
PoseStamped,
queue_size=1)
self.marker_pub_app = rospy.Publisher('approach_pose',
PoseStamped,
queue_size=1)
def execute(self, userdata):
if userdata.Entity.pointcloud.header.frame_id == "":
grasp, approach = self.getGraspWithoutPointcloud(userdata.Entity)
else:
grasp, approach = self.getGraspFromPointcloud(userdata.Entity)
# return the chosen poses
userdata.GraspingPose = grasp
userdata.ApproachPose = approach
# Creates markers for the chosen pose
stamped = PoseStamped()
stamped.header.frame_id = "base_link"
stamped.header.stamp = rospy.Time.now()
stamped.pose = grasp
self.marker_pub.publish(stamped)
stamped.pose = approach
self.marker_pub_app.publish(stamped)
return 'done'
def graspToPose(self, grasp):
pose = Pose()
pose.position = grasp.top
yaw = math.atan2(grasp.approach.y, grasp.approach.x)
distXY = (grasp.approach.x**2 + grasp.approach.y**2)**0.5
pitch = -math.atan2(grasp.approach.z, distXY)
approach = np.array(
[grasp.approach.x, grasp.approach.y, grasp.approach.z])
approach /= (approach**2).sum()**0.5 # Get the unit vector
binormal = np.array(
[grasp.binormal.x, grasp.binormal.y, grasp.binormal.z])
binormal /= (binormal**2).sum()**0.5 # Get the unit vector
binormal_ref_x = np.cross(np.array([0, 0, 1]), approach)
binormal_ref_y = np.cross(binormal_ref_x, approach)
roll = math.atan2(np.vdot(approach, binormal_ref_y),
np.vdot(approach, binormal_ref_x)) * math.pi / 2
# Transformation to quaternion for a Pose
quat = quaternion_from_euler(roll, pitch, yaw, axes='sxyz')
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
return pose
def getGraspFromPointcloud(self, entity):
Logger.loginfo("Selected entity : " + str(entity.ID))
Logger.loginfo("Current position : (" + str(entity.position.x) + ", " +
str(entity.position.y) + ", " + str(entity.position.x) +
")")
# Convert to Pointcloud and change frame of reference to base)link
pointCloud = PointCloud()
pointCloud.header = entity.pointcloud.header
for p in point_cloud2.read_points(entity.pointcloud):
point = Point32()
point.x, point.y, point.z = [p[0], p[1], p[2]]
pointCloud.points.append(point)
pointCloud.header.stamp = rospy.Time.now() - rospy.Duration(1)
self.listener.waitForTransform(pointCloud.header.frame_id,
"/base_link", rospy.Time(0),
rospy.Duration(10))
pointCloud = self.listener.transformPointCloud("/base_link",
pointCloud)
cloud = []
for p in pointCloud.points:
cloud.append([p.x, p.y, p.z])
Logger.loginfo("Cloud size : " + str(len(cloud)))
# if len(cloud) > 0:
cloud = np.asarray(cloud)
X = cloud
A = np.c_[X[:, 0], X[:, 1], np.ones(X.shape[0])]
C, _, _, _ = lstsq(A, X[:, 2])
a, b, c, d = C[0], C[1], -1., C[
2] # coefficients of the form: a*x + b*y + c*z + d = 0.
dist = ((a * X[:, 0] + b * X[:, 1] + d) - X[:, 2])**2
err = dist.sum()
idx = np.where(dist > 0.01)
msg = CloudIndexed()
header = Header()
header.frame_id = "/base_link"
header.stamp = rospy.Time.now()
msg.cloud_sources.cloud = point_cloud2.create_cloud_xyz32(
header, cloud.tolist())
msg.cloud_sources.view_points.append(Point(0, -0.5, 1.5))
for i in xrange(cloud.shape[0]):
msg.cloud_sources.camera_source.append(Int64(0))
for i in idx[0]:
msg.indices.append(Int64(i))
# s = raw_input('Hit [ENTER] to publish')
self.pub.publish(msg)
i = 0
################################
# Temporary setting a timeout
while self.graspList == None:
i = i + 1
rospy.sleep(1)
if i > 20:
return self.getGraspWithoutPointcloud(entity)
bestScore = 0
bestGrasp = None
# Normalisation des scores de grasp
for grasp in self.graspList:
if grasp.score.data > self.maxgraspScore:
self.maxgraspScore = grasp.score.data
for grasp in self.graspList:
# Poses with a negative approach gets a negative multiplier
if grasp.approach.z < 0: # Approche par le haut
# poseScore = self.calculateGraspScore(pose)
ref = [0.577350269, 0.577350269, -0.577350269]
app = [grasp.approach.x, grasp.approach.y, grasp.approach.z]
poseScore = np.dot(app, ref)
rospy.loginfo("Total pose score (Positive approach): %s",
str(poseScore))
if bestScore < poseScore:
bestScore = poseScore
bestGrasp = grasp
if bestGrasp is not None:
pose = self.graspToPose(bestGrasp)
# Generate approach pose
approach_pose = Pose()
applength = np.linalg.norm([
bestGrasp.approach.x, bestGrasp.approach.y,
bestGrasp.approach.z
])
approach_pose.position.x = pose.position.x - bestGrasp.approach.x / applength * self.approachDistance
approach_pose.position.y = pose.position.y - bestGrasp.approach.y / applength * self.approachDistance
approach_pose.position.z = pose.position.z - bestGrasp.approach.z / applength * self.approachDistance
approach_pose.orientation = pose.orientation
return pose, approach_pose
return self.getGraspWithoutPointcloud(entity)
def getGraspWithoutPointcloud(self, entity):
# verifie si on recoit une pose ou un point
grasp = Pose()
grasp.position = entity.position
gripperX = 0
gripperY = -0.5
# calcul des angles
yaw = math.atan2((grasp.position.y - gripperY),
(grasp.position.x - gripperX))
dist = ((grasp.position.y - gripperY)**2 +
(grasp.position.x - gripperX)**2)**0.5
pitch = 0
# calcul du quaternion
quat = quaternion_from_euler(0, pitch, yaw)
self.quat = Quaternion()
self.quat.x = quat[0]
self.quat.y = quat[1]
self.quat.z = quat[2]
self.quat.w = quat[3]
grasp.orientation = self.quat
# calcul du vecteur dapproche avec les points
dX = (gripperX - grasp.position.x)
dY = (gripperY - grasp.position.y)
length = (dX**2 + dY**2)**0.5
dX *= self.approachDistance / length
dY *= self.approachDistance / length
# applique le vecteur dapproche
approach = Pose()
approach.position.x = grasp.position.x + dX
approach.position.y = grasp.position.y + dY
approach.position.z = grasp.position.z
approach.orientation = self.quat
return grasp, approach
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:
""" 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)
