def visualize(self):
def make_pose_from_xy_theta(xytheta):
pose = Pose()
pose.position.x = xytheta[0]
pose.position.y = xytheta[1]
pose.position.z = 0
orien = tf.transformations.quaternion_from_euler(0, 0, xytheta[2])
pose.orientation.x = orien[0]
pose.orientation.y = orien[1]
pose.orientation.z = orien[2]
pose.orientation.w = orien[3]
return pose
print_locks("Entering lock visualize")
self.state_lock.acquire()
print_locks("Entered lock visualize")
start_time = time.time()
laser = self.sensor_model.last_laser # no deepcopy
particles = np.copy(self.particles)
weights = np.copy(self.weights)
print_locks("Exiting lock visualize (computation will continue)")
self.state_lock.release()
# compute pose
pose = self.expected_pose(particles, weights)
# (1) Publishes a tf between the map and the laser. Necessary for visualizing the laser scan in the map
self.publish_tf(pose)
# (2) Publishes the most recent laser measurement. Note that the frame_id of this message should be the child_frame_id of the tf from (1)
laser.header.frame_id = "/laser" #"laser"
self.pub_laser.publish(laser) # /scan /map /map
# (3) Publishes a PoseStamped message indicating the expected pose of the car
pose_msg = PoseStamped()
pose_msg.header.stamp = rospy.Time.now()
pose_msg.header.frame_id = '/map'
pose_msg.pose = make_pose_from_xy_theta(pose)
self.pose_pub.publish(pose_msg) #PoseStamped(pose)
# (4) Publishes a subsample of the particles (use self.MAX_VIZ_PARTICLES).
# Sample so that particles with higher weights are more likely to be sampled.
particle_indices = np.random.choice(self.MAX_PARTICLES,
size=self.MAX_VIZ_PARTICLES,
replace=False,
p=weights)
sampled_particles = particles[particle_indices, :]
particles_msg = PoseArray()
particles_msg.header.stamp = rospy.Time.now()
particles_msg.header.frame_id = '/map'
particles_msg.poses = [
make_pose_from_xy_theta(sampled_particles[i])
for i in range(len(sampled_particles))
]
self.particle_pub.publish(particles_msg)
print_locks("Finished visualize")
print_benchmark("visualize", start_time, time.time())
def resample_naiive(self):
print_locks("Entering lock resample_naiive")
self.state_lock.acquire()
print_locks("Entered lock resample_naiive")
start_time = time.time()
# Use np.random.choice to re-sample
# YOUR CODE HERE
assert len(self.particles) == len(self.weights)
M = len(self.particles)
#print("RESAMLING WEIGHTS: ")
best_weight_indices = [
x
for x in reversed(sorted(range(M), key=lambda i: self.weights[i]))
][0:20]
self.particle_indices = np.random.choice(M,
size=M,
replace=True,
p=self.weights)
self.particles[:, :] = self.particles[self.particle_indices, :]
#print("Post-resmaple particle shape", self.particles.shape)
#print("====")
#print("")
self.weights[:] = (1.0 / M)
print_locks("Exiting lock resample_naiive")
print_benchmark("resample_naiive", start_time, time.time())
self.state_lock.release()
def lidar_cb(self, msg):
# # comment back in to no-op
# self.last_laser = msg
# self.do_resample = True
# return
print_locks("Entering lock lidar_cb")
self.state_lock.acquire()
print_locks("Entered lock lidar_cb")
start_time = time.time()
# Compute the observation
# obs is a a two element tuple
# obs[0] is the downsampled ranges
# obs[1] is the downsampled angles
# Each element of obs must be a numpy array of type np.float32 (this is a requirement for GPU processing)
# Use self.LASER_RAY_STEP as the downsampling step
# Keep efficiency in mind, including by caching certain things that won't change across future iterations of this callback
# obs_ranges = np.array(msg.ranges[0::self.LASER_RAY_STEP], dtype=np.float32)
# obs_angles = np.array( \
# np.arange(0, len(msg.ranges), self.LASER_RAY_STEP, dtype=np.float32) * msg.angle_increment + msg.angle_min, \
# dtype=np.float32)
# obs = [obs_ranges, obs_angles]
obs = [[], []]
for i in range(0, len(msg.ranges), self.LASER_RAY_STEP):
obs[0].append(msg.ranges[i])
obs[1].append(msg.angle_min + i * msg.angle_increment)
obs = [
np.array(obs[0], dtype=np.float32),
np.array(obs[1], dtype=np.float32)
]
#prior = np.array(self.weights, dtype=np.float32)
weights, weights_squashed = self.apply_sensor_model(
self.particles, obs, self.weights)
#print("WEIGHTS", np.percentile(weights, [0, 25, 50, 75, 100]))
self.weights_unsquashed[:] = weights
self.weights[:] = weights_squashed / np.sum(weights_squashed)
#after = np.array(self.weights, dtype=np.float32)
#print("PRIOR", [x for x in reversed(sorted(prior))][0:20])
#print("AFTER", [x for x in reversed(sorted(after))][0:20])
#print("DELTA", [x for x in reversed(sorted(after - prior))][0:20])
self.last_laser = msg
self.do_resample = True
print_locks("Exiting lock lidar_cb")
print_benchmark("lidar_cb", start_time, time.time())
self.state_lock.release()
def motion_cb(self, msg):
print_locks("Entering lock motion_cb")
self.state_lock.acquire()
print_locks("Entered lock motion_cb")
start_time = time.time()
x, y = msg.pose.pose.position.x, msg.pose.pose.position.y
theta = Utils.quaternion_to_angle(msg.pose.pose.orientation)
#print("have", x, y, theta)
self.inner.update([x, y, theta])
print_locks("Releasing lock motion_cb")
print_benchmark("odometry motion_cb", start_time, time.time())
self.state_lock.release()
def resample_low_variance(self, lock_taken=False):
if not lock_taken:
print_locks("Entering lock resample_low_variance")
self.state_lock.acquire()
print_locks("Entered lock resample_low_variance")
start_time = time.time()
M = len(self.particles)
r = np.random.uniform(0.0, 1.0 / M)
weights_cdf = np.cumsum(self.weights)
search_ws = r + np.arange(M, dtype=float) / float(M)
search_ws, ignore = np.modf(search_ws) #fractional, int
particle_indices = np.searchsorted(weights_cdf, search_ws)
self.particles[:, :] = self.particles[particle_indices, :]
self.weights[:] = 1.0 / M
# for i in range(M):
# ind = numpy.searchsorted(weights_cdf, r)
# # sanity, don't understand searchsorted propss
# ind = max(min(M - 1, ind), 0)
#
# w = self.weights[ind]
# r += 1.0 / M
# r = r - math.floor(r)
# M = len(self.particles)
# r = np.random.uniform(0.0, 1.0) # Draw random number in the range [0, 1/M]
# U = r - (1.0 / M)
# particle_index = 0
# weight_total = self.weights[0]
# self.particle_indices = np.zeros(M, dtype=int)
#
# for new_particle_index in range(M):
# U += (1.0 / M)
# while U > weight_total:
# particle_index += 1
# weight_total += self.weights[particle_index % M]
# particle_index = particle_index % M
# self.particle_indices[new_particle_index] = particle_index
#
# self.particles[:, :] = self.particles[self.particle_indices, :]
# self.weights[:] = (1.0 / M)
if not lock_taken:
print_locks("Exiting lock resample_low_variance")
self.state_lock.release()
print_benchmark("resample_low_variance", start_time, time.time())
def motion_cb(self, msg):
print_locks("Entering lock motion_cb")
self.state_lock.acquire()
print_locks("Entered lock motion_cb")
start_time = time.time()
# if no servo info, skip
if self.last_servo_cmd is None:
print("Releasing lock motion_cb early (no servo command)")
self.state_lock.release()
return
# if no prev timestamp, then play as if dt is 0
if self.last_vesc_stamp is None:
self.last_vesc_stamp = msg.header.stamp
dt = (msg.header.stamp - self.last_vesc_stamp).to_sec()
self.last_vesc_stamp = msg.header.stamp
# Convert raw msgs to controls
# Note that control = (raw_msg_val - offset_param) / gain_param
curr_speed = float(
float(msg.state.speed) - float(self.SPEED_TO_ERPM_OFFSET)) / float(
self.SPEED_TO_ERPM_GAIN)
curr_steering_angle = float(
float(self.last_servo_cmd) -
float(self.STEERING_TO_SERVO_OFFSET)) / float(
self.STEERING_TO_SERVO_GAIN)
# print("Velocity ", curr_speed)
# print("Steering Angle: ", curr_steering_angle, " -- ", self.last_servo_cmd, " -- ", self.STEERING_TO_SERVO_OFFSET)
# print("Delta Time: ", dt)
self.inner.update([curr_speed, curr_steering_angle, dt])
print_locks("Releasing lock motion_cb")
print_benchmark("kinematic motion_cb", start_time, time.time())
self.state_lock.release()
def monte_carlo_localization(self, weights_unsquashed):
print_locks("Entering lock monte")
self.state_lock.acquire()
print_locks("Entered lock monte")
start_time = time.time()
new_particles = np.zeros(self.particles.shape)
M = len(self.weights)
w_avg = (1.0 / M) * weights_unsquashed.sum()
#w_avg = -math.log10(w_avg_)
self.weight_fast = (1.0 - ALPHA1) * self.weight_fast + ALPHA1 * w_avg
self.weight_slow = (1.0 - ALPHA2) * self.weight_slow + ALPHA2 * w_avg
prob_new_particle = max(0.0, 1.0 - self.weight_fast / self.weight_slow)
print(w_avg, "fast", self.weight_fast, "slow", self.weight_slow,
"ratio", self.weight_fast / self.weight_slow, "p",
prob_new_particle)
if prob_new_particle == 0.0:
self.resample_low_variance(True)
print_locks("Exiting lock monte (A!)")
self.state_lock.release()
return
print("party time!")
random_kept = np.random.rand(M) > prob_new_particle
ok_thres = np.percentile(self.weights, 0.1)
force_kept = self.weights > ok_thres
kept = np.logical_or(random_kept, force_kept)
choice = 1.0 - np.array(kept, dtype=float)
variance_particle_indices = np.random.choice(M,
size=M,
replace=True,
p=self.weights)
variance_particles = self.particles[variance_particle_indices]
new_particles_indices = np.random.randint(
0, self.map_free_space.shape[0] - 1, M)
new_particles = np.zeros((M, 3))
new_particles[:, 0] = self.map_free_space[new_particles_indices, 0]
new_particles[:, 1] = self.map_free_space[new_particles_indices, 1]
new_particles[:, 2] = np.random.uniform(0, 2 * np.pi, M)
self.particles[:, :] = new_particles * choice[:, np.
newaxis] + variance_particles * (
1.0 - choice
)[:, np.newaxis]
self.weights[:] = (1.0 / M)
"""
for m in range(M):
if prob_new_particle <= np.random.rand():
ind = random.randint(0, len(self.map_free_space) - 1)
new_particles[m][0] = self.map_free_space[ind][0]
new_particles[m][1] = self.map_free_space[ind][1]
new_particles[m][2] = np.random.uniform(0, 2*np.pi)
else:
particle_index = np.random.choice(M, size=None, replace=True, p=self.weights)
new_particles[m] = self.particles[particle_index]
self.particles[:,:] = new_particles
self.weights[:] = (1.0 / M)
"""
print_locks("Exiting lock monte")
self.state_lock.release()
print_benchmark("resample_monte", start_time, time.time())