class ParticleFilter(object): """ The class that represents a Particle Filter ROS Node """ def __init__(self): rospy.init_node('pf') # pose_listener responds to selection of a new approximate robot # location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publisher for the particle cloud for visualizing in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # create instances of two helper objects that are provided to you # as part of the project self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = \ self.transform_helper.convert_pose_to_xy_and_theta(msg.pose.pose) # TODO this should be deleted before posting self.transform_helper.fix_map_to_odom_transform(msg.pose.pose, msg.header.stamp) # initialize your particle filter based on the xy_theta tuple def run(self): r = rospy.Rate(5) while not(rospy.is_shutdown()): # in the main loop all we do is continuously broadcast the latest # map to odom transform self.transform_helper.send_last_map_to_odom_transform() r.sleep()
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node( 'pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 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 = 300 # the number of particles to use self.initial_uncertainty_xy = 1 # Amplitute factor of initial x and y uncertainty self.initial_uncertainty_theta = 0.5 # Amplitude factor of initial theta uncertainty self.variance_scale = 0.15 # Scaling term for variance effect on resampling self.n_particles_average = 20 # Number of particles to average for pose update self.linear_var_thresh = 0.05 # Maximum confidence along x/y (meters) self.angular_var_thresh = 0.2 # Maximum confidence along theta (radians) # self.resample_noise_xy = 0.1 # Amplitude factor of resample x and y noise # self.resample_noise_theta = 0.1 # Amplitude factor of resample theta noise self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi/6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # publish custom particle array messge type self.particle_viz_pub = rospy.Publisher("weighted_particlecloud", ParticleArray, queue_size=10) # laser_subscriber listens for data from the lidar rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] # change use_projected_stable_scan to True to use point clouds instead of laser scans self.use_projected_stable_scan = False self.last_projected_stable_scan = None if self.use_projected_stable_scan: # subscriber to the odom point cloud rospy.Subscriber("projected_stable_scan", PointCloud, self.projected_scan_received) self.current_odom_xy_theta = [] self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() self.initialized = True def update_robot_pose(self, timestamp): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) Our implementation averages a couple of the best particles to update the pose """ # first make sure that the particle weights are normalized self.normalize_particles() sum_x, sum_y, sum_theta = 0, 0, 0 # sort our particles by weight best_particles = sorted(self.particle_cloud) # take the top particles with the highest weights best_particles = best_particles[-self.n_particles_average:] # find the average of this subset for p in best_particles: sum_x += p.x sum_y += p.y sum_theta += p.theta # Assign the latest pose into self.robot_pose as a Pose object self.robot_pose = self.transform_helper.convert_xy_and_theta_to_pose( sum_x/self.n_particles_average, sum_y/self.n_particles_average, sum_theta/self.n_particles_average) self.transform_helper.fix_map_to_odom_transform(self.robot_pose, timestamp) def projected_scan_received(self, msg): self.last_projected_stable_scan = msg def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2])# COUNTERCLOCKWISE self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # update particles r = math.sqrt(delta[0]**2 + delta[1]**2) for particle in self.particle_cloud: particle.x += r*math.cos(particle.theta) particle.y += r*math.sin(particle.theta) particle.theta += delta[2] def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ xs, ys, thetas, weights = [],[],[],[] # Make a list of particle stats for draw_random_sample to use for p in self.particle_cloud: xs.append(p.x) ys.append(p.y) thetas.append(p.theta) weights.append(p.w) # Throw out some particles self.particle_cloud = self.draw_random_sample(self.particle_cloud, weights, self.n_particles) # Compute variance of particles x_var = np.var(xs) y_var = np.var(ys) theta_var = np.var(weights) # Set a threshold for minimum linear and angular variance # This prevents our filter from becoming "overconfident" in the estimate if x_var < self.linear_var_thresh: x_var = self.linear_var_thresh if y_var < self.linear_var_thresh: y_var = self.linear_var_thresh if theta_var < self.angular_var_thresh: theta_var = self.angular_var_thresh # Inject some noise into the new cloud based on current variance for p in self.particle_cloud: noise = np.random.randn(3) p.x += noise[0] * x_var * self.variance_scale p.y += noise[1] * y_var * self.variance_scale p.theta += noise[2] * theta_var * self.variance_scale self.normalize_particles() def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ for p in self.particle_cloud: # Compute delta to x,y coords in map frame of each lidar point assuming # lidar is centered at the base_link # TODO: Account for the offset between the lidar and the base_link angles = np.linspace(0, 2*math.pi, num=361) dxs = np.array(msg.ranges) * np.cos(angles + p.theta) dys = np.array(msg.ranges) * np.sin(angles + p.theta) # Initialize total distance to 0 d = 0 # Initialize number of valid points valid_pts = len(dxs) for dx,dy in zip(dxs, dys): # Ignore points with invalid ranges if dx == 0 and dy == 0: continue # Apply delta x = p.x + dx y = p.y + dy # Find nearest point to each lidar point according to map dist = self.occupancy_field.get_closest_obstacle_distance(x, y) # Check to make sure lidar point is actually on the map if not np.isnan(dist): d += dist else: valid_pts -= 1 # If there aren't enough valid points for the particle, assume that it's # not good # TODO: Add a ROS param threshold for this if valid_pts < 10: p.w = 0 else: # Update particle weight based on inverse of average squared difference if d != 0: p.w = 1 / ((d ** 2)/valid_pts) else: # If difference is exactly 0, something's likely wrong rospy.logwarn("Computed difference between particle projection and lidar scan is exactly 0") self.normalize_particles() @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(msg.header.stamp, xy_theta) def initialize_particle_cloud(self, timestamp, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is omitted, the odometry will be used """ if xy_theta is None: xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] for i in range(self.n_particles): noise = (np.random.randn(3)*self.initial_uncertainty_xy) noise[2] = np.random.randn()*self.initial_uncertainty_theta new_pose = np.array(xy_theta) + noise new_particle = Particle(*new_pose) self.particle_cloud.append(new_particle) self.normalize_particles() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ cumulative_weight = 0 # Compute cumulative weight for p in self.particle_cloud: cumulative_weight += p.w # Normalize weights for p in self.particle_cloud: p.w = p.w/cumulative_weight def publish_particles(self, msg): particles_conv = [] custom_particle_msgs = [] for p in self.particle_cloud: particle_pose = p.as_pose() particles_conv.append(particle_pose) # Create new particle message new_particle = Particle() new_particle.pose = particle_pose new_particle.weight = p.w custom_particle_msgs.append(new_particle) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) # send our custom message to visualize weights in rviz self.particle_viz_pub.publish(ParticleArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), particles=custom_particle_msgs)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, we hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): # wait for initialization to complete rospy.loginfo_once("Waiting for initial pose estimate...") return else: rospy.loginfo_once("Initial pose estimate found!") # wait a little while to see if the transform becomes available. This fixes a race # condition where the scan would arrive a little bit before the odom to base_link transform # was updated. self.tf_listener.waitForTransform(self.base_frame, msg.header.frame_id, msg.header.stamp, rospy.Duration(0.5)) if not(self.tf_listener.canTransform(self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative to the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(self.odom_pose.pose) if not self.current_odom_xy_theta: self.current_odom_xy_theta = new_odom_xy_theta return if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud(msg.header.stamp) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry if self.last_projected_stable_scan: last_projected_scan_timeshift = deepcopy(self.last_projected_stable_scan) last_projected_scan_timeshift.header.stamp = msg.header.stamp self.scan_in_base_link = self.tf_listener.transformPointCloud("base_link", last_projected_scan_timeshift) self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose(msg.header.stamp) # update robot's pose self.resample_particles() # resample particles to focus on areas of high density # publish particles (so things like rviz can see them) self.publish_particles(msg)
class SensorLikelihoodTest(object): def __init__(self): rospy.init_node("sensor_likelihood_test") self.occupancy_field = OccupancyField() self.tf_helper = TFHelper() self.latest_scan_ranges = [] rospy.Subscriber('/scan', LaserScan, self.read_sensor) self.odom_poses = PoseArray() self.odom_poses.header.frame_id = "odom" self.particle_pose_pub = rospy.Publisher('/particle_pose_array', PoseArray, queue_size=10) self.odom_pose_pub = rospy.Publisher('odom_pose', PoseArray, queue_size=10) self.marker_pub = rospy.Publisher('/visualization_marker_array', MarkerArray, queue_size=10) self.p_distrib = ParticleDistribution() self.init_particles() # self.p = Particle(x=0, y=0, theta=0, weight=1) self.particle_poses = PoseArray() self.particle_poses.header.frame_id = "map" self.last_odom_pose = PoseStamped() self.last_odom_pose.header.frame_id = "odom" self.last_odom_pose.header.stamp = rospy.Time(0) self.base_link_pose = PoseStamped() self.base_link_pose.header.frame_id = "base_link" self.base_link_pose.header.stamp = rospy.Time(0) self.counter = 0 self.is_first = True # pose_listener responds to selection of a new approximate robot # location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) def init_particles(self): self.p_distrib.particle_list = [] # generate initial list of hypothesis (particles) for i in range(self.p_distrib.num_particles): # Find a random valid point on the map x = random.uniform(-2, 2) y = random.uniform(-2, 2) theta = random.randint(0, 361) weight = 1.0 / self.p_distrib.num_particles # Add new particle to list self.p_distrib.particle_list.append( Particle(x=x, y=y, theta=theta, weight=weight)) # Normalize weights # self.p_distrib.normalize_weights() def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = \ self.tf_helper.convert_pose_to_xy_and_theta(msg.pose.pose) self.tf_helper.fix_map_to_odom_transform(msg.pose.pose, msg.header.stamp) self.tf_helper.send_last_map_to_odom_transform() # initialize your particle filter based on the xy_theta tuple def read_sensor(self, scan_msg): self.latest_scan_ranges = scan_msg.ranges def run(self): self.tf_helper.fix_map_to_odom_transform(Pose(), rospy.Time(0)) r = rospy.Rate(1) while not rospy.is_shutdown(): try: print("Trying") (trans, rot) = self.tf_helper.tf_listener.lookupTransform( '/odom', '/base_link', rospy.Time(0)) # print("Transform: Trans: {} \n Rot: {}".format(trans, rot)) if (self.latest_scan_ranges != []): new_odom_pose = self.tf_helper.tf_listener.transformPose( 'odom', self.base_link_pose) self.odom_poses.poses.append(new_odom_pose.pose) self.odom_poses.header.stamp = rospy.Time.now() self.odom_pose_pub.publish(self.odom_poses) # new_odom_pose_map = self.tf_helper.tf_listener.transformPose('map', new_odom_pose) new_odom_x, new_odom_y, new_odom_theta = self.tf_helper.convert_pose_to_xy_and_theta( new_odom_pose.pose) last_odom_x, last_odom_y, last_odom_theta = self.tf_helper.convert_pose_to_xy_and_theta( self.last_odom_pose.pose) x_change = new_odom_x - last_odom_x y_change = new_odom_y - last_odom_y angular_change = self.tf_helper.angle_diff( new_odom_theta, last_odom_theta) # radians print("x: {}, y: {}, theta: {}".format( x_change, y_change, degrees(angular_change))) self.is_first = False if not self.is_first: self.propagate(x_change, y_change, angular_change) self.last_odom_pose = new_odom_pose # # Update particle to be in odom for check # x, y, theta = self.tf_helper.convert_pose_to_xy_and_theta(new_odom_pose_map.pose) # print("x: {}, y: {}, theta: {}".format(x, y, theta)) # # self.p.x = x # self.p.y = y # self.p.theta = degrees(theta) self.get_how_likely() self.p_distrib.normalize_weights() self.p_distrib.resample() self.p_distrib.normalize_weights() # self.tf_helper.send_last_map_to_odom_transform() # x = raw_input("Press Enter to continue") r.sleep() except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue def propagate(self, dx, dy, dtheta): for p in self.p_distrib.particle_list: # Update the last odom pose in the process r = sqrt(dx**2 + dy**2) angle = radians( p.theta) # Add p.theta to account for particle's rotation # print("p.theta: {} + new_angle: {} = angle: {}".format(radians(self.p.theta), atan2(dy, dx), angle)) p.x += r * cos(angle) p.y += r * sin(angle) p.theta = (p.theta + degrees(dtheta) + random.randint(1, 180)) % 360 # Wrap angle self.particle_poses = self.p_distrib.get_particle_pose_array() self.particle_poses.header.stamp = rospy.Time.now() self.particle_pose_pub.publish(self.particle_poses) def get_how_likely(self): angles = [0, 45] # Use only some of the angles for now num_angles = 0 marker_arr = MarkerArray() for p in self.p_distrib.particle_list: for angle in angles: reading = self.latest_scan_ranges[angle] if (reading > 0.0): print("Have a valid reading: {} at angle: {}".format( reading, angle)) num_angles += 1 # Take into account robot's yaw yaw = p.theta angle_in_map = yaw + angle predicted_obstacle_x, predicted_obstacle_y = self.move_coordinate( p.x, p.y, angle_in_map, reading) my_marker = Marker() my_marker.header.stamp = rospy.Time.now() my_marker.header.frame_id = "map" my_marker.color.a = 0.5 my_marker.type = Marker.SPHERE my_marker.id = self.counter self.counter += 1 my_marker.pose.position.x = predicted_obstacle_x my_marker.pose.position.y = predicted_obstacle_y my_marker.lifetime = rospy.Time(1) marker_arr.markers.append(my_marker) # error = self.get_predicted_obstacle_error( # reading, pos[0], pos[1], angle_in_map) predicted_reading = self.occupancy_field.get_closest_obstacle_distance( predicted_obstacle_x, predicted_obstacle_y) print("Predicted x: {}, y: {}, reading: {}".format( predicted_obstacle_x, predicted_obstacle_y, predicted_reading)) error = predicted_reading if (predicted_reading != predicted_reading): # Check for nan print("Got Nan") my_marker.color.g = 1.0 my_marker.scale.x = 0.1 my_marker.scale.y = 0.1 my_marker.scale.z = 0.1 else: my_marker.color.b = 1.0 - error my_marker.color.r = error + 0.1 my_marker.scale.x = error + 0.1 my_marker.scale.y = error + 0.1 my_marker.scale.z = error + 0.1 # Gaussian probability sigma = 0.1 p.weight += math.exp((-error**2) / (2 * sigma**2)) if (num_angles > 0): p.weight = p.weight / num_angles self.marker_pub.publish(marker_arr) def get_predicted_obstacle_error(self, distance_reading, x, y, angle): # Predict location of objects based on laser scan reading. predicted_obstacle_x, predicted_obstacle_y = self.move_coordinate( x, y, angle, distance_reading) # Find the closest obstacle to the predicted obstacle position predicted_reading = self.occupancy_field.get_closest_obstacle_distance( predicted_obstacle_x, predicted_obstacle_y) print("Predicted x: {}, y: {}, reading: {}".format( predicted_obstacle_x, predicted_obstacle_y, predicted_reading)) return predicted_reading def move_coordinate(self, x, y, angle, distance): return (x + cos(radians(angle)) * distance, y + sin(radians(angle)) * distance) def get_uniform_probability(self, error): # x axis is width max_width = 1.0 * self.occupancy_field.map.info.width * map_model.occupancy_field.map.info.resolution / 2.0 # y axis is height max_height = 1.0 * self.occupancy_field.map.info.height * map_model.occupancy_field.map.info.resolution / 2.0 max_distance = sqrt(max_width**2 + max_height**2) return (max_distance - error) / max_distance
class ParticleFilter(): def __init__(self): # Initialize node and attributes rospy.init_node("ParticleFilter") # Subscribers self.lidar_sub = rospy.Subscriber("/scan", LaserScan, self.lidar_callback) self.odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback) self.initial_estimate_sub = rospy.Subscriber( "/initialpose", PoseWithCovarianceStamped, self.pose_estimate_callback) # publishers self.all_particles_pub = rospy.Publisher("/visualization_particles", MarkerArray, queue_size=10) self.init_particles_pub = rospy.Publisher("/visualization_init", MarkerArray, queue_size=10) self.new_particles_pub = rospy.Publisher("/particlecloud", PoseArray, queue_size=10) # constants self.number_of_particles = 30 pos_std_dev = 0.25 ori_std_dev = 25 * math.pi / 180 self.initial_std_dev = np.array( [[pos_std_dev, pos_std_dev, ori_std_dev]]).T self.lidar_std_dev = 0.02 self.resample_threshold = 0.1 # changing attributes self.particles = np.ones([3, self.number_of_particles], dtype=np.float) self.weights = np.ones(self.number_of_particles, dtype=np.float) self.odom_tf_time = 0 self.base_tf_time = 0 self.scan = None self.prev_pose = None self.delta_pose = None self.initial_pose_estimate = None self.pose = None # helper classes self.occupancy_field = OccupancyField() self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.transform_helper = TFHelper() rospy.loginfo("Initialized") def lidar_callback(self, msg): # Lidar Subscriber callback function. self.scan = msg.ranges def odom_callback(self, msg): # Odometry Subscriber callback function. x = msg.pose.pose.position.x y = msg.pose.pose.position.y t = euler_from_quaternion([ msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z ])[0] pose = -np.array([[x, y, t]]).T if self.prev_pose is None: self.prev_pose = pose self.pose = pose self.apply_odom_transform() self.plot_particles(self.particles) self.transform_helper.send_last_map_to_odom_transform() def pose_estimate_callback(self, msg): # Rviz pose estimate callback function. position = msg.pose.pose.position orientation = euler_from_quaternion([ msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z ])[0] pose = [position.x, position.y, -orientation - math.pi] self.initial_pose_estimate = np.array([pose]).T def sample_points(self, mean, std): # samples a uniform distribution of points stds = np.repeat(std, self.number_of_particles, 1) return np.random.normal(mean, stds, [3, self.number_of_particles]) def resample_points(self): # Takes the weights of each particle and resamples them according to that weight replace = self.weights < self.resample_threshold kept_weights = np.multiply(replace, self.weights) probs = kept_weights / sum(kept_weights) replace_inds = np.arange(self.weights.size)[replace] if kept_weights.size == 0: rospy.logerr("No particles meet threshold") else: for i in replace_inds: choice = np.random.choice(kept_weights.size, p=probs) weight = kept_weights[choice] self.particles[:, i] = self.particles[:, choice] self.weights[i] = weight def apply_odom_transform(self): # Takes rotation and translation from odom and transforms the particles accordingly. Also adds a bit of noise self.delta_pose = self.pose - self.prev_pose self.prev_pose = self.pose if abs( self.delta_pose[2] ) < 0.5: # this was added to get rid of the orientation wrap around effect delta_std = abs(self.delta_pose) * np.transpose([[1.5, 1.5, 0.9]]) noisy_deltas = self.sample_points(self.delta_pose, delta_std) self.particles = self.particles + noisy_deltas def calc_prob(self): # Reweight particles based on compatibility with laser scan scan = self.scan particles = self.particles for i, p in enumerate(self.particles.T): weight_sum = 0 xs, ys = self.polar_to_cartesian(self.scan, np.radians(range(361)), p[2]) lidar_x = xs + p[0] lidar_y = ys + p[1] # Average the probability associated with each LIDAR reading for x, y in zip(lidar_x[::2], lidar_y[::2]): dist = self.occupancy_field.get_closest_obstacle_distance(x, y) prob = norm(0, self.lidar_std_dev).pdf(dist) / ( 0.4 / self.lidar_std_dev) weight_sum += prob**3 self.weights[i] = weight_sum / len(lidar_x[::2]) def update_transform(self, pose, target_frame='base_laser_link'): # Currently unused, Updates the transform between the map frame and the odom frame if ((rospy.get_rostime() != self.odom_tf_time and target_frame == 'odom') or (rospy.get_rostime() != self.base_tf_time and target_frame == 'base_laser_link')): self.tf_broadcaster.sendTransform( (pose[0], pose[1], 0), quaternion_from_euler(0, 0, pose[2] + math.pi), rospy.get_rostime(), target_frame, 'map') if (target_frame == 'odom'): self.odom_tf_time = rospy.get_rostime() else: self.base_tf_time = rospy.get_rostime() def polar_to_cartesian(self, rs, thetas, theta_offset): # read the function name ok np_rs = np.array(rs) np_thetas = np.array(thetas) + theta_offset xs = np_rs * np.cos(np_thetas) ys = np_rs * np.sin(np_thetas) return xs, ys def plot_particles(self, particles): # plots the particles in the pose array particle cloud topic pose_array = PoseArray() pose_array.header = Header(stamp=rospy.Time.now(), frame_id="map") for i, particle in enumerate(particles.T): w = self.weights[i] * 10 nextPose = Pose() nextPose.position = Point(x=particle[0], y=particle[1], z=0) nextPose.orientation = Quaternion( *quaternion_from_euler(0, 0, particle[2])) pose_array.poses.append(nextPose) self.new_particles_pub.publish(pose_array) def calc_avg_particle(self): # calculates the weighted average position of the particles return np.sum(self.particles * self.weights, axis=1) / sum( self.weights) def main(self): r = rospy.Rate(5) while (not rospy.is_shutdown() and (self.scan is None or self.initial_pose_estimate is None or self.pose is None)): rospy.logwarn( "One of the necessary components has not yet been initialized") r.sleep() self.particles = self.sample_points(self.initial_pose_estimate, self.initial_std_dev) while (not rospy.is_shutdown()): self.calc_prob() best_pose = self.calc_avg_particle() robot_pose = Pose(position=Point(x=best_pose[0], y=best_pose[1]), orientation=Quaternion( *quaternion_from_euler(0, 0, best_pose[2]))) self.transform_helper.fix_map_to_odom_transform( robot_pose, rospy.Time.now()) self.resample_points() r.sleep()
class ParticleFilterNode(object): """ The class that represents a Particle Filter ROS Node """ def __init__(self): rospy.init_node('pf') real_robot = False # create instances of two helper objects that are provided to you # as part of the project self.particle_filter = ParticleFilter() self.occupancy_field = OccupancyField() self.TFHelper = TFHelper() self.sensor_model = sensor_model = SensorModel( model_noise_rate=0.5, odometry_noise_rate=0.15, world_model=self.occupancy_field, TFHelper=self.TFHelper) self.position_delta = None # Pose, delta from current to previous odometry reading self.last_scan = None # list of ranges self.odom = None # Pose, current odometry reading self.x_y_spread = 0.3 # Spread constant for x-y initialization of particles self.z_spread = 0.2 # Spread constant for z initialization of particles self.n_particles = 150 # number of particles # pose_listener responds to selection of a new approximate robot # location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) rospy.Subscriber("odom", Odometry, self.update_position) rospy.Subscriber("stable_scan", LaserScan, self.update_scan) # publisher for the particle cloud for visualizing in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) def update_scan(self, msg): """Updates the scan to the most recent reading""" self.last_scan = [(i, msg.ranges[i]) for i in range(len(msg.ranges))] def update_position(self, msg): """Calculate delta in position since last odometry reading, update current odometry reading""" self.position_delta = Vector3() this_pos = self.TFHelper.convert_pose_to_xy_and_theta(msg.pose.pose) if self.odom is not None: prev_pos = self.TFHelper.convert_pose_to_xy_and_theta(self.odom) self.position_delta.x = this_pos.x - prev_pos.x self.position_delta.y = this_pos.y - prev_pos.y self.position_delta.z = self.TFHelper.angle_diff( this_pos.z, prev_pos.z) else: self.position_delta = this_pos self.odom = msg.pose.pose self.particle_filter.predict(self.position_delta) def reinitialize_particles(self, initial_pose): """Reinitialize particles when a new initial pose is given.""" self.particle_filter.particles = [] for i in range(self.n_particles): pos = Vector3() initial_pose_trans = self.TFHelper.convert_pose_to_xy_and_theta( initial_pose) pos.x = initial_pose_trans.x + (2 * randn() - 1) * self.x_y_spread pos.y = initial_pose_trans.y + (2 * randn() - 1) * self.x_y_spread pos.z = initial_pose_trans.z + (2 * randn() - 1) * self.z_spread new_particle = Particle(position=pos, weight=1 / float(self.n_particles), sensor_model=self.sensor_model) self.particle_filter.add_particle(new_particle) def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ self.reinitialize_particles(msg.pose.pose) self.TFHelper.fix_map_to_odom_transform(msg.pose.pose, msg.header.stamp) def publish_particles(self): """ Extract position from each particle, transform into pose, and publish them as PoseArray""" pose_array = PoseArray() for p in self.particle_filter.particles: pose_array.poses.append( self.TFHelper.convert_vector3_to_pose(p.position)) pose_array.header.frame_id = "map" self.particle_pub.publish(pose_array) def run(self): r = rospy.Rate(5) while not (rospy.is_shutdown()): # in the main loop all we do is continuously broadcast the latest # map to odom transform self.TFHelper.send_last_map_to_odom_transform() if len(self.particle_filter.particles) > 0: if self.last_scan != None: self.particle_filter.integrate_observation(self.last_scan) self.last_scan = None self.particle_filter.normalize() self.publish_particles() self.particle_filter.resample() r.sleep()
class ParticleFilter(object): """ Class to represent Particle Filter ROS Node Subscribes to /initialpose for initial pose estimate Publishes top particle estimate to /particlebest and all particles in cloud to /particlecloud """ def __init__(self): """ __init__ function to create main attributes, setup threshold values, setup rosnode subs and pubs """ rospy.init_node('pf') self.initialized = False self.num_particles = 150 self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.particle_cloud = [] self.lidar_points = [] self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) self.best_particle_pub = rospy.Publisher("particlebest", PoseStamped, queue_size=10) self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.best_guess = ( None, None) # (index of particle with highest weight, its weight) self.particles_to_replace = .075 self.n_effective = 0 # this is a measure of the particle diversity # pose_listener responds to selection of a new approximate robot # location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() # laser_subscriber listens for data from the lidar rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # create instances of two helper objects that are provided to you # as part of the project self.current_odom_xy_theta = [] self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() self.initialized = True def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( msg.pose.pose) print("xy_theta", xy_theta) self.initialize_particle_cloud( msg.header.stamp, xy_theta) # creates particle cloud at position passed in # by message print("INITIALIZING POSE") # Use the helper functions to fix the transform def initialize_particle_cloud(self, timestamp, xy_theta): """ Creates initial particle cloud based on robot pose estimate position """ self.particle_cloud = [] angle_variance = math.pi / 10 # POint the points in the general direction of the robot x_cur = xy_theta[0] y_cur = xy_theta[1] theta_cur = self.transform_helper.angle_normalize(xy_theta[2]) # print("theta_cur: ", theta_cur) for i in range(self.num_particles): # Generate values for and add a new particle!! x_rel = random.uniform(-.3, .3) y_rel = random.uniform(-.3, .3) new_theta = (random.uniform(theta_cur - angle_variance, theta_cur + angle_variance)) # TODO: Could use a tf transform to add x and y in the robot's coordinate system new_particle = Particle(x_cur + x_rel, y_cur + y_rel, new_theta) self.particle_cloud.append(new_particle) print("Done initializing particles") self.normalize_particles() # publish particles (so things like rviz can see them) self.publish_particles() print("normalized correctly") self.update_robot_pose(timestamp) print("updated robot pose") def normalize_particles(self): """ Normalizes particle weights to total but retains weightage """ total_weights = sum([particle.w for particle in self.particle_cloud]) # if your weights aren't normalized then normalize them if total_weights != 1.0: for i in self.particle_cloud: i.w = i.w / total_weights def update_robot_pose(self, timestamp): """ Update the estimate of the robot's pose in the map frame given the updated particles. There are two logical methods for this: (1): compute the mean pose based on all the high weight particles (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() print("Normalized particles in update robot pose") # create average pose for robot pose based on entire particle cloud average_x = 0 average_y = 0 average_theta = 0 # walk through all particles, calculate weighted average for x, y, z, in particle map. for p in self.particle_cloud: average_x += p.x * p.w average_y += p.y * p.w average_theta += p.theta * p.w # # create new particle representing weighted average values, pass in Pose to new robot pose self.robot_pose = Particle(average_x, average_y, average_theta).as_pose() print(timestamp) self.transform_helper.fix_map_to_odom_transform( self.robot_pose, timestamp) print("Done fixing map to odom") def publish_particles(self): """ Publish entire particle cloud as pose array for visualization in RVIZ Also publish the top / best particle based on its weight """ # Convert the particles from xy_theta to pose!! pose_particle_cloud = [] for p in self.particle_cloud: pose_particle_cloud.append(p.as_pose()) self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=pose_particle_cloud)) # doing shit based off best pose best_pose_quat = max(self.particle_cloud, key=attrgetter('w')).as_pose() #self.best_particle_pub.publish(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), pose=best_pose_quat) def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # TODO: FIX noise incorporation into movement. min_travel = 0.2 xy_spread = 0.02 / min_travel # More variance with driving forward theta_spread = .005 / min_travel random_vals_x = np.random.normal(0, abs(delta[0] * xy_spread), self.num_particles) random_vals_y = np.random.normal(0, abs(delta[1] * xy_spread), self.num_particles) random_vals_theta = np.random.normal(0, abs(delta[2] * theta_spread), self.num_particles) for p_num, p in enumerate(self.particle_cloud): # compute phi, or basically the angle from 0 that the particle # needs to be moving - phi equals OG diff angle - robot angle + OG partilce angle # ADD THE NOISE!! noisy_x = (delta[0] + random_vals_x[p_num]) noisy_y = (delta[1] + random_vals_y[p_num]) ang_of_dest = math.atan2(noisy_y, noisy_x) # calculate angle needed to turn in angle_to_dest ang_to_dest = self.transform_helper.angle_diff( self.current_odom_xy_theta[2], ang_of_dest) d = math.sqrt(noisy_x**2 + noisy_y**2) phi = p.theta + ang_to_dest p.x += math.cos(phi) * d p.y += math.sin(phi) * d p.theta += self.transform_helper.angle_normalize( delta[2] + random_vals_theta[p_num]) self.current_odom_xy_theta = new_odom_xy_theta def update_particles_with_laser(self, msg): """ calculate particle weights based off laser scan data passed into param """ # print("Updating particles with Laser") lidar_points = msg.ranges for p_deg, p in enumerate(self.particle_cloud): # do we need to compute particle pos in diff frame? p.occ_scan_mapped = [] # reset list for scan_distance in lidar_points: # handle edge case if scan_distance == 0.0: continue # calc a delta theta and use that to overlay scan data onto the particle headings pt_rad = deg2rad(p_deg) particle_pt_theta = self.transform_helper.angle_normalize( p.theta + pt_rad) particle_pt_x = p.x + math.cos( particle_pt_theta) * scan_distance particle_pt_y = p.y + math.sin( particle_pt_theta) * scan_distance # calculate distance from every single scan point in particle frame occ_value = self.occupancy_field.get_closest_obstacle_distance( particle_pt_x, particle_pt_y) # Think about cutting off max penalty if occ_value is too big p.occ_scan_mapped.append(occ_value) # assign weights based off newly assigned occ_scan_mapped # apply gaussian e**-d**2 to every weight, then cube to emphasize p.occ_scan_mapped = [(math.e / (d)**2) if (d)**2 != 0 else (math.e / (d + .01)**2) for d in p.occ_scan_mapped] p.occ_scan_mapped = [d**3 for d in p.occ_scan_mapped] p.w = sum(p.occ_scan_mapped) #print("Set weight to: ", p.w) p.occ_scan_mapped = [] self.normalize_particles() @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def resample_particles(self): """ Re initialize particles in self.particle_cloud based on preivous weightages. """ weights = [p.w for p in self.particle_cloud] # after calculating all particle weights, we want to calc the n_effective # self.n_effective = 0 self.n_effective = 1 / sum( [w**2 for w in weights]) # higher is more diversity, so less noise print("n_effective: ", self.n_effective) temp_particle_cloud = self.draw_random_sample( self.particle_cloud, weights, int((1 - self.particles_to_replace) * self.num_particles)) # temp_particle_cloud = self.draw_random_sample(self.particle_cloud, weights, self.num_particles) particle_cloud_to_transform = self.draw_random_sample( self.particle_cloud, weights, self.num_particles - int( (1 - self.particles_to_replace) * self.num_particles)) # NOISE POLLUTION - larger noise, smaller # particles # normal_std_xy = .25 normal_std_xy = 10 / self.n_effective # feedback loop? 8,3 normal_std_theta = 3 / self.n_effective # normal_std_theta = math.pi/21 random_vals_x = np.random.normal(0, normal_std_xy, len(particle_cloud_to_transform)) random_vals_y = np.random.normal(0, normal_std_xy, len(particle_cloud_to_transform)) random_vals_theta = np.random.normal(0, normal_std_theta, len(particle_cloud_to_transform)) for p_num, p in enumerate( particle_cloud_to_transform): # add in noise in x,y, theta p.x += random_vals_x[p_num] p.y += random_vals_y[p_num] p.theta += random_vals_theta[p_num] # reset the partilce cloud based on the newly transformed particles self.particle_cloud = temp_particle_cloud + particle_cloud_to_transform def scan_received(self, msg): """ Callback function for recieving laser scan - should pass data into global scan object """ """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, we hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative to the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) # grab from listener & store the the odometry pose in a more convenient format (x,y,theta) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) if not self.current_odom_xy_theta: self.current_odom_xy_theta = new_odom_xy_theta return # Now we've done all calcs, we exit the scan_recieved() method by either initializing a cloud if not (self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud # TODO: Where do we get the xy_theta needed for initialize_particle_cloud? self.initialize_particle_cloud(msg.header.stamp, self.current_odom_xy_theta) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose(msg.header.stamp) # update robot's pose self.resample_particles( ) # resample particles to focus on areas of high density # # publish particles (so things like rviz can see them) self.publish_particles() def run(self): """ main run loop for rosnode """ r = rospy.Rate(5) print("Nathan and Adi ROS Loop code is starting!!!") while not (rospy.is_shutdown()): # in the main loop all we do is continuously broadcast the latest # map to odom transform self.transform_helper.send_last_map_to_odom_transform() r.sleep()
class ParticleFilter(object): """ The class that represents a Particle Filter ROS Node """ def __init__(self): rospy.init_node('pf') # create instances of two helper objects that are provided to you # as part of the project self.occupancy_field = OccupancyField() self.tfh = TFHelper() self.ros_boss = RosBoss() # publisher for the particle cloud for visualizing in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # pose_listener responds to selection of a new approximate robot # location (for instance using rviz) rospy.Subscriber("/odom", Odometry, self.update_initial_pose) self.current_particles = {} self.max_particle_number = 100 self.map_width = self.occupancy_field.map.info.width self.map_height = self.occupancy_field.map.info.height self.map_origin = (self.occupancy_field.map.info.origin.position.x, self.occupancy_field.map.info.origin.position.y) self.map_resolution = self.occupancy_field.map.info.resolution self.particle_viz = ParticlesMarker() self.last_time = time.time() def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ self.tfh.fix_map_to_odom_transform( msg.pose.pose, msg.header.stamp) # Update transform between map and odom. if self.ros_boss.poseGiven(): xy_theta = self.tfh.convert_pose_to_xy_and_theta( self.ros_boss.stamped_pose ) # Get position and orientation from pose if len(self.current_particles ) == 0: #Initial generation of particles self.generate_initial_particles(xy_theta) else: # self.current_particles = {(Particle : probability} threshold = self.thresholdWeight( ) # Calculate threshold weight self.sample_best_particles( threshold ) # self.current_particles = selected best particles. self.generate_probability_dist( ) # "Refills" any missing particles by placing them around the most likely current particles. for particle in self.current_particles.keys(): self.calc_new_weights( particle ) # Calculates new probabilities for all particles (Evaluation) timesince = time.time() - self.last_time self.estimate_future_particles( particle, timesince) # Move particles forward in time self.last_time = time.time() self.normalize_weights( particle, max(self.current_particles.values())) # Update RViz map and particles self.particle_viz.updateParticles( self.current_particles, self.map_origin, self.map_resolution) # Update rviz visualization def generate_initial_particles(self, xy_theta): """ Generate a number of initial particles and add them to the dict. x & y are in meters.""" raw_x, raw_y, theta = xy_theta timenow = rospy.get_rostime() # self.transform_helper.tf_listener.transformPoint("odom", Point, base_point); # + self.transform_helper.translation.y for num in range(self.max_particle_number): # Generate random x,y, theta coordinate for particle, and add as key-value pair to dict of particles [x, y] = np.random.multivariate_normal([raw_x, raw_y], [[0.0, 0.0], [0.0, 0.0]]) theta = np.random.uniform(0.0, 2 * math.pi) # pose = self.tfh.transform(Pose(Point(raw_x, raw_y, 0.0), self.tfh.euler_to_quat(raw_theta))) # x, y, theta = self.tfh.convert_pose_to_xy_and_theta(Pose(pose.pose.position, pose.pose.orientation)) self.current_particles[Particle( x, y, theta)] = (1.0 / self.max_particle_number) self.particle_viz.bestParticle = (list( self.current_particles.items()))[0] return def sample_best_particles(self, threshold): """ Iterates through list of particles and recycles the ones below the threshold. """ # Two step process to avoid changing the dict while iterating through best_particles = { particle: probability for particle, probability in self.current_particles.iteritems() if self.current_particles[particle] <= threshold } self.current_particles = best_particles if len(self.current_particles) == 0: return None return def thresholdWeight(self): """ Picks the threshold value below which particles will be recycled. """ return sum(self.current_particles.values()) / len( self.current_particles.values()) # Average value def generate_probability_dist(self): """ Generates more particles around the existing most likely particles. """ if len(self.current_particles.values()) < self.max_particle_number: num_created_particles = self.max_particle_number - len( self.current_particles.values()) # Iterate through sorted list of particles to get list of best options topProbabilities = dict( sorted(self.current_particles.iteritems(), key=lambda (particle, probability): (particle, probability), reverse=True)[:(len(self.current_particles) % num_created_particles)]) if len(topProbabilities) > 0: # Get best particle (first in sorted dict) best_particle, probability = (list( topProbabilities.items()))[0] # Set value in particle_viz self.particle_viz.bestParticlePose = Pose( Point(best_particle.x, best_particle.y, 0.0), self.tfh.euler_to_quat(best_particle.theta)) # Create new particles. preload = math.floor( len(self.current_particles) / num_created_particles) new_particles = {} index = 0 for particle in self.current_particles.keys(): # if num desired particles is larger than current list of particles, we must create triplicates, etc. for num in range(int(preload)): new_particles[Particle( particle.x, particle.y, particle.theta, particle.distance)] = self.current_particles[particle] # These are the ones that get an extra first, if the number of new particles is not evenly divisible. if (index <= (len(self.current_particles) % num_created_particles)): new_particles[Particle( particle.x, particle.y, particle.theta, particle.distance)] = self.current_particles[particle] index += 1 # Add new dict entries to self.current_particles self.current_particles.update(new_particles) return def calc_new_weights(self, particle): """ Iterates through list of particles and calculates new weights. """ angle_probability = self.tfh.angle_diff(self.ros_boss.minAngle, particle.theta) / (2 * math.pi) distance_probability = abs( self.ros_boss.minDistance - particle.distance ) / self.ros_boss.minDistance # Shortest laser scan - shortest laser scan for current particle. self.current_particles[ particle] = angle_probability + distance_probability return def estimate_future_particles(self, particle, timesince): """ Estimate future particle locations given noise and current speed. Particles are in the map frame. """ particle.distance = math.sqrt( (self.ros_boss.linear_vel.x * timesince)**2 + (self.ros_boss.linear_vel.y * timesince)**2) + np.random.uniform( -0.1, 0.1) particle.x = particle.x + self.ros_boss.linear_vel.x * timesince + np.random.uniform( -0.1, 0.1) # DeltaX = Velx * time + noise particle.y = particle.y + self.ros_boss.linear_vel.y * timesince + np.random.uniform( -0.1, 0.1) return def normalize_weights(self, particle, max_val): """ Normalizes all weights to be between 0 and 1. """ self.current_particles[ particle] = self.current_particles[particle] / max_val return def run(self): r = rospy.Rate(5) while not (rospy.is_shutdown()): # in the main loop all we do is continuously broadcast the latest # map to odom transform self.tfh.send_last_map_to_odom_transform() r.sleep()
class ParticleFilter(object): def __init__(self): rospy.init_node('pf_node') # Initialize subscribers to sensors and motors rospy.Subscriber('/scan', LaserScan, self.read_sensor) # Initialize publishers for visualization self.particle_pose_pub = rospy.Publisher('/particle_pose_array', PoseArray, queue_size=10) self.odom_pose_pub = rospy.Publisher('odom_pose', PoseArray, queue_size=10) self.map_marker_pub = rospy.Publisher('/map_marker', Marker, queue_size=10) # self.particle_obstacles_pub = rospy.Publisher('/particle_obstacles', Marker, queue_size=10) self.latest_scan_ranges = [] # Class initializations self.p_distrib = ParticleDistribution() self.motion_model = MotionModel() self.sensor_model = SensorModel() self.map_model = MapModel() self.tf_helper = TFHelper() # When to run the particle filter self.distance_moved_threshold = 0.2 # m self.angle_turned_threshold = 10 # deg # After map model has been initialized, create the initial particle distribution self.p_distrib.init_particles(self.map_model) self.particle_pose_pub.publish( self.p_distrib.get_particle_pose_array()) ''' Function: read_sensor Inputs: LaserScan scan_msg Save the ranges of the laser scanner. ''' def read_sensor(self, scan_msg): self.latest_scan_ranges = scan_msg.ranges ''' Function: update_pose_estimate Inputs: Returns a new pose estimate after running a particle filter. Called if the robot has moved or turned enough to merit a new pose estimate. The current particles (representing hypothetical poses) are propagated based on the change in odom pose of the robot (how much it has moved since the last estimate). Each new particle is assigned a new weight based on how likely it is to observe the laser scan values. The pose estimate is a weighted average of all of the particle's poses. ''' def update_pose_estimate(self): # Propagate the particles because the robot has moved. The sensor update # should happen on the new poses. # Display the new pose self.odom_pose_pub.publish(self.motion_model.get_pose_array()) self.motion_model.propagate(self.p_distrib.particle_list, self.tf_helper) # self.p_distrib.print_distribution() pose_array = self.p_distrib.get_particle_pose_array() pose_array.header.stamp = rospy.Time(0) self.particle_pose_pub.publish(pose_array) # Update particle weights based on the sensor readings. if (self.latest_scan_ranges != []): scan_ranges = self.latest_scan_ranges # Assuming this will not change the object if we get a new scan. self.sensor_model.update_particle_weights( scan_ranges, self.p_distrib.particle_list, self.map_model) self.p_distrib.normalize_weights() # Resample the particle distribution print("Resample") self.p_distrib.resample() self.p_distrib.normalize_weights() # Display the new distribution pose_array = self.p_distrib.get_particle_pose_array() pose_array.header.stamp = rospy.Time(0) self.particle_pose_pub.publish(pose_array) new_pose_estimate = self.p_distrib.get_pose_estimate( ) # Just Pose, not stamped return new_pose_estimate def publish_map_markers(self): # figure out and publish map origin map_origin_pose = self.map_model.occupancy_field.map.info.origin x = map_origin_pose.position.x y = map_origin_pose.position.y point = Point(x, y, 0.0) quaternion = Quaternion(0, 0, 0, 0) header = Header(frame_id="map", stamp=rospy.Time.now()) pose = map_origin_pose scale = Vector3(.1, .1, .1) color = ColorRGBA(255, 20, 147, 255) type = 2 map_origin_marker = Marker(header=header, pose=Pose(position=point, orientation=quaternion), color=color, type=type, scale=scale) self.map_marker_pub.publish(map_origin_marker) # publish def run(self): # Send the first map to odom transform using the 0, 0, 0 pose. self.tf_helper.fix_map_to_odom_transform(Pose(), rospy.Time(0)) while not rospy.is_shutdown(): # continuously broadcast the latest map to odom transform # Changes to the map to base_link come from our pose estimate from # the particle filter. self.publish_map_markers() self.tf_helper.send_last_map_to_odom_transform() if (self.motion_model.has_moved_enough( self.tf_helper, self.distance_moved_threshold, self.angle_turned_threshold)): # Run the particle filter new_pose_estimate = self.update_pose_estimate() # Update the map to odom transform using new pose estimate self.tf_helper.fix_map_to_odom_transform( new_pose_estimate, rospy.Time(0))
class ParticleFilter(object): """ The class that represents a Particle Filter ROS Node """ def __init__(self): rospy.init_node('pf') # pose_listener responds to selection of a new approximate robot # location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publisher for the particle cloud for visualizing in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # create instances of two helper objects that are provided to you # as part of the project self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() self.particles = Particles() self.particles.initialize_particles() self.ranges = [] def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = \ self.transform_helper.convert_pose_to_xy_and_theta(msg.pose.pose) # TODO this should be deleted before posting self.transform_helper.fix_map_to_odom_transform(msg.pose.pose, msg.header.stamp) # initialize your particle filter based on the xy_theta tuple def add_noise_to_particles(self, position_change): # OUTPUT FORMAT : List[Tuple(x, y, theta, original particle position)] current_particles = self.particles.get_locations() new_particles = [] for particle in current_particles: for i in range(NEW_PARTICLES): x_noise = np.random.normal(loc=0, scale=.25) y_noise = np.random.normal(loc=0, scale=.25) theta_noise = np.random.randint(360) # appending (x, y, theta, original particle position) new_particles.append((particle[0] + position_change.x + x_noise, particle[1] + position_change.y + y_noise, (particle[2] + position_change.z + theta_noise) % 360, particle)) return new_particles def calculate_particle_probs(self, particles): # OUTPUT FORMAT : List[Tuple(cur_loc, prev_loc, confidence weight)] # iterate through particles, determine likelihood of each total_weight = 0 potential_locations = [] for loc_tuple in particles: # get actual measured distance and the map's distance to the closest obstacle measured_distance = self.get_closest_obstacle_from_laserscan()[1] map_distance = self.occupancy_field.get_closest_obstacle_distance(loc_tuple[0], loc_tuple[1]) # basic weight calculator based on measured and map distances if measured_distance == 0 and map_distance == 0: new_weight == 0 elif measured_distance == 0 or map_distance == 0: new_weight == 0 elif measured_distance-map_distance != 0: new_weight = 1/abs(measured_distance-map_distance) else: new_weight = 500 # appending in format (cur_loc, prev_loc, confidence weight) potential_locations.append(((loc_tuple[0], loc_tuple[1], loc_tuple[2]), loc_tuple[3], new_weight)) total_weight += new_weight # normalize the weight to be a probability if total_weight: return [(cur_loc, prev_loc, new_weight / total_weight) for cur_loc, prev_loc, new_weight in potential_locations] return potential_locations def get_closest_obstacle_from_laserscan(self): # this function is used to calculate probabilties of each particle closest_laserscan = (0,0) for i, scan_val in enumerate(self.ranges): if scan_val > 0 and (scan_val < closest_laserscan[1] or closest_laserscan[1] == 0): closest_laserscan = (i, scan_val) return closest_laserscan def run(self): r = rospy.Rate(5) scan_sub = rospy.Subscriber('/scan', LaserScan, self.update_ranges) scan_sub = rospy.Subscriber('/pos_change', Vector3, self.position_update_listener) while not(rospy.is_shutdown()): # in the main loop all we do is continuously broadcast the latest # map to odom transform self.transform_helper.send_last_map_to_odom_transform() r.sleep() def position_update_listener(self, msg): # function serves to update the positions of the particles each time it is called pos_change = msg new_particles = self.add_noise_to_particles(pos_change) potential_locations = self.calculate_particle_probs(new_particles) self.particles.update_locations(potential_locations) def update_ranges(self, msg): # callback function from listener self.ranges = msg.ranges
class NeatoMDP(object): def __init__(self, num_positions=500, num_orientations=10): # TODO: Interface with SLAM algorithm's published map # Initialize map. rospy.init_node( "neato_mdp") # May break if markov_model is also subscribed...? rospy.wait_for_service("static_map") static_map = rospy.ServiceProxy("static_map", GetMap) # Initialize MDP self.mdp = MDP(num_positions=num_positions, num_orientations=num_orientations, map=static_map().map) self.state_idx = None # Current state idx is unknown. self.curr_odom_pose = Pose() self.tf_helper = TFHelper() # Velocity publisher self.cmd_vel_publisher = rospy.Publisher("/cmd_vel", Twist, queue_size=10, latch=True) self.odom_subscriber = rospy.Subscriber('/odom', Odometry, self.set_odom) self.goal_state = None # Visualize robot self.robot_state_pub = rospy.Publisher('/robot_state_marker', Marker, queue_size=10) self.robot_state_pose_pub = rospy.Publisher('/robot_state_pose', PoseArray, queue_size=10) self.goal_state_pub = rospy.Publisher('/goal_state_marker', Marker, queue_size=10) # # pose_listener responds to selection of a new approximate robot location (for instance using rviz) # self.odom_pose = PoseStamped() self.odom_pose.header.stamp = rospy.Time(0) self.odom_pose.header.frame_id = 'odom' # rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) rospy.Subscriber("move_base_simple/goal", PoseStamped, self.update_goal_state) def set_odom(self, msg): self.curr_odom_pose = msg.pose.pose def set_goal_idxs(self): idxs = [] for i, reward in enumerate(self.mdp.rewards): if reward > 0: idxs.append(i) return idxs def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ print("Got an initial pose estimate...?") self.tf_helper.fix_map_to_odom_transform(msg.pose.pose, msg.header.stamp) self.tf_helper.send_last_map_to_odom_transform() # Wait for the transform to really get sent. Otherwise, it complains that it does not exist. rospy.sleep(2.) map_pose = self.tf_helper.tf_listener.transformPose( 'map', self.odom_pose) x, y, theta = self.tf_helper.convert_pose_to_xy_and_theta( map_pose.pose) self.state_idx = self.mdp.markov_model.get_closest_state_idx( State(x=x, y=y, theta=theta)) def update_goal_state(self, msg): map_pose = self.tf_helper.tf_listener.transformPose('map', msg) x, y, theta = self.tf_helper.convert_pose_to_xy_and_theta( map_pose.pose) theta = theta % (2 * math.pi) self.goal_state = State(x=x, y=y, theta=theta) self.goal_state_pub.publish(self.goal_state.get_marker()) def execute_action(self, policy): # Identify which state you are in #TODO: Interface with SLAM algorithm to get predicted position action = Action.get_all_actions()[policy[self.state_idx]] print("Taking action: ", Action.to_str(action)) new_state = self.move(action) # print(new_state) # Update state idx. self.state_idx = self.mdp.markov_model.get_closest_state_idx( new_state, start_state_idx=self.state_idx) def move(self, action): linear, angular = Action.get_pose_change(action) print("linear: {}, angular: {}".format(linear, angular)) twist_msg = Twist() twist_msg.linear.x = 0.25 * linear twist_msg.angular.z = 0.3 * angular # publish this twist message self.cmd_vel_publisher.publish(twist_msg) start_odom_x, start_odom_y, start_odom_theta = self.tf_helper.convert_pose_to_xy_and_theta( self.curr_odom_pose) start_odom_theta = start_odom_theta % (2 * math.pi) linear_change, angular_change = self.get_change_in_motion( start_odom_x, start_odom_y, start_odom_theta) r = rospy.Rate(1) while (action == Action.FORWARD and abs(linear_change) < abs(linear) ) or (action != Action.FORWARD and abs(angular_change) < abs(angular)): map_pose = self.tf_helper.tf_listener.transformPose( 'map', self.odom_pose) x, y, theta = self.tf_helper.convert_pose_to_xy_and_theta( map_pose.pose) theta = theta % (2 * math.pi) new_state = State(x=x, y=y, theta=theta) self.publish_robot_odom(new_state) r.sleep() # Wait a little. # Check change again. linear_change, angular_change = self.get_change_in_motion( start_odom_x, start_odom_y, start_odom_theta) print("Lin change: {} Ang change: {}".format( linear_change, angular_change)) # Convert odom state to map frame. self.odom_pose.pose = self.curr_odom_pose # self.odom_pose.header.stamp = rospy.Time.now() map_pose = self.tf_helper.tf_listener.transformPose( 'map', self.odom_pose) x, y, theta = self.tf_helper.convert_pose_to_xy_and_theta( map_pose.pose) theta = theta % (2 * math.pi) new_state = State(x=x, y=y, theta=theta) self.publish_robot_odom(new_state) print("New state is: ") print(new_state) return new_state def stop(self): twist_msg = Twist() self.cmd_vel_publisher.publish(twist_msg) def publish_robot_odom(self, curr_state): self.robot_state_pub.publish( curr_state.get_marker(r=0.0, g=0.0, b=1.0, scale=0.15)) robot_pose = PoseArray() robot_pose.header.frame_id = "map" robot_pose.poses = [curr_state.get_pose()] self.robot_state_pose_pub.publish(robot_pose) def get_change_in_motion(self, start_odom_x, start_odom_y, start_odom_theta): new_odom_x, new_odom_y, new_odom_theta = self.tf_helper.convert_pose_to_xy_and_theta( self.curr_odom_pose) new_odom_theta = new_odom_theta % (2 * math.pi) return (new_odom_x - start_odom_x, self.tf_helper.angle_diff(new_odom_theta, start_odom_theta)) def run(self): # TODO: Parametrize goal state, and be able to dynamically update it? # Set an initial goal state. # goal_state = State(x=1, y=1, theta=math.radians(40)) # Solve the MDP while self.goal_state == None: continue policy, iter, time = self.mdp.get_policy(self.goal_state) self.mdp.visualize_policy(policy, self.goal_state) goal_idxs = self.set_goal_idxs() print(goal_idxs) r = rospy.Rate(0.5) while not rospy.is_shutdown(): if (self.state_idx == None): # print("Current state is unknown. Cannot execute policy.") continue else: # Keep checking whether you are in a different state and should # execute a different action. if self.state_idx in goal_idxs: self.stop() print( "Finished executing policy. Would you like to go again?" ) if (raw_input() in ['n', 'NO', 'N', 'no']): print("Exiting") break else: print("Enter goal state: ") goal_state_idx = input() self.goal_state = self.mdp.markov_model.model_states[ goal_state_idx] # Visualize the goal state as sphere. self.goal_state_pub.publish( self.mdp.markov_model.model_states[goal_state_idx]. get_marker()) # Solve the MDP policy, iter, time = self.mdp.get_policy( self.goal_state) goal_idxs = self.set_goal_idxs() else: self.execute_action(policy)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initialization is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node( 'pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 150 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model self.radius = 2 # ac 109_1 #self.radius = 1 # ac_109_2 self.num_best_particles = 5 # ac 109_1 # self.num_best_particles = 8 # ac 109_2 # standard deviation of random noise distribution (Gaussian) for updating particle with odom # self.sigma_random_noise_update_odom = 0.01 # parameter for trying to not require a good initial estimate self.sigma_random_noise_update_odom = 0.008 # standard deviation of p(z^k_t | x_t, map) self.sigma_hit_update_scan = 0.01 #self.sigma_hit_update_scan = 0.05 # parameter for trying to not require a good initial estimate (did not work) self.z_hit = 1 self.z_rand = 0 # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # laser_subscriber listens for data from the lidar rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] # change use_projected_stable_scan to True to use point clouds instead of laser scans self.use_projected_stable_scan = False self.last_projected_stable_scan = None if self.use_projected_stable_scan: # subscriber to the odom point cloud rospy.Subscriber("projected_stable_scan", PointCloud, self.projected_scan_received) self.current_odom_xy_theta = [] self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() self.initialized = True def update_robot_pose(self, timestamp): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() # just to get started we will fix the robot's pose to always be at the origin # self.robot_pose = Pose() x_sum = 0 y_sum = 0 theta_sum = 0 # Take the average of the best particles to be the robot's pose estimate particles_most_likely = sorted(self.particle_cloud, key=lambda x: x.w, reverse=True) for p in particles_most_likely[0:self.num_best_particles]: x_sum += p.x y_sum += p.y theta_sum += p.theta # should not do this, for some reason messed up the yaw # theta_sin_sum += math.sin(p.theta) # theta_cos_sum += math.cos(p.theta) x_avg = x_sum / self.num_best_particles y_avg = y_sum / self.num_best_particles theta_avg = theta_sum / self.num_best_particles # theta_avg = math.atan2(theta_sin_sum, theta_cos_sum) / self.num_best_particles (this is bad) mean_pose = Particle(x_avg, y_avg, theta_avg) self.robot_pose = mean_pose.as_pose() self.transform_helper.fix_map_to_odom_transform( self.robot_pose, timestamp) def projected_scan_received(self, msg): self.last_projected_stable_scan = msg def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # Update the odom of the particles accordingly d = math.sqrt(delta[0]**2 + delta[1]**2) for p in self.particle_cloud: p.x += d * math.cos(p.theta) + normal( 0, self.sigma_random_noise_update_odom) p.y += d * math.sin(p.theta) + normal( 0, self.sigma_random_noise_update_odom) p.theta += delta[2] + normal(0, self.sigma_random_noise_update_odom) def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized self.normalize_particles() # Sort the particles first particles_most_likely = sorted(self.particle_cloud, key=lambda x: x.w, reverse=True) new_particle_cloud = [] # Low variance resampler from Probabilistic Robotics p87 count_inv = 1.0 / self.n_particles # the case when particles have equal weights r_num = random.uniform( 0, 1) * count_inv # draw a number in the interval [0,1/M] for m in range(self.n_particles): # Repeatedly add fixed amount to 1/M to random number r where 1/M represents the case where particles have # equal weights u = r_num + m * count_inv c = 0 # cumulative weights of particles for particle in particles_most_likely: c += particle.w # Add the first particle i such that the sum of weights of all particles from 0->i >= u if c >= u: new_particle_cloud.append(deepcopy(particle)) break self.particle_cloud = new_particle_cloud def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ scans = msg.ranges for particle in self.particle_cloud: total_prob = 1 for angle, scan in enumerate(scans): if not math.isinf(scan): # convert scan measurement from view of the particle to map x_scan = particle.x + scan * math.cos(particle.theta + math.radians(angle)) y_scan = particle.y + scan * math.sin(particle.theta + math.radians(angle)) d = self.occupancy_field.get_closest_obstacle_distance( x_scan, y_scan) # Compute p(z^k_t | x_t, map) p_z_hit = self.z_hit * math.exp( -d**2 / (2 * (self.sigma_hit_update_scan**2)) ) / (self.sigma_hit_update_scan * math.sqrt(2 * math.pi)) p_z_rand = self.z_rand / self.laser_max_distance # z_random / z_max Probabilistic Robotics p143 p_z = p_z_hit + p_z_rand # We sum the cube of the probability # total_prob += p_z ** 3 # trying to make the model not require a good initial estimate total_prob += p_z**6 # total_prob = total_prob/len(msg.ranges) # It works better not to average -> converge faster # Reassign weight with newly computed p(z_t | x_t, map) particle.w = total_prob self.normalize_particles() @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( msg.pose.pose) self.initialize_particle_cloud(msg.header.stamp, xy_theta) def initialize_particle_cloud(self, timestamp, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is omitted, the odometry will be used """ if xy_theta is None: xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) self.particle_cloud = [] self.particle_cloud.append( Particle(xy_theta[0], xy_theta[1], xy_theta[2])) for p in range(self.n_particles - 1): p_yaw = random.uniform(0, 2 * math.pi) radius = random.uniform(0, 1) * self.radius theta_facing = random.uniform(0, 2 * math.pi) # Forward x axis of Neato is x p_x = radius * math.sin(theta_facing) + xy_theta[0] p_y = radius * math.cos(theta_facing) + xy_theta[1] self.particle_cloud.append(Particle(p_x, p_y, p_yaw)) self.normalize_particles() self.update_robot_pose(timestamp) def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ total_weight = 0 for p in self.particle_cloud: total_weight += p.w for p in self.particle_cloud: p.w = p.w / total_weight def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, we hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return # wait a little while to see if the transform becomes available. This fixes a race # condition where the scan would arrive a little bit before the odom to base_link transform # was updated. self.tf_listener.waitForTransform(self.base_frame, self.odom_frame, msg.header.stamp, rospy.Duration(0.5)) if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative to the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) if not self.current_odom_xy_theta: self.current_odom_xy_theta = new_odom_xy_theta return if not (self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud(msg.header.stamp) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry if self.last_projected_stable_scan: last_projected_scan_timeshift = deepcopy( self.last_projected_stable_scan) last_projected_scan_timeshift.header.stamp = msg.header.stamp self.scan_in_base_link = self.tf_listener.transformPointCloud( "base_link", last_projected_scan_timeshift) self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose(msg.header.stamp) # update robot's pose self.resample_particles( ) # resample particles to focus on areas of high density # publish particles (so things like rviz can see them) self.publish_particles(msg)
class ParticleFilter(object): """particle filtering of a neato's location in a known map""" def __init__(self): """initialize node, ROS things, etc.""" rospy.init_node("ParticleFilter") rospy.Subscriber('/scan', LaserScan, self.process_scan) self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) self.pub2 = rospy.Publisher('Robot_marker', Marker, queue_size=10) self.pub3 = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10) self.particle_pub = rospy.Publisher("particle_cloud", PointCloud, queue_size=10) rospy.Subscriber('/odom', Odometry, self.read_pos) self.rate = rospy.Rate(20) self.xs_bl = [] # list of xs from lidar in base link frame self.ys_bl = [] # list of ys from lidar in base link frame self.scanx_now = [] self.scany_now = [] self.last_pose = None self.pose = None #as a Twist self.orientation = None #as a quaternion self.tf = TFHelper() self.real_pose = None self.particle_array = [] self.num_particles = 500 #adjust to increase number of particles self.update_rate = .5 #in seconds self.sd_filter = self.num_particles * 3.0 / ( 4) #standard deviation for random sampling self.current_pose = (0, 0, 0) self.previous_pose = (0, 0, 0) self.previous_time = rospy.get_time() self.current_time = rospy.get_time() self.cloud_array = PointCloud( ) #point cloud for laser scan visualization self.x_array = [] self.y_array = [] self.initialize_pf() print('Init complete') def process_scan(self, message): """take in scan data, returns x and y values in baselink ref frameself. omits points with 0 range value.""" ranges = message.ranges xs = [] ys = [] for i in range(len(ranges)): if ranges[i] != 0: theta = math.radians(i + 90) r = ranges[i] xf = math.cos(theta) * r yf = math.sin(theta) * r xs.append(xf) ys.append(yf) # store vals in pf object attributes self.xs_bl = xs self.ys_bl = ys def initialize_pf(self): """intialize the particle filter, called once""" for i in range(self.num_particles): #randomize x and y positions in a 5x5 meter square x_pos = (random.randint(1, 100) - 50) / 10.0 y_pos = (random.randint(1, 100) - 50) / 10.0 theta = random.randint(1, 360) #randomize initial theta particle = Particle(x_pos, y_pos, theta, i) self.particle_array.append(particle) def get_pose(self): """gets current pose based on motor model""" return self.real_pose def create_cloud_array(self, x_array, y_array): "turns x and y laser points into cloud_array" cloud_array = [] for i in range(len(x_array)): x = x_array[i] y = y_array[i] z = 0 point = Point32() point.x = x point.y = y point.z = 0 cloud_array.append(point) self.cloud_array.header.frame_id = 'map' self.cloud_array.header.stamp = rospy.Time.now() self.cloud_array.points = cloud_array def map_odom_transform(self, pose): "converts map to odom coordinates" self.tf.fix_map_to_odom_transform(pose, rospy.Time.now()) def update_markers(self): "updates all particle markers - only show the first 10" self.markerArray = MarkerArray() #if we have particles if (self.particle_array != None and len(self.particle_array) > 0): for i in range(10): particle = self.particle_array[i] x_pos = particle.x y_pos = particle.y #convert particle angle to marker quaternian quaternion = self.tf.convert_angle_to_quaternion(particle.w) self.create_particle_marker(x_pos, y_pos, quaternion) self.marker.id = particle.id #for the highest weight particle, change color and size of arrow if (particle.id == 0): self.marker.color.b = 1 self.marker.color.g = 0 self.marker.scale.x = 1 #map robot marker to particle with highest weight self.create_robot_marker(x_pos, y_pos, quaternion) self.markerArray.markers.append(self.marker) def sort_particles_by_weight(self): "sorts particles by their weight, normalizes weight, and id is reset" #sorts particles by weight self.particle_array.sort(key=lambda x: x.weight, reverse=True) #gets total weight of particles total_weight = sum(particle.weight for particle in self.particle_array) if (total_weight > 0): #normalizes weight and resets particle id for i, particle in enumerate(self.particle_array): particle.weight = particle.weight / total_weight particle.id = i #gets first particle laser scan if (len(self.particle_array) != None): self.x_array = self.particle_array[0].x_map self.y_array = self.particle_array[0].y_map def resample_particles(self, dx, dy, dtheta): "takes current odom reading and updates particles with noise" if (self.particle_array != None and len(self.particle_array) > 0): new_array = [] #sd weight increases standard deviation if weight is lower sd_weight = 1 - self.particle_array[0].weight**(1 / 4) ran = False for i in range(self.num_particles): sample_id = self.num_particles #sample_id is a randomly choosen particle with a distribution while (sample_id >= self.num_particles): sample = np.random.normal(loc=0.0, scale=sd_weight * self.sd_filter) sample_id = int(abs(sample)) if (not ran): sample_id = 0 #moves particle with reference to the odom movement angle_bot = math.degrees(math.atan2(dy, dx)) dist = math.sqrt(dx**2 + dy**2) current_angle = self.particle_array[sample_id].w moved_theta = self.particle_array[sample_id].w + dtheta cos_theta = math.cos(math.radians(current_angle)) sin_theta = math.sin(math.radians(current_angle)) moved_x = self.particle_array[sample_id].x + dist * cos_theta moved_y = self.particle_array[sample_id].y + dist * sin_theta #after moving particle, adds noise to particle if (ran): noisy_particle = self.particle_array[ sample_id].return_particle_with_noise( moved_x, moved_y, moved_theta, i, sd_weight) else: noisy_particle = self.particle_array[sample_id] ran = True new_array.append(noisy_particle) self.particle_array = new_array def create_particle_marker(self, x, y, quaternion): "creates marker with position x,y" self.marker = Marker() self.marker.header.frame_id = "map" self.marker.type = self.marker.ARROW self.marker.action = self.marker.ADD self.marker.pose.position.x = x self.marker.pose.position.y = y self.marker.pose.orientation.x = quaternion[0] self.marker.pose.orientation.y = quaternion[1] self.marker.pose.orientation.z = quaternion[2] self.marker.pose.orientation.w = quaternion[3] scale = .15 self.marker.scale.x = .5 #scale self.marker.scale.y = .05 self.marker.scale.z = .05 self.marker.color.a = 1 self.marker.color.g = 1 def read_pos(self, data): "reads position of robot" self.pose = data.pose.pose self.real_pose = self.tf.convert_pose_to_xy_and_theta(self.pose) x = self.real_pose[0] y = self.real_pose[1] theta = math.degrees(self.real_pose[2]) self.real_pose = (x, y, theta) def create_robot_marker(self, x_pos, y_pos, quaternion): "creates the robot marker" self.robot_marker = Marker() self.robot_marker.header.frame_id = "map" self.robot_marker.type = self.robot_marker.CUBE self.robot_marker.pose.position.x = x_pos self.robot_marker.pose.position.y = y_pos self.marker.pose.orientation.x = quaternion[0] self.marker.pose.orientation.y = quaternion[1] self.marker.pose.orientation.z = quaternion[2] self.marker.pose.orientation.w = quaternion[3] scale = .25 self.robot_marker.scale.x = scale self.robot_marker.scale.y = scale self.robot_marker.scale.z = scale self.robot_marker.color.a = 1 self.robot_marker.color.b = 1 def run_points(self): """runs all the important functions for point cloud evaluation and propogation.""" for particle in self.particle_array: particle.particles_to_map(self.scanx_now, self.scany_now) particle.get_particle_weight() def run(self): "runs particle filter" while not rospy.is_shutdown(): if (self.xs_bl != None and self.pose != None and self.real_pose != None): self.current_time = rospy.get_time() if (self.current_time - self.previous_time > self.update_rate): self.current_pose = self.get_pose() dx = self.current_pose[0] - self.previous_pose[0] dy = self.current_pose[1] - self.previous_pose[1] dtheta = self.current_pose[2] - self.previous_pose[2] self.resample_particles(dx, dy, dtheta) self.previous_time = rospy.get_time() self.previous_pose = self.get_pose() self.scanx_now = self.xs_bl self.scany_now = self.ys_bl self.run_points() self.sort_particles_by_weight() self.update_markers() self.tf.send_last_map_to_odom_transform() self.map_odom_transform(self.pose) self.create_cloud_array(self.x_array, self.y_array) #publish things self.particle_pub.publish(self.cloud_array) self.pub2.publish(self.robot_marker) self.pub3.publish(self.markerArray) self.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 = 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 ParticleErrorValidation(object): def __init__(self): rospy.init_node('error_validation_node') # Initialize subscribers to sensors and motors rospy.Subscriber('/scan', LaserScan, self.read_sensor) # Initialize publishers for visualization self.error_markers_pub = rospy.Publisher('/error_markers', MarkerArray, queue_size=10) self.odom_pose_pub = rospy.Publisher('odom_particle_pose', Pose, queue_size=10) self.latest_scan_ranges = [] # pose_listener responds to selection of a new approximate robot # location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # Class initializations self.map_model = MapModel() self.tf_helper = TFHelper() self.motion_model = MotionModel() self.sensor_model = SensorModel() """for static error validation""" self.static_particle = Particle(x=0, y=0, theta=0, weight=1) self.sample_ranges = np.ones(361) self.predicted_obstacle_x = 0.0 self.predicted_obstacle_y = 0.0 def read_sensor(self, scan_msg): self.latest_scan_ranges = scan_msg.ranges def update_particle_pose(self): particle_pose = self.motion_model.last_odom_pose.pose print(particle_pose.position.x, particle_pose.position.y) self.odom_pose_pub.publish(particle_pose) def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = \ self.tf_helper.convert_pose_to_xy_and_theta(msg.pose.pose) # TODO this should be deleted before posting self.tf_helper.fix_map_to_odom_transform(msg.pose.pose, msg.header.stamp) # initialize your particle filter based on the xy_theta tuple def get_predicted_obstacle_error(self, distance_reading, x, y, angle): # Predict location of objects based on laser scan reading. self.predicted_obstacle_x, self.predicted_obstacle_y = self.move_coordinate( x, y, angle, distance_reading) # Find the closest obstacle to the predicted obstacle position predicted_reading = self.map_model.occupancy_field.get_closest_obstacle_distance( self.predicted_obstacle_x, self.predicted_obstacle_y) #print("Predicted x: {}, y: {}, reading: {}".format(predicted_obstacle_x, predicted_obstacle_y, predicted_reading)) return 2 * predicted_reading def move_coordinate(self, x, y, angle, distance): return (x + cos(radians(angle)) * distance, y + sin(radians(angle)) * distance) def publish_static_marker_array(self): # universal constraints header = Header(frame_id="map", stamp=rospy.Time.now()) type = 2 # publish # particle marker: RED particle_x = self.static_particle.x particle_y = self.static_particle.y particle_point = Point(particle_x, particle_y, 0.0) particle_scale = Vector3(.1, .1, .1) particle_color = ColorRGBA(255, 0, 0, 1) # error constants error_color = ColorRGBA(0, 0, 255, .1) # predicted marker constants (same as error marker but smaller and diff color): GREEN predicted_scale = Vector3(.1, .1, .1) predicted_color = ColorRGBA(0, 255, 0, 1) # create markers x = 0 marker_array = MarkerArray() while x < 360: self.particle.theta = x # error marker: BLUE error_dist = self.get_predicted_obstacle_error( self.sample_ranges[x], particle_x, particle_y, self.particle.theta) error_x = self.predicted_obstacle_x error_y = self.predicted_obstacle_y error_point = Point(error_x, error_y, 0.0) error_scale = Vector3(error_dist, error_dist, error_dist) # create markers particle_id = x particle_marker = Marker(header=header, pose=Pose(position=particle_point), color=particle_color, type=type, scale=particle_scale, id=particle_id) error_id = x + 361 error_marker = Marker(header=header, pose=Pose(position=error_point), color=error_color, type=type, scale=error_scale, id=x + error_id) predicted_id = x + 722 predicted_marker = Marker(header=header, pose=Pose(position=error_point), color=predicted_color, type=type, scale=predicted_scale, id=predicted_id) marker_array.markers.append(predicted_marker) marker_array.markers.append(particle_marker) marker_array.markers.append(error_marker) x += 90 #print(len(marker_array)) # publish self.error_markers_pub.publish(marker_array) def run(self): # Send the first map to odom transform using the 0, 0, 0 pose. self.tf_helper.fix_map_to_odom_transform(Pose(), rospy.Time(0)) while not rospy.is_shutdown(): # continuously broadcast the latest map to odom transform # Changes to the map to base_link come from our pose estimate from # the particle filter. self.update_particle_pose() self.tf_helper.send_last_map_to_odom_transform()
class RobotLocalizer(object): """ doc """ def __init__(self): print("init RobotLocalizer") rospy.init_node('localizer') self.tfHelper = TFHelper() self.xs = None self.ys = None self.ranges = [] # Lidar scan self.last_odom_msg = None print(self.last_odom_msg) self.diff_transform = { 'translation': None, 'rotation': None, } self.odom_changed = False # Toggles to True when the odom frame has changed enough # subscribers and publisher self.laser_sub = rospy.Subscriber('/scan', LaserScan, self.process_scan) self.odom_sub = rospy.Subscriber("/odom", Odometry, self.update_odom) ### Used for the particle filter # publisher for the particle cloud for visualizing in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # publisher for the top weighted particle self.topparticle_pub = rospy.Publisher("topparticle", PoseArray, queue_size=10) # publisher for rviz markers self.pub_markers = rospy.Publisher('/visualization_markerarray', MarkerArray, queue_size=10) self.particle_filter = ParticleFilter(self.topparticle_pub, self.particle_pub, self.pub_markers) print("ParticleFilter initialized") print("RobotLocalizer initialized") def update_odom(self, msg): MIN_TRAVEL_DISANCE = 0.1 MIN_TRAVEL_ANGLE = math.radians(5) if self.last_odom_msg is None: self.last_odom_msg = msg return last_xyt = self.tfHelper.convert_pose_to_xy_and_theta( self.last_odom_msg.pose.pose) current_xyt = self.tfHelper.convert_pose_to_xy_and_theta(msg.pose.pose) # Get translation in odom translation = [ current_xyt[0] - last_xyt[0], # x current_xyt[1] - last_xyt[1], # y ] # rotate to vehicle frame translation = self.tfHelper.rotate_2d_vector( translation, -1 * last_xyt[2]) # negative angle to counter rotation # get orientation diff theta = self.tfHelper.angle_diff(current_xyt[2], last_xyt[2]) # Schedule to update particle filter if there's enough change distance_travelled = math.sqrt(translation[0]**2 + translation[1]**2) if distance_travelled > MIN_TRAVEL_DISANCE or theta > MIN_TRAVEL_ANGLE: # TODO(matt): consider using actual transform # last_to_current_transform = self.tfHelper.convert_translation_rotation_to_pose( # translation, self.tfHelper.convert_theta_to_quaternion(theta) # ) print("travelled: {}".format(distance_travelled)) last_to_current_transform = { 'translation': translation, 'rotation': theta, } print("transform\n x: {}\n y: {}".format( last_to_current_transform['translation'][0], last_to_current_transform['translation'][1])) self.diff_transform = last_to_current_transform self.last_odom_msg = msg self.odom_changed = True def process_scan(self, m): """Storing lidar data """ #TODO: self.ranges = m.ranges xs = [] ys = [] for i in range(len(self.ranges)): if self.ranges[i] != 0: theta = math.radians(i) r = self.ranges[i] xf = math.cos(theta) * r yf = math.sin(theta) * r xs.append(xf) ys.append(yf) self.xs = xs self.ys = ys def get_x_directions(self, x): interval = 360 / x angle = 0 directions = [] for i in range(x): dist = self.ranges[angle] directions.append((math.radians(angle), dist)) angle = angle + interval return directions def gen_neighbor_particles(self): """Generates particles around given points""" #TODO: pass def find_all_nearest_objects(self): """Determines nearest non-zero point of all point (loops through)""" #TODO: pass def get_encoder_value(self): """Records odom movement, translate to x, y, and theta""" #TODO: pass """ Functions to write or figure out where they are: Order of particle filter: 1. DONE generate initial 500 random particles 2. DONE get ranges from robot -determine 8 values for directions -find lowest distance to obstacle 3. Process particles - project lowest distance from robot onto each particle -DONE for each particle get nearest object -> error distance -DONE 1/error distance = particle.weight 4. DONE publish particle with highest weight 5. DONE resample particles based on weight 6. DONE move robot - get transform 7. DONE transform resampled points with randomness """ def run(self): # save odom position (Odom or TF Module) # self.generate_random_points() NUM_DIRECTIONS = 8 self.particle_filter.gen_init_particles() # # Get lidar readings in x directions # robo_pts = self.get_x_directions(NUM_DIRECTIONS) # # For each particle compare lidar scan with map # self.particle_filter.compare_points(robo_pts) # # # Publish best guessself.particle_filter.gen_init_particles() # self.particle_filter.publish_top_particle(self.topparticle_pub) # # # Resample particles # self.particle_filter.resample_particles() # # # Publish cloud # self.particle_filter.publish_particle_cloud(self.particle_pub) r = rospy.Rate(5) while not (rospy.is_shutdown()): self.particle_filter.gauge_particle_position() if (self.odom_changed): print("Odom changed, let's do some stuff") # Get lidar readings in x directions robo_pts = self.get_x_directions(NUM_DIRECTIONS) # Update particle poses self.particle_filter.update_all_particles(self.diff_transform) # Display new markers self.particle_filter.draw_markerArray() # For each particle compare lidar scan with map self.particle_filter.compare_points(robo_pts) # Publish best guess self.particle_filter.gen_init_particles() top_particle_pose = self.particle_filter.publish_top_particle() self.tfHelper.fix_map_to_odom_transform( top_particle_pose, rospy.Time(0)) # TODO: Move? # Resample particles self.particle_filter.resample_particles() # Publish cloud self.particle_filter.publish_particle_cloud() # Wait until robot moves enough again self.odom_changed = False self.tfHelper.send_last_map_to_odom_transform() r.sleep()
class ParticleFilter(object): """ The class that represents a Particle Filter ROS Node """ def __init__(self, top_particle_pub, particle_cloud_pub, particle_cloud_marker_pub): # # pose_listener responds to selection of a new approximate robot # # location (for instance using rviz) # rospy.Subscriber("initialpose", # PoseWithCovarianceStamped, # self.update_initial_pose) self.top_particle_pub = top_particle_pub self.particle_cloud_pub = particle_cloud_pub self.particle_cloud_marker_pub = particle_cloud_marker_pub # create instances of two helper objects that are provided to you # as part of the project self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() self.particles = [] self.markerArray = MarkerArray() def gauge_particle_position(self): xs = [particle.x for particle in self.particles] ys = [particle.y for particle in self.particles] print("min x: {} --- average x: {} --- max x: {} \nmin y: {} --- average y: {} --- max y: {}".format( min(xs), sum(xs)/len(xs), max(xs), min(ys), sum(ys)/len(ys), max(ys) )) def get_marker(self, x, y): marker = Marker() marker.header.frame_id = "base_link" marker.type = marker.SPHERE marker.pose.position.x = x marker.pose.position.y = y marker.pose.position.z = 0 marker.scale.x = 0.3 marker.scale.y = 0.3 marker.scale.z = 0.3 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 return marker def draw_markerArray(self): markerArray = MarkerArray() for p in self.particles: m = self.get_marker(p.x, p.y) markerArray.markers.append(m) self.particle_cloud_marker_pub.publish(markerArray) def publish_particle_cloud(self): msg = PoseArray() msg.header.frame_id = "map" # Make pose from particle for all particles msg.poses = [particle.get_pose() for particle in self.particles] # Publish self.particle_cloud_pub.publish(msg) def publish_top_particle(self): msg = PoseArray() top_particle = self.particles[0] for particle in self.particles: if particle.weight > top_particle.weight: top_particle = particle msg.poses.append(top_particle.get_pose()) # print(msg) self.top_particle_pub.publish(msg) return top_particle.get_pose() def gen_init_particles(self): """Generating random particles with x, y, and t values""" # width = self.occupancy_field.map.info.width # height = self.occupancy_field.map.info.height width = 10 height = 10 print("map width: {}, height: {}".format(width, height)) for i in range(500): # x = r.randrange(0,width) # y = r.randrange(0,height) x = r.uniform(-5, 5) y = r.uniform(-5, 5) t = math.radians(r.randrange(0,360)) p = Particle(x,y,t) self.particles.append(p) def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = \ self.transform_helper.convert_pose_to_xy_and_theta(msg.pose.pose) # TODO this should be deleted before posting self.transform_helper.fix_map_to_odom_transform(msg.pose.pose, msg.header.stamp) # initialize your particle filter based on the xy_theta tuple def resample_particles(self): """Resample particles with replacement.""" if len(self.particles): weights = [particle.weight if not math.isnan(particle.weight) else 0.0001 for particle in self.particles] total_weight = sum(weights) weights = [weight / total_weight for weight in weights] before = time.time() self.particles = [particle.deep_copy() for particle in list(np.random.choice( # self.particles = [particle for particle in list(np.random.choice( self.particles, size=len(self.particles), replace=True, p=weights, ))] after = time.time() print("************************************timer: {}".format(after-before)) print("number of particles: {}".format(len(self.particles))) else: print("No particles to resample from") return None def update_all_particles(self, transform): for particle in self.particles: # self.update_particle(particle, transform) self.update_particle_with_randomness(particle, transform) def update_particle_with_randomness(self, particle, transform): # TODO(matt): Make this a tunable param DISTANCE_VAR_SCALE = 0.001 ANGLE_VAR_SCALE = math.radians(0.5) # NOTE: We scale the variance instead of the standard deviation because # that makes it independent of the update time (the noise in one update # will be the same as the sum of the noise in two updates) distance = math.sqrt(transform['translation'][0]**2 + transform['translation'][1]**2) translation_mean, translation_var = 0, DISTANCE_VAR_SCALE #* distance # scale with magnitude rotation_mean, rotation_var = 0, ANGLE_VAR_SCALE modified_transform = transform # modified_transform['translation'][0] += float(np.random.normal(translation_mean, math.sqrt(translation_var), 1)) # modified_transform['translation'][1] += float(np.random.normal(translation_mean, math.sqrt(translation_var), 1)) # modified_transform['rotation'] += float(np.random.normal(rotation_mean, math.sqrt(rotation_var))) modified_transform['translation'][0] += float(np.random.uniform(-0.01, 0.01, 1)) modified_transform['translation'][1] += float(np.random.uniform(-0.01, 0.01, 1)) modified_transform['rotation'] += float(np.random.uniform(-math.radians(5), -math.radians(5), 1)) self.update_particle(particle, modified_transform) def update_particle(self, particle, transform): # rotate translation in the particle's direction particle_translation = self.transform_helper.rotate_2d_vector(transform['translation'], particle.theta) particle.translate(particle_translation) particle.rotate(transform['rotation']) # def compare_points(self): # """Compares translated particle to lidar scans, returns weights values""" # distances = [] # errordis = 0 # for a in range(500): # particle.ParticleCloud(self.particle[a]) # for b in range(8): # d[b] = OccupancyField.get_closest_obstacle_distance(particle.ParticleCloud[b][1],particle.ParticleCloud[b][2]) # particle.Particle.weight = 1 / (sum(d) + .01) def compare_points(self, robo_pts): """ This function determines the weights for each particle. Args: robo_pts (list): is a list of lidar readings for n directions. It can be obtained by calling get_x_directions in robot localizerself. """ for p in self.particles: p_cloud = ParticleCloud(p) p_cloud.generate_points(robo_pts) d = [] for pt in p_cloud.pts: if pt[1] != 0: d dist = abs(self.occupancy_field.get_closest_obstacle_distance(pt[0],pt[1])) d.append(math.exp(-d**2/1)) p.weight = 1 / (sum(d) + .01) def run(self): pass
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node( 'pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 600 # the number of particles to use self.particle_init_options = ParticleInitOptions.UNIFORM_DISTRIBUTION self.d_thresh = 0.1 # the amount of linear movement before performing an update self.a_thresh = math.pi / 12.0 # the amount of angular movement before performing an update self.num_lidar_points = 180 # int from 1 to 360 # Note: self.laser_max_distance is NOT implemented # TODO: Experiment with setting a max acceptable distance for lidar scans self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # laser_subscriber listens for data from the lidar rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received, queue_size=1) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # publish our hypotheses points self.hypothesis_pub = rospy.Publisher("hypotheses", MarkerArray, queue_size=10) # Publish our hypothesis points right before they get udpated through odom self.before_odom_hypothesis_pub = rospy.Publisher( "before_odom_hypotheses", MarkerArray, queue_size=10) # Publish where the hypothesis points will be once they're updated with the odometry self.future_hypothesis_pub = rospy.Publisher("future_hypotheses", MarkerArray, queue_size=10) # Publish the lidar scan that pf.py sees self.lidar_pub = rospy.Publisher("lidar_visualization", MarkerArray, queue_size=1) # Publish the lidar scan projected from the first hypothesis self.projected_lidar_pub = rospy.Publisher( "projected_lidar_visualization", MarkerArray, queue_size=1) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] # change use_projected_stable_scan to True to use point clouds instead of laser scans self.use_projected_stable_scan = False self.last_projected_stable_scan = None if self.use_projected_stable_scan: # subscriber to the odom point cloud rospy.Subscriber("projected_stable_scan", PointCloud, self.projected_scan_received) self.current_odom_xy_theta = [] self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() self.initialized = True def update_robot_pose(self, timestamp): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # assign the best particle's pose to self.robot_pose as a geometry_msgs.Pose object best_particle = self.particle_cloud[0] for particle in self.particle_cloud[1:]: if particle.w > best_particle.w: best_particle = particle self.robot_pose = best_particle.as_pose() self.transform_helper.fix_map_to_odom_transform( self.robot_pose, timestamp) def projected_scan_received(self, msg): self.last_projected_stable_scan = msg def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # Publish a visualization of all our particles before they get updated timestamp = rospy.Time.now() particle_color = (1.0, 0.0, 0.0) particle_markers = [ particle.as_marker(timestamp, count, "before_odom_hypotheses", particle_color) for count, particle in enumerate(self.particle_cloud) ] # Publish the visualization of all the particles in Rviz self.before_odom_hypothesis_pub.publish( MarkerArray(markers=particle_markers)) # delta xy_theta is relative to the odom frame, which is a global frame # Global Frame -> Robot Frame # Delta also works for relative to robot _> need to rotate it properly # Robot Frame - Rotate it so that it's projected from the particle in the particle frame # Need the difference between the particle theta and the robot theta # That's how much to rotate it by # diff_theta = self.current_odom_xy_theta[2] - # Particle Frame -> Global Frame for index, particle in enumerate(self.particle_cloud): diff_theta = self.current_odom_xy_theta[2] - (particle.theta - math.pi) partRotMtrx = np.array([[np.cos(diff_theta), -np.sin(diff_theta)], [np.sin(diff_theta), np.cos(diff_theta)]]) translationMtrx = np.array([[delta[0]], [delta[1]]]) partTranslationOp = partRotMtrx.dot(translationMtrx) # update particle position to move with delta self.particle_cloud[index].x -= partTranslationOp[0, 0] self.particle_cloud[index].y -= partTranslationOp[1, 0] self.particle_cloud[index].theta += delta[2] if len(self.particle_cloud) == 1: # For debugging purposes print("") print("Robot Theta: ", self.current_odom_xy_theta[2]) print("Particle Theta:", particle.theta) print("Diff Theta: ", diff_theta) print("Deltas before transformations:\nDelta x: ", delta[0], " | Delta y: ", delta[1], " | Delta theta: ", delta[2]) print("Deltas after transformations:\nDelta x: ", partTranslationOp[0, 0], " | Delta y: ", partTranslationOp[1, 0]) # Build up a list of all the just moved particles as Rviz Markers timestamp = rospy.Time.now() particle_color = (0.0, 0.0, 1.0) particle_markers = [ particle.as_marker(timestamp, count, "future_hypotheses", particle_color) for count, particle in enumerate(self.particle_cloud) ] # Publish the visualization of all the particles in Rviz self.future_hypothesis_pub.publish( MarkerArray(markers=particle_markers)) def map_calc_range(self, x, y, theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # cull particles # set looping variable values and initalize array to store significant points def returnFunc(part): return part.w self.particle_cloud.sort(key=returnFunc, reverse=True) numResamplingNodes = 500 resamplingNodes = self.particle_cloud[0:numResamplingNodes] # Calculate the number of particles to cluster around each resamplingNode cluster_size = math.ceil( (self.n_particles - numResamplingNodes) / numResamplingNodes) # Uniformly cluster the lowest weighted particles around the highest weighted particles (resamplingNodes) num_cluster = 0 cluster_radius = 0.25 cluster_theta_range = math.pi / 2.0 for resamplingNode in resamplingNodes: start_cluster_index = numResamplingNodes + num_cluster * cluster_size end_cluster_index = start_cluster_index + cluster_size if end_cluster_index > len(self.particle_cloud): end_cluster_index = len(self.particle_cloud) for particle_index in range(start_cluster_index, end_cluster_index): self.particle_cloud[particle_index].x = uniform( (resamplingNode.x - cluster_radius), (resamplingNode.x + cluster_radius)) self.particle_cloud[particle_index].y = uniform( (resamplingNode.y - cluster_radius), (resamplingNode.y + cluster_radius)) self.particle_cloud[particle_index].theta = uniform( (resamplingNode.w - cluster_theta_range), (resamplingNode.w + cluster_theta_range)) self.particle_cloud[particle_index].w = resamplingNode.w # self.particle_cloud[particle_index].w = uniform((resamplingNode.w - cluster_theta_range),(resamplingNode.w + cluster_theta_range)) num_cluster += 1 # TODO: Experiment with clustering points dependending on weight of the resamplingNode # #repopulate field # #loop through all the significant weighted particles (or nodes in the probability field) # nodeIndex = 0 # particleIndex = 0 # while nodeIndex < len(resamplingNodes): # #place points around nodes # placePointIndex = 0 # #loop through the number of points that need to be placed given the weight of the particle # while placePointIndex < self.n_particles * resamplingNodes[nodeIndex].w: # #place point in circular area around node # radiusRepopCircle = resamplingNodes[nodeIndex].w*10.0 # #create a point in the circular area # self.particle_cloud[particleIndex] = Particle(uniform((resamplingNodes[nodeIndex].x - radiusRepopCircle),(resamplingNodes[nodeIndex].x + radiusRepopCircle)),uniform((resamplingNodes[nodeIndex].y - radiusRepopCircle),(resamplingNodes[nodeIndex].y + radiusRepopCircle)),resamplingNodes[nodeIndex].theta) # #update iteration variables # particleIndex += 1 # placePointIndex += 1 # nodeIndex += 1 def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ # Note: This only updates the weights. This does not move the particles themselves # Only get the specified number of lidar points at regular slices downsampled_angle_range_list = [] downsampled_angles = np.linspace(0, 360, self.num_lidar_points, False) downsampled_angles_int = downsampled_angles.astype(int) for angle, range_ in enumerate(msg.ranges[0:360]): if angle in downsampled_angles_int: downsampled_angle_range_list.append((angle, range_)) # Filter out invalid ranges filtered_angle_range_list = [] for angle, range_ in downsampled_angle_range_list: if range_ != 0.0: filtered_angle_range_list.append((angle, range_)) # Transform ranges into numpy array of xs and ys relative_to_robot = np.zeros((len(filtered_angle_range_list), 2)) for index, (angle, range_) in enumerate(filtered_angle_range_list): relative_to_robot[index, 0] = range_ * np.cos(angle * np.pi / 180.0) # xs relative_to_robot[index, 1] = range_ * np.sin(angle * np.pi / 180.0) # ys # Build up an array of lidar markers for visualization lidar_markers = [] for index, xy_point in enumerate(relative_to_robot): lidar_markers.append( build_lidar_marker(msg.header.stamp, xy_point[0], xy_point[1], index, "base_link", "lidar_visualization", (1.0, 0.0, 0.0))) # Make sure to delete any old markers num_deletion_markers = 360 - len(lidar_markers) for _ in range(num_deletion_markers): marker_id = len(lidar_markers) lidar_markers.append( build_deletion_marker(msg.header.stamp, marker_id, "lidar_visualization")) # Publish lidar points for visualization self.lidar_pub.publish(MarkerArray(markers=lidar_markers)) # For every particle (hypothesis) we have for particle in self.particle_cloud: # Combine the xy positions of the scan with the xy w of the hypothesis # Rotation matrix could be helpful here (https://en.wikipedia.org/wiki/Rotation_matrix) # Build our rotation matrix R = np.array([[np.cos(particle.theta), -np.sin(particle.theta)], [np.sin(particle.theta), np.cos(particle.theta)]]) # Rotate the points according to particle orientation relative_to_particle = (R.dot(relative_to_robot.T)).T # relative_to_particle = relative_to_robot.dot(R) # Translate points to be relative to map origin relative_to_map = deepcopy(relative_to_particle) relative_to_map[:, 0:1] = relative_to_map[:, 0:1] + particle.x * np.ones( (relative_to_map. shape[0], 1)) relative_to_map[:, 1:2] = relative_to_map[:, 1:2] + particle.y * np.ones( (relative_to_map. shape[0], 1)) # Get the distances of each projected point to nearest obstacle distance_list = [] for xy_projected_point in relative_to_map: distance = self.occupancy_field.get_closest_obstacle_distance( xy_projected_point[0], xy_projected_point[1]) if not np.isfinite(distance): # Note: ac109 map has approximately a 10x10 bounding box # Hardcode 1m as the default distance in case the projected point is off the map distance = 1.0 distance_list.append(distance) # Calculate a weight for for this particle # Note: The further away a projected point is from an obstacle point, # the lower its weight should be weight = 1.0 / sum(distance_list) particle.w = weight # Normalize the weights self.normalize_particles() # Grab the first particle particle = self.particle_cloud[0] # Visualize the projected points around that particle projected_lidar_markers = [] for index, xy_point in enumerate(relative_to_map): projected_lidar_markers.append( build_lidar_marker(msg.header.stamp, xy_point[0], xy_point[1], index, "map", "projected_lidar_visualization")) # Make sure to delete any old markers num_deletion_markers = 360 - len(projected_lidar_markers) for _ in range(num_deletion_markers): marker_id = len(projected_lidar_markers) projected_lidar_markers.append( build_deletion_marker(msg.header.stamp, marker_id, "projected_lidar_visualization")) # Publish the projection visualization to rviz self.projected_lidar_pub.publish( MarkerArray(markers=projected_lidar_markers)) # Build up a list of all the particles as Rviz Markers timestamp = rospy.Time.now() particle_markers = [ particle.as_marker(timestamp, count) for count, particle in enumerate(n.particle_cloud) ] # Publish the visualization of all the particles in Rviz self.hypothesis_pub.publish(MarkerArray(markers=particle_markers)) @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( msg.pose.pose) self.initialize_particle_cloud(msg.header.stamp, xy_theta) def initialize_particle_cloud(self, timestamp, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is omitted, the odometry will be used """ # TODO: Check if moving the xy_theta stuff to where the robot initializes around a given set of points is helpful # if xy_theta is None: # xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(self.odom_pose.pose) # Check how the algorithm should initialize its particles # Distribute particles uniformly with parameters defining the number of particles and bounding box if self.particle_init_options == ParticleInitOptions.UNIFORM_DISTRIBUTION: #create an index to track the x cordinate of the particles being created #calculate the number of particles to place widthwize vs hightwize along the map based on the number of particles and the dimensions of the map num_particles_x = math.sqrt(self.n_particles) num_particles_y = num_particles_x index_x = -3 #iterate over the map to place points in a uniform grid while index_x < 4: index_y = -4 while index_y < 3: #create a particle at the location with a random orientation new_particle = Particle(index_x, index_y, uniform(0, 2 * math.pi)) #add the particle to the particle array self.particle_cloud.append(new_particle) #increment the index to place the next particle index_y += 7 / (num_particles_y) #increment index to place next column of particles index_x += 7 / num_particles_x # Distribute particles uniformly, but hard-coded (mainly for quick tests) elif self.particle_init_options == ParticleInitOptions.UNIFORM_DISTRIBUTION_HARDCODED: # Make a list of hypotheses that can update based on values xs = np.linspace(-3, 4, 21) ys = np.linspace(-4, 3, 21) for y in ys: for x in xs: for i in range(5): new_particle = Particle( x, y, np.random.uniform(0, 2 * math.pi)) self.particle_cloud.append(new_particle) # Create a single arbitrary particle (For debugging) elif self.particle_init_options == ParticleInitOptions.SINGLE_PARTICLE: new_particle = Particle(3.1, 0.0, -0.38802401685700466 + math.pi) self.particle_cloud.append(new_particle) # TODO: Set up robot pose on particle cloud initialization # self.update_robot_pose(timestamp) def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ #set variable inital values index = 0 weightSum = 0 # calulate the total particle weight while index < len(self.particle_cloud): weightSum += self.particle_cloud[index].w index += 1 index = 0 #normalize the weight for each particle by divifdng by the total weight while index < len(self.particle_cloud): self.particle_cloud[ index].w = self.particle_cloud[index].w / weightSum index += 1 pass def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, we hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative to the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) if not self.current_odom_xy_theta: self.current_odom_xy_theta = new_odom_xy_theta return if not (self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud(msg.header.stamp) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry if self.last_projected_stable_scan: last_projected_scan_timeshift = deepcopy( self.last_projected_stable_scan) last_projected_scan_timeshift.header.stamp = msg.header.stamp self.scan_in_base_link = self.tf_listener.transformPointCloud( "base_link", last_projected_scan_timeshift) self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose(msg.header.stamp) # update robot's pose self.resample_particles( ) # resample particles to focus on areas of high density # publish particles (so things like rviz can see them) self.publish_particles(msg)
class ParticleFilter(object): """ The class that represents a Particle Filter ROS Node """ def __init__(self): rospy.init_node('pf') # pose_listener responds to selection of a new approximate robot # location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publisher for the particle cloud for visualizing in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # create instances of two helper objects that are provided to you # as part of the project self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() def laserCallback(self, msg): self.stable_scan = msg def normalize_particles(self): ''' make sure all weights add up to 1.0''' sum_w = sum(particle.w for particle in self.particle_cloud) for particle in self.particle_cloud: particle.w/=sum_w def draw_random_sample(n, probabilities, k): """ Return a random sample of k elements from the set n with the specified probabilities n: the total values for all samples probabilities: the probability of selecting each element 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(n))) probabilities = np.array(probabilities) bins = np.add.accumulate(probabilities) new_values = values[np.digitize(random_sample(n), bins)] # choose the new particles based on the probabilities of the old ones samples = [] for i in new_values: samples.append(deepcopy(n[int(i)])) # make a new particle cloud return samples def resample_particles(self): ''' resamples particles according to new weights which are updated based on laser scan messages ''' self.normalize_particles() # creates n (particles) and probabilities (particle weights) n = [] probabilities = [] num_samples = len(self.particle_cloud) for particle in self.particle_cloud: n.append(particle) probabilities.append(particle.w) # resamples particle cloud based on random sampling self.particle_cloud = self.draw_random_sample(n, probabilities, num_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) # TODO this should be deleted before posting self.transform_helper.fix_map_to_odom_transform(msg.pose.pose, msg.header.stamp) # initialize your particle filter based on the xy_theta tuple def run(self): r = rospy.Rate(5) while not(rospy.is_shutdown()): # in the main loop all we do is continuously broadcast the latest # map to odom transform self.transform_helper.send_last_map_to_odom_transform() r.sleep()
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) start_particles: the number of particles first initalized end_particles: the number of particles which end in the filter middle_step: the step at which the number of particles has decayed about 50% d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): """ Define a new particle filter """ print("RUNNING") self.initialized = False # make sure we don't perform updates before everything is setup self.kidnap = False rospy.init_node( 'pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.start_particles = 1000 # the number of particles to use self.end_particles = 200 self.resample_count = 10 self.middle_step = 10 self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # publish weights for live graph node self.weight_pub = rospy.Publisher("/graph_data", Float64MultiArray, queue_size=10) # laser_subscriber listens for data from the lidar rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] # change use_projected_stable_scan to True to use point clouds instead of laser scans self.use_projected_stable_scan = False self.last_projected_stable_scan = None if self.use_projected_stable_scan: # subscriber to the odom point cloud rospy.Subscriber("projected_stable_scan", PointCloud, self.projected_scan_received) self.current_odom_xy_theta = [] self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() # publish the marker array # self.viz = rospy.Publisher('/particle_marker', Marker, queue_size=10) # self.marker = Marker() self.viz = rospy.Publisher('/particle_marker', MarkerArray, queue_size=10) self.markerArray = MarkerArray() self.initialized = True # assigns robot pose. used only a visual debugger, the real data comes from the bag file. def update_robot_pose(self, timestamp): #print("Guessing Robot Position") self.normalize_particles(self.particle_cloud) weights = [p.w for p in self.particle_cloud] index_best = weights.index(max(weights)) best_particle = self.particle_cloud[index_best] self.robot_pose = self.transform_helper.covert_xy_and_theta_to_pose( best_particle.x, best_particle.y, best_particle.theta) self.transform_helper.fix_map_to_odom_transform( self.robot_pose, timestamp) def projected_scan_received(self, msg): self.last_projected_stable_scan = msg # deadreckons particles with respect to robot motion. def update_particles_with_odom(self, msg): """ To apply the robot transformations to a particle, it can be broken down into a rotations, a linear movement, and another rotation (which could equal 0) """ #print("Deadreckoning") new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) delta_x = delta[0] delta_y = delta[1] delta_theta = delta[2] direction = math.atan2(delta_y, delta_x) theta_1 = self.transform_helper.angle_diff( direction, self.current_odom_xy_theta[2]) for p in self.particle_cloud: distance = math.sqrt((delta_x**2) + (delta_y**2)) + np.random.normal( 0, 0.001) dx = distance * np.cos(p.theta + theta_1) dy = distance * np.sin(p.theta + theta_1) p.x += dx p.y += dy p.theta += delta_theta + np.random.normal(0, 0.005) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # print("Resampling Particles") # make sure the distribution is normalized self.normalize_particles(self.particle_cloud) particle_cloud_length = len(self.particle_cloud) n_particles = ParticleFilter.sigmoid_function(self.resample_count, self.start_particles, self.end_particles, self.middle_step, 1) print("Number of Particles Reassigned: " + str(n_particles)) norm_weights = [p.w for p in self.particle_cloud] # print("Weights: "+ str(norm_weights)) top_percent = 0.20 ordered_indexes = np.argsort(norm_weights) ordered_particles = [ self.particle_cloud[index] for index in ordered_indexes ] best_particles = ordered_particles[int(particle_cloud_length * (1 - top_percent)):] new_particles = ParticleFilter.draw_random_sample( self.particle_cloud, norm_weights, n_particles - int(particle_cloud_length * top_percent)) dist = 0.001 # adding a square meter of noise around each ideal particle self.particle_cloud = [] self.particle_cloud += best_particles for p in new_particles: x_pos, y_pos, angle = p.x, p.y, p.theta x_particle = np.random.normal(x_pos, dist) y_particle = np.random.normal(y_pos, dist) theta_particle = np.random.normal(angle, 0.05) self.particle_cloud.append( Particle(x_particle, y_particle, theta_particle)) self.normalize_particles(self.particle_cloud) self.resample_count += 1 def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ #transform laser data from particle's perspective to map coords #print("Assigning Weights") scan = msg.ranges num_particles = len(self.particle_cloud) num_scans = 361 step = 2 angles = np.arange(num_scans) # will be scan indices (0-361) distances = np.array(scan) # will be scan values (scan) angles_rad = np.deg2rad(angles) for p in self.particle_cloud: sin_values = np.sin(angles_rad + p.theta) cos_values = np.cos(angles_rad + p.theta) d_angles_sin = np.multiply(distances, sin_values) d_angles_cos = np.multiply(distances, cos_values) d_angles_sin = d_angles_sin[0:361:step] d_angles_cos = d_angles_cos[0:361:step] total_beam_x = np.add(p.x, d_angles_cos) total_beam_y = np.add(p.y, d_angles_sin) particle_distances = self.occupancy_field.get_closest_obstacle_distance( total_beam_x, total_beam_y) cleaned_particle_distances = [ 2 * np.exp(-(dist**2)) for dist in particle_distances if (math.isnan(dist) != True) ] p_d_cubed = np.power(cleaned_particle_distances, 3) p.w = np.sum(p_d_cubed) self.normalize_particles(self.particle_cloud) @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ #print("Initial Pose Set") xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( msg.pose.pose) self.initialize_particle_cloud(msg.header.stamp, xy_theta) def initialize_particle_cloud(self, timestamp, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is omitted, the odometry will be used Also check to see if we are attempting the robot kidnapping problem or are given an initial 2D pose """ if self.kidnap: print("Kidnap Problem") x_bound, y_bound = self.occupancy_field.get_obstacle_bounding_box() x_particle = np.random.uniform(x_bound[0], x_bound[1], size=self.start_particles) y_particle = np.random.uniform(y_bound[0], y_bound[1], size=self.start_particles) theta_particle = np.deg2rad( np.random.randint(0, 360, size=self.start_particles)) else: print("Starting with Inital Position") if xy_theta is None: print("No Position Definied") xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) x, y, theta = xy_theta x_particle = np.random.normal(x, 0.25, size=self.start_particles) y_particle = np.random.normal(y, 0.25, size=self.start_particles) theta_particle = np.random.normal(theta, 0.001, size=self.start_particles) self.particle_cloud = [Particle(x_particle[i],\ y_particle[i],\ theta_particle[i]) \ for i in range(self.start_particles)] def normalize_particles(self, particle_list): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ #print("Normalize Particles") old_weights = [p.w for p in particle_list] new_weights = [] for p in particle_list: p.w = float(p.w) / sum(old_weights) new_weights.append(p.w) float_array = Float64MultiArray() float_array.data = new_weights self.weight_pub.publish(float_array) def publish_particles(self, msg): """ Publishes particle poses on the map. Uses Paul's default code at the moment, maybe later attempt to publish a visualization/MarkerArray """ particles_conv = [] for num, p in enumerate(self.particle_cloud): particles_conv.append(p.as_pose()) self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) # self.marker_update("map", self.particle_cloud, False) # self.viz.publish() def scan_received(self, msg): """ All control flow happens here! Special init case then goes into loop """ if not (self.initialized): # wait for initialization to complete return # wait a little while to see if the transform becomes available. This fixes a race # condition where the scan would arrive a little bit before the odom to base_link transform # was updated. # self.tf_listener.waitForTransform(self.base_frame, msg.header.frame_id, msg.header.stamp, rospy.Duration(0.5)) if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative to the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) if not self.current_odom_xy_theta: self.current_odom_xy_theta = new_odom_xy_theta return if not (self.particle_cloud): print("Particle Cloud Empty") # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud(msg.header.stamp) self.update_particles_with_laser(msg) self.normalize_particles(self.particle_cloud) self.update_robot_pose(msg.header.stamp) self.resample_particles() elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! print("UPDATING PARTICLES") self.update_particles_with_odom(msg) # update based on odometry if self.last_projected_stable_scan: last_projected_scan_timeshift = deepcopy( self.last_projected_stable_scan) last_projected_scan_timeshift.header.stamp = msg.header.stamp self.scan_in_base_link = self.tf_listener.transformPointCloud( "base_link", last_projected_scan_timeshift) self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose(msg.header.stamp) # update robot's pose self.resample_particles( ) # resample particles to focus on areas of high density # publish particles (so things like rviz can see them) self.publish_particles(msg) def marker_update(self, frame_id, p_cloud, delete): num = 0 if (delete): self.markerArray.markers = [] else: for p in p_cloud: marker = Marker() marker.header.frame_id = frame_id marker.header.stamp = rospy.Time.now() marker.ns = "my_namespace" marker.id = num marker.type = Marker.ARROW marker.action = Marker.ADD marker.pose = p.as_pose() marker.pose.position.z = 0.5 marker.scale.x = 1.0 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 # Don't forget to set the alpha! marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 num += 1 self.markerArray.markers.append(marker) @staticmethod def sigmoid_function(value, max_output, min_output, middle, inc=1): particle_difference = max_output - min_output exponent = inc * (value - (middle / 2)) return int(particle_difference / (1 + np.exp(exponent)) + min_output)
class ParticleFilter(object): """ Particle Filter ROS Node. """ def __init__(self): """ Initialize node and necessary helper functions and p """ rospy.init_node('pf') # Helper functions and debugging. # Occupancy field used to get closest obstacle distance. self.occupancy_field = OccupancyField() # Helper functions for coordinate transformations and operations. self.transform_helper = TFHelper() # Set debug to true to print robot state information to the terminal. self.debug = True # Particle filter attributes. # List of each particle in the filter. self.particle_cloud = [] # Config attributes: # n = Number of particles in the particle cloud. # xy_spread_size: Scale factor for the spread of the x and y # coordinates of the initial particle cloud. # theta_spread_size: Scale factor for the spread of the angles # in the initial particle cloud. # xy_update_thresh: Change in x and y coordinates of the robot # position (as determined by odometry data) at # which to re-estimate robot position and # resample the particle cloud. # theta_update_thresh: Change (in degrees) of the robot's # orientation (as determined by odometry data) at # which to re-estimate robot position and # resample the particle cloud. self.particle_cloud_config = { "n": 100, "xy_spread_size": 1, "theta_spread_size": 30, "xy_update_thresh": 0.005, "theta_update_thresh": 0.001 } # The mininum weight of a particle, used to ensure non weights are NaN. self.minimum_weight = 0.0000001 # Robot location attributes. # Initial pose estimate, stored as a triple (x, y, theta). # Used to create particle cloud. self.xy_theta = None # Pose estimate, stored as a pose message type. # Used to track changes in pose and update pose markers. self.current_pose_estimate = Pose() # The overall change in the pose of the robot. self.pose_delta = [0, 0, 0] # Whether or not there is an initial pose value. self.pose_set = False # The frame of the robot base. self.base_frame = "base_link" # The name of the map coordinate frame. self.map_frame = "map" # The name of the odom coordinate frame. self.odom_frame = "odom" # The number of the most highly-weighted particles to incorporate # in the mean value used to update the robot position estimate. self.particles_to_incoporate_in_mean = 100 # Adjustment factor for the magnitude of noise added to the cloud # during the resampling step. self.noise_adjustment_factor = 0.001 # ROS Publishers/Subscribers # Listen for new approximate initial robot location. # Selected in rviz through the "2D Pose Estimate" button. rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.initialize_pose_estimate) # Get input data from laser scan. rospy.Subscriber("scan", LaserScan, self.laser_scan_callback) # Publish particle cloud for rviz. self.particle_pub = rospy.Publisher("/particlecloud", PoseArray, queue_size=10) def initialize_pose_estimate(self, msg): """ Initialize new pose estimate and particle cloud. Store the pose estimate as it as a triple with the format (x, y, theta). msg: PoseWithCovarianceStamped message received on the initialpose topic after selection of the "2D Pose Estimate" button in rviz. """ if self.debug: print("Got initial pose.") self.xy_theta = \ self.transform_helper.convert_pose_to_xy_and_theta(msg.pose.pose) self.create_particle_cloud(msg.header.stamp) self.pose_set = True def update_pose_estimate(self, timestamp): """ Update robot's pose estimate given the current particle cloud. Calculate the pose estimate using the mean of the most highly weighted particles, and then convert the mean coordinates and angle to a pose stored in self.current_pose_estimate. Call fix_map_to_odom transform to update the displayed current pose estimate. timestamp: The timestamp of the current particle cloud in type time. """ self.normalize_particles() mean_x = 0 mean_y = 0 mean_theta = 0 # Calculate the mean of the top se particle_cloud_majority = sorted(self.particle_cloud, key=lambda x: x.w, reverse=True) for particle in particle_cloud_majority[self.particles_to_incoporate_in_mean:]: mean_x += particle.x * particle.w mean_y += particle.y * particle.w mean_theta = particle.theta * particle.w mean_x /= self.particles_to_incoporate_in_mean mean_y /= self.particles_to_incoporate_in_mean mean_theta /= self.particles_to_incoporate_in_mean # Use particle methods to convert particle to pose. current_pose_particle = Particle(mean_x, mean_y, mean_theta) self.current_pose_estimate = current_pose_particle.as_pose() # Send out next map to odom transform with updated pose estimate. self.transform_helper.fix_map_to_odom_transform(self.current_pose_estimate, timestamp) def normalize_particles(self): """ Ensures particle weights sum to 1 by finding the sum of the particle weights and then dividing each value by this sum. Modifies self.particle_cloud directly. """ # Ensure that none of the particle weights are NaN. self.set_minimum_weight() total_w = sum(p.w for p in self.particle_cloud) if total_w != 1.0: for i in range(len(self.particle_cloud)): self.particle_cloud[i].w /= total_w def set_minimum_weight(self): """ Change any NaN weights in self.particle_cloud to the minimum weight value instead. Modifies self.particle_cloud directly. """ for i in range(len(self.particle_cloud)): if math.isnan(self.particle_cloud[i].w): self.particle_cloud[i].w = self.minimum_weight def create_particle_cloud(self, timestamp): """ Generate a new particle cloud using the parameters stored in self.particle_cloud_config, and then normalize the particles and update the current pose estimate based on the state of the created particle cloud. timestamp: The timestamp of the current particle cloud in type time. """ if (self.debug): print("Creating particle cloud.") if self.xy_theta is None: self.xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) # Sample particle values from normal distributions x_spread = np.random.normal( self.xy_theta[0], self.particle_cloud_config["xy_spread_size"], self.particle_cloud_config["n"]) y_spread = np.random.normal( self.xy_theta[1], self.particle_cloud_config["xy_spread_size"], self.particle_cloud_config["n"]) theta_spread = np.random.normal( self.xy_theta[2], np.deg2rad(self.particle_cloud_config["theta_spread_size"]), self.particle_cloud_config["n"]) self.particle_cloud = [] for (x, y, theta) in zip(x_spread, y_spread, theta_spread): self.particle_cloud.append(Particle(x, y, theta, 1)) self.normalize_particles() self.update_pose_estimate(timestamp) def resample(self): """ Select new distribution of particles, weighted by each particle's weight w. Then add some noise to the system to aid in visualization and increase system robustness. Modifies self.particle_cloud instance directly. """ self.set_minimum_weight() if len(self.particle_cloud): self.normalize_particles() weights = [particle.w if not math.isnan(particle.w) else self.minimum_weight for particle in self.particle_cloud] # Resample points based on their weights. self.particle_cloud = [deepcopy(particle) for particle in list(np.random.choice( self.particle_cloud, size=len(self.particle_cloud), replace=True, p=weights, ))] # Add noise to each particle. for p in self.particle_cloud: particle_noise = np.random.randn(3) p.x += particle_noise[0] * self.noise_adjustment_factor p.y += particle_noise[1] * self.noise_adjustment_factor p.theta += particle_noise[2] * self.noise_adjustment_factor if self.debug: print("Resampling executed.") def publish_particle_viz(self): """ Publish a visualization of self.particle_cloud for use in rviz. """ self.particle_pub.publish( PoseArray( header=Header( stamp=rospy.Time.now(), frame_id=self.map_frame), poses=[ p.as_pose() for p in self.particle_cloud])) if self.debug: print("Publishing new visualization.") def update_pose_delta(self, pose1, pose2): """ Calculate floating point distance between pose triples. pose1: The first pose, stored as a triple (x, y, theta). pose2: The second pose, stored as a triple (x, y, theta). Returns a new triple with the difference between the poses in the form of (x, y, theta) and also updates self.pose_delta. """ self.pose_delta[0] = pose2[0] - pose1[0] self.pose_delta[1] = pose2[1] - pose1[1] self.pose_delta[2] = self.transform_helper.angle_diff(pose2[2], pose1[2]) return self.pose_delta def update_thresholds_met(self, msg): """ Calculate the estimated laser scan and robot pose, and then update the current pose. Return if the difference between the previous and current pose exceeds a given threshold. msg: Incoming laser scan data of message type LaserScan. Returns a boolean indicating whether the change in the robot's position exceeds the given movement threshold. """ # Calculate pose of laser relative to the robot base. p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.transform_helper.tf_listener.transformPose(self.base_frame, p) # Find out where the robot thinks it is based on its odometry. p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.transform_helper.tf_listener.transformPose(self.odom_frame, p) # Store the the odometry pose into (x,y,theta). current_pose = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) # If the old pose has not definition, the robot needs to move more. if not hasattr(self, "old_pose"): self.old_pose = current_pose return False # Otherwise, update self.pose_delta and return whether its magnitude # exceeds the threshold. x_d, y_d, theta_d = self.update_pose_delta(self.old_pose, current_pose) self.old_pose = current_pose return math.fabs(x_d) > self.particle_cloud_config["xy_update_thresh"] or \ math.fabs(y_d) > self.particle_cloud_config["xy_update_thresh"] or \ math.fabs(theta_d) > self.particle_cloud_config["theta_update_thresh"] def odom_update(self): """ Use self.pose_delta to update particle locations to reflect the change in the robot's position. Modifies self.particle_cloud directly. """ x_d, y_d, theta_d = self.pose_delta for i in range(len(self.particle_cloud)): self.particle_cloud[i].x -= x_d self.particle_cloud[i].y -= y_d self.particle_cloud[i].theta += theta_d def laser_update(self, msg): """ Use scan data in msg to update particle weights by using the occupancy field to determine the distance from the closest obstacle and adjusting the particle weights accordingly. msg: Incoming laser scan data of message type LaserScan. Modifies self.particle_cloud in place. """ for particle in self.particle_cloud: # Get total distances for each angle for each particle. total_distance = 0 x_values = msg.ranges * np.cos(np.degrees(np.arange(0, 361, dtype=float) + particle.theta)) y_values = msg.ranges * np.sin(np.degrees(np.arange(0, 361, dtype=float) + particle.theta)) for x, y in zip(x_values, y_values): # Disregard any invalid distances. if x == 0 and y == 0: continue o_d = self.occupancy_field.get_closest_obstacle_distance(particle.x + x, particle.y + y) if not(math.isnan(o_d)) and o_d != 0: # Cube the distance to weight closer objects more highly. total_distance += o_d**3 # Set particle weight to the inverse of the total distance. if total_distance > 0: particle.w = 1.0/total_distance # Ensure that particle weights sum to 1. self.normalize_particles() def laser_scan_callback(self, msg): """ Process incoming laser scan data. If the change in position exceeds the thresholds, update the pose estimate and resample. msg: Incoming laser scan data of message type LaserScan. """ if not self.pose_set: return self.transform_helper.tf_listener.waitForTransform(self.base_frame, msg.header.frame_id, msg.header.stamp, rospy.Duration(0.5)) if not(self.transform_helper.tf_listener.canTransform(self.base_frame, msg.header.frame_id, msg.header.stamp)) or \ not(self.transform_helper.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): return # Regardless of update, publish particle cloud visualization. self.publish_particle_viz() if self.update_thresholds_met(msg): self.odom_update() self.laser_update(msg) # Update the self.current_pose_estimate with the mean particle and resample. self.update_pose_estimate(msg.header.stamp) self.resample() else: if self.debug: print("Update thresholds not met!") def run(self): """ As most of the processing happens through callback functions for laser scan and odometry data, simply publish the most recent transform for as long as the node runs. """ r = rospy.Rate(2) while not(rospy.is_shutdown()): # in the main loop all we do is continuously broadcast the latest # map to odom transform self.transform_helper.send_last_map_to_odom_transform() r.sleep()