def init_particle_filter(): """ Initialize the particle filter. """ num_particles = 500 num_states = 2 dynamics_matrix = [[1, 0], [1, 1]] lower_bounds = [0, 0] upper_bounds = [200, 200] noise_type = 'gaussian' noise_param1 = num_states * [0.0] noise_param2 = num_states * [5.0] final_state_decision_method = 'weighted_average' pf = ParticleFilter(num_particles, num_states, dynamics_matrix, lower_bounds, upper_bounds, noise_type, noise_param1, noise_param2, final_state_decision_method) particle_init_method = 'uniform' pf.init_particles(particle_init_method, lower_bounds, upper_bounds) return pf
class MonteCarloLocalization: def __init__(self, num_particles, xmin, xmax, ymin, ymax): # Initialize node rospy.init_node("MonteCarloLocalization") # Get map from map server print("Wait for static_map from map server...") rospy.wait_for_service('static_map') map = rospy.ServiceProxy("static_map", GetMap) resp1 = map() self.grid_map = resp1.map print("Map resolution: " + str(self.grid_map.info.resolution)) print("Map loaded.") self.particle_filter = ParticleFilter(num_particles, self.grid_map, xmin,xmax,ymin,ymax, 0,0,0,0, 0.25, # translation 0.1, # orientation 0.3) # measurement self.particle_filter.init_particles() self.last_scan = None self.mutex = Lock() self.particles_pub = rospy.Publisher('/particle_filter/particles', MarkerArray, queue_size=1) self.mcl_estimate_pose_pub = rospy.Publisher('/mcl_estimate_pose', PoseStamped, queue_size=1) self.publish_fake_scan = rospy.Publisher('/fake_scan', LaserScan, queue_size=1) rospy.Subscriber('/base_pose_ground_truth', Odometry, self.odometry_callback, queue_size=1) rospy.Subscriber('/scan', LaserScan, self.laser_callback, queue_size=1) self.first_laser_flag = True self.odom_initialized = False self.laser_initialized = False self.mutex = Lock() #********** Callbacks **********# def odometry_callback(self, msg): self.particle_filter.robot_odom = msg if not self.odom_initialized: self.odom_initialized = True def laser_callback(self, msg): if not self.laser_initialized: print("Got first laser callback.") self.particle_filter.laser_min_angle = msg.angle_min self.particle_filter.laser_max_angle = msg.angle_max self.particle_filter.laser_min_range = msg.range_min self.particle_filter.laser_max_range = msg.range_max self.laser_initialized = True self.laser_data = msg # -------------------------------- # def publish_mcl_estimate_pose(self): estimate_pose = PoseStamped() # Populate estimate_pose.header.stamp = rospy.Time.now() estimate_pose.header.frame_id = "map" estimate_pose.pose.position.x = self.particle_filter.mean_estimate_particle.x estimate_pose.pose.position.y = self.particle_filter.mean_estimate_particle.y quat_data = tr.quaternion_from_euler(0,0, self.particle_filter.mean_estimate_particle.theta) estimate_pose.pose.orientation.z = quat_data[2] estimate_pose.pose.orientation.w = quat_data[3] # Publish self.mcl_estimate_pose_pub.publish(estimate_pose) #********** Publishers ************# def publish_particle_markers(self): marker_array = MarkerArray() timestamp = rospy.Time.now() for i in range(len(self.particle_filter.particles)): marker_array.markers.append(self.get_rviz_particle_marker(timestamp, self.particle_filter.particles[i], i)) self.particles_pub.publish(marker_array) #----------------------------------# def get_rviz_particle_marker(self, timestamp, particle, marker_id): """Returns an rviz marker that visualizes a single particle""" msg = Marker() msg.header.stamp = timestamp msg.header.frame_id = 'map' msg.ns = 'particles' msg.id = marker_id msg.type = 0 msg.action = 0 msg.lifetime = rospy.Duration(1) yaw_in_map = particle.theta vx = cos(yaw_in_map) vy = sin(yaw_in_map) msg.color = ColorRGBA(0, 1.0, 0, 1.0) msg.points.append(Point(particle.x, particle.y, 0.2)) msg.points.append(Point(particle.x + 0.3*vx, particle.y + 0.3*vy, 0.2)) msg.scale.x = 0.05 msg.scale.y = 0.05 msg.scale.z = 0.1 return msg def run(self): rate = rospy.Rate(5) while not rospy.is_shutdown(): self.publish_particle_markers() self.run_mcl() rate.sleep() def run_mcl(self): if self.odom_initialized and self.laser_initialized: odom = copy.deepcopy(self.particle_filter.robot_odom) laser = copy.deepcopy(self.laser_data) # print "old samples" self.particle_filter.sample_motion_model_odometry(odom) self.particle_filter.handle_observation(laser) # Re-sample particles based on weight particle_indices = np.arange(self.particle_filter.num_particles) new_particles_indices = np.random.choice(particle_indices, size=self.particle_filter.num_particles, p=self.particle_filter.weights) current_particles = copy.deepcopy(self.particle_filter.particles) new_particles = [copy.deepcopy(current_particles[i]) for i in new_particles_indices] self.particle_filter.particles = copy.deepcopy(new_particles) self.particle_filter.calculate_mean_particle() self.publish_scan() self.publish_mcl_estimate_pose() def publish_scan(self): ls = LaserScan() ls.header.stamp = rospy.Time.now() ls.header.frame_id = "base_laser_link" ls.angle_min = np.min(self.particle_filter.ls_angles) ls.angle_max = np.max(self.particle_filter.ls_angles) ls.angle_increment = np.abs(self.particle_filter.ls_angles[0] - self.particle_filter.ls_angles[1]) ls.range_min = 0.0 ls.range_max = np.max(self.particle_filter.ls_ranges) ls.ranges = self.particle_filter.ls_ranges self.publish_fake_scan.publish(ls)