def one_run(filter_type,
num_steps,
data_factor=1,
filter_factor=1,
num_particles=100,
seed=None):
if seed is not None:
np.random.seed(seed)
alphas = np.array([0.05**2, 0.005**2, 0.1**2, 0.01**2])
beta = np.diag([np.deg2rad(5)**2])
env = Field(data_factor * alphas, data_factor * beta)
policy = policies.OpenLoopRectanglePolicy()
initial_mean = np.array([180, 50, 0]).reshape((-1, 1))
initial_cov = np.diag([10, 10, 1])
if filter_type == 'none':
filt = None
elif filter_type == 'ekf':
filt = ExtendedKalmanFilter(initial_mean, initial_cov,
filter_factor * alphas,
filter_factor * beta)
elif filter_type == 'pf':
filt = ParticleFilter(initial_mean, initial_cov, num_particles,
filter_factor * alphas, filter_factor * beta)
# You may want to edit this line to run multiple localization experiments.
#mean_position_error= localize(env, policy, filt, initial_mean, num_steps,False)
mean_position_error, anees = localize(env, policy, filt, initial_mean,
num_steps, False)
return mean_position_error, anees
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 __init__(self):
print('Initializing')
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node('localizer')
self.pf = ParticleFilter()
self.base_frame = "base_link" # Robot base frame
self.map_frame = "map" # Map coord frame
self.odom_frame = "odom" # Odom coord frame
self.scan_topic = "scan" # Laser scan topic
self.linear_threshold = 0.1 # the amount of linear movement before performing an update
self.angular_threshold = math.pi / 10 # the amount of angular movement before performing an update
self.max_dist = 2.0 # maximum penalty to assess in the likelihood field model
self.odom_pose = PoseStamped()
self.robot_pose = Pose()
self.robot_pose = Pose()
self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.process_scan)
# init pf
# subscribers and publisher
self.odom_sub = rospy.Subscriber("/odom", Odometry,
self.odom_particle_updater)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.pose_updater)
# enable listening for and broadcasting coord transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.current_odom_xy_theta = []
print("initialization complete")
self.initialized = True
alphas = np.array([0.05**2, 0.005**2, 0.1**2, 0.01**2])
beta = np.diag([np.deg2rad(5)**2])
env = Field(args.data_factor * alphas, args.data_factor * beta)
policy = policies.OpenLoopRectanglePolicy()
initial_mean = np.array([180, 50, 0]).reshape((-1, 1))
initial_cov = np.diag([10, 10, 1])
if args.filter_type == 'none':
filt = None
elif args.filter_type == 'ekf':
filt = ExtendedKalmanFilter(
initial_mean,
initial_cov,
args.filter_factor * alphas,
args.filter_factor * beta
)
elif args.filter_type == 'pf':
filt = ParticleFilter(
initial_mean,
initial_cov,
args.num_particles,
args.filter_factor * alphas,
args.filter_factor * beta
)
# You may want to edit this line to run multiple localization experiments.
localize(env, policy, filt, initial_mean, args.num_steps, args.plot)
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 RobotLocalizer(object):
'''
The class that represents a Particle Filter ROS Node
'''
def __init__(self):
print('Initializing')
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node('localizer')
self.pf = ParticleFilter()
self.base_frame = "base_link" # Robot base frame
self.map_frame = "map" # Map coord frame
self.odom_frame = "odom" # Odom coord frame
self.scan_topic = "scan" # Laser scan topic
self.linear_threshold = 0.1 # the amount of linear movement before performing an update
self.angular_threshold = math.pi / 10 # the amount of angular movement before performing an update
self.max_dist = 2.0 # maximum penalty to assess in the likelihood field model
self.odom_pose = PoseStamped()
self.robot_pose = Pose()
self.robot_pose = Pose()
self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.process_scan)
# init pf
# subscribers and publisher
self.odom_sub = rospy.Subscriber("/odom", Odometry,
self.odom_particle_updater)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.pose_updater)
# enable listening for and broadcasting coord transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.current_odom_xy_theta = []
print("initialization complete")
self.initialized = True
def robot_pose_updater(self):
''' Update the estimate of the robot's pose given the updated particles by computing the mean pose'''
self.pf.particle_normalizer()
# Calculate avg particle position based on pose
mean_particle = Particle(0, 0, 0, 0)
mean_particle_theta_x = 0
mean_particle_theta_y = 0
for particle in self.pf.particle_cloud:
mean_particle.x += particle.x * particle.w
mean_particle.y += particle.y * particle.w
# Using trig to calculate angle (@Paul I hate Trigonometry!)
distance_vector = np.sqrt(
np.square(particle.y) + np.square(particle.x))
mean_particle_theta_x += distance_vector * np.cos(
particle.theta) * particle.w
mean_particle_theta_y += distance_vector * np.sin(
particle.theta) * particle.w
mean_particle.theta = np.arctan2(float(mean_particle_theta_y),
float(mean_particle_theta_x))
self.robot_pose = mean_particle.particle_to_pose()
# Get the laser messages
def laserCallback(self, msg):
self.laserCallback = msg
def odom_particle_updater(self, msg):
''' Updates particles based on new odom pose using a delta value for x,y,theta'''
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
# compute the change (delta) in x,y,theta
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
odom_noise = .25 # noise level
'''
updates the particles based on angle1, dist, and angle2.
angle1: Angle by which the robot rotates to face new xy position
dist: Distance to move forward to xy position
angle2: Angle by which the robot rotates to face final direction
'''
for particle in self.pf.particle_cloud:
# calculates angle1, d, and angle2
angle1 = np.arctan2(float(delta[1]), float(
delta[0])) - old_odom_xy_theta[2]
dist = np.sqrt(np.square(delta[0]) + np.square(delta[1]))
angle2 = delta[2] - angle1
# updates the particles with the above variables, while also adding in some noise
#This is the part of class where Paul moved and we all updated based on that movement
particle.theta = particle.theta + angle1 * (
random_sample() * odom_noise + (1 - odom_noise / 2.0))
particle.x = particle.x + dist * np.cos(
particle.theta) * (random_sample() * odom_noise +
(1 - odom_noise / 2.0))
particle.y = particle.y + dist * np.sin(
particle.theta) * (random_sample() * odom_noise +
(1 - odom_noise / 2.0))
particle.theta = particle.theta + angle2 * (
random_sample() * odom_noise + (1 - odom_noise / 2.0))
def particle_resampler(self):
'''Resample the particles according to the new particle weights.'''
# make sure the distribution is normalized
self.pf.particle_normalizer()
particle_list = []
weights = []
num_samples = len(self.pf.particle_cloud)
for particle in self.pf.particle_cloud:
particle_list.append(particle)
weights.append(particle.w)
self.pf.particle_cloud = self.draw_random_sample(
particle_list, weights, num_samples)
def draw_random_sample(self, choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
# sets up an index list for the chosen particles, and makes bins for the probabilities
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(
random_sample(n), bins
)] # chooses the new particles based on the probabilities of the old ones
samples = []
for i in inds:
samples.append(
deepcopy(choices[int(i)])
) # makes the new particle cloud based on the chosen particles
return samples
def laser_particle_updater(self, msg):
'''Updates the particle weights in response to the scan contained in the msg'''
# Find the total error for each particle based on 36 laser measurements taken from the Neato's actual position
for particle in self.pf.particle_cloud:
error = []
for theta in range(0, 360, 10):
rad = np.radians(theta)
err = self.occupancy_field.get_closest_obstacle_distance(
particle.x +
msg.ranges[theta] * np.cos(particle.theta + rad),
particle.y +
msg.ranges[theta] * np.sin(particle.theta + rad))
if (
math.isnan(err)
): # if the get_closest_obstacle_distance method finds that a point is out of bounds, then the particle can't ever be it
particle.w = 0
break
if (
sum(error) == 0
): # if the particle is basically a perfect match, then we make the particle almost always enter the next iteration through resampling
particle.w = 1.0
else:
particle.w = 1.0 / sum(
error
) # the errors are inverted such that large errors become small and small errors become large
def pose_updater(self, msg):
''' Restart particle filter based on updated pose '''
xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
msg.pose.pose)
#self.fix_map_to_odom_transform(msg)
self.pf.particle_cloud_init(xy_theta)
self.fix_map_to_odom_transform(msg)
print("particle cloud initialized")
def process_scan(self, msg):
'''Handling laser data to update our understanding of the robot based on laser and odometry'''
if not (self.initialized):
print("first if not")
# wait for initialization to complete
return
if not (self.tf_listener.canTransform('base_link', msg.header.frame_id,
msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not (self.tf_listener.canTransform('base_link', 'odom',
msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
pose = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, pose)
# find out where the robot thinks it is based on its odometry
pose = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, pose)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
if not (self.pf.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.pf.particle_cloud_init()
# cache the last odometry pose so we can only update our particle filter if we move more than self.linear_threshold or self.angular_threshold
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
print("Trying to initialize!")
self.fix_map_to_odom_transform(msg)
print("Initialized finally!")
self.pf.particle_publisher(msg)
else:
# we have moved far enough to do an update!
#self.odom_particle_updater(msg) # update based on odometry
#print("map!")
#self.laser_particle_updater(msg) # update based on laser scan
#self.robot_pose_updater() # update robot's pose
self.particle_resampler(
) # resample particles to focus on areas of high density
self.fix_map_to_odom_transform(
msg
) # update map to odom transform now that we have new particles
self.pf.particle_publisher(msg)
def fix_map_to_odom_transform(self, msg):
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer """
(translation, rotation) = \
self.transform_helper.convert_pose_inverse_transform(self.robot_pose)
pose = PoseStamped(
pose=self.transform_helper.convert_translation_rotation_to_pose(
translation, rotation),
header=Header(stamp=msg.header.stamp, frame_id=self.base_frame))
self.tf_listener.waitForTransform(self.base_frame,
self.odom_frame, msg.header.stamp,
rospy.Duration(1.0))
self.odom_to_map = self.tf_listener.transformPose(
self.odom_frame, pose)
(self.translation,
self.rotation) = self.transform_helper.convert_pose_inverse_transform(
self.odom_to_map.pose)
def send_last_map_to_odom_transform(self):
if not (hasattr(self, 'translation') and hasattr(self, 'rotation')):
print("sup dude")
return
self.tf_broadcaster.sendTransform(self.translation, self.rotation,
rospy.get_rostime(), 'odom', 'map')
dest='m_noise',
required=False,
type=float,
help='The amount of motion noise to add to the particle filter.')
parser.add_argument(
'--ignore-regions',
dest='ignore_regions',
action='store_true',
help='Set this flag to disable using the region probabilities.')
args = parser.parse_args()
config = get_pf_config(args.config_file)
building_map = BuildingMap(args.map_data)
# If this is a feed generator run, start the user simulator.
if args.make_feed:
simulation = Simulation(building_map, args.feed)
w = DisplayWindow(building_map, args.map_image, sim=simulation)
w.start_make_feed()
# Otherwise, run the particle filter on an existing feed.
else:
display_on = False if args.no_disp else True
c_noise = args.c_noise if args.c_noise else 0
m_noise = args.m_noise if args.m_noise else 0
feed_processor = FeedProcessor(args.feed,
args.loop_feed,
c_noise,
m_noise,
ignore_regions=args.ignore_regions)
pf = ParticleFilter(config, building_map, feed_processor)
w = DisplayWindow(building_map, args.map_image, pf, display=display_on)
w.start_particle_filter()