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