def test_readings_for_obstacles_regression_1():
"""
Regression test for bug before normalize_radians().
"""
a = 0.299541
b = 0.0500507
laser_angles = lasers.default_laser_angles()
laser_max_range = lasers.default_laser_max_range()
obstacles = np.array([
[-4.475, 1.45, 0.35],
[-1.3, 1.025, 0.35],
[-3., -1.55, 0.35],
[0.65, -1.95, 0.35],
[-1.95, -3.8, 0.35],
[0.15, -5.625, 0.35]])
x = 0.18361848856646254
y = -4.2881577071112806
theta = -2.2011317013010205
lx, ly, ltheta = lasers.car_loc_to_laser_loc(x, y, theta, a, b)
obs_lasers = fast.readings_for_obstacles(
lx, ly, ltheta, laser_angles, laser_max_range, obstacles)
# For debugging when the test fails:
if False:
for i, val in enumerate(obs_lasers):
print i, val
lasers.plot_lasers(
lx, ly, ltheta, laser_angles, laser_max_range,
obstacles, obs_lasers, plt.gca())
plt.show()
nose.tools.assert_almost_equals(obs_lasers[16], 1.89867472652, 10)
nose.tools.assert_almost_equals(obs_lasers[85], 10.0, 10)
nose.tools.assert_almost_equals(obs_lasers[278], 0.739595449593, 10)
def test_readings_for_obstacle_vectorized():
lx, ly, ltheta = -7.0, -7.0, 1.7
laser_angles = lasers.default_laser_angles()
laser_max_range = lasers.default_laser_max_range()
ox, oy, orad = 7.0, 9.0, 1.0
readings_1 = lasers.readings_for_obstacle(
lx, ly, ltheta, laser_angles, laser_max_range, ox, oy, orad)
readings_2 = lasers.readings_for_obstacle_vectorized(
lx, ly, ltheta, laser_angles, laser_max_range, ox, oy, orad)
nose.tools.assert_true(np.sum(np.abs(readings_1 - readings_2)) < 1e-6)
def test_readings_for_obstacles_old_new():
laser_angles = lasers.default_laser_angles()
laser_max_range = lasers.default_laser_max_range()
obstacles = np.array([
[-4.475, 1.45, 0.4],
[-1.3, 1.025, 0.4],
[-3.0, -1.55, 0.4],
[0.65, -1.95, 0.4],
[-1.95, -3.8, 0.4],
[0.15, -5.625, 0.4]
])
lx = -6.1039
ly = -0.0499926
for ltheta in np.arange(-np.pi, np.pi, 0.1):
print ltheta
readings_1 = lasers.readings_for_obstacles_old(
lx, ly, ltheta, laser_angles, laser_max_range, obstacles)
readings_2 = fast.readings_for_obstacles(
lx, ly, ltheta, laser_angles, laser_max_range, obstacles)
nose.tools.assert_true(np.sum(np.abs(readings_1 - readings_2)) < 1e-6)
def test_readings_for_obstacles_regression_2():
"""
Regression test for assertion failure in fast_lasers.c.
"""
lx, ly, ltheta = -5.8043534, -2.562654, -0.000112593
laser_angles = lasers.default_laser_angles()
laser_max_range = lasers.default_laser_max_range()
obstacles = np.array([
[4.6, -3.4, 0.68],
])
obs_lasers = fast.readings_for_obstacles(
lx, ly, ltheta, laser_angles, laser_max_range, obstacles)
# For debugging when the test fails:
if False:
for i, val in enumerate(obs_lasers):
print i, val
lasers.plot_lasers(
lx, ly, ltheta, laser_angles, laser_max_range,
obstacles, obs_lasers, plt.gca())
plt.show()
nose.tools.assert_almost_equals(obs_lasers[170], 9.761751166778446, 10)
def __init__(self, dataset, num_particles):
super(LocPF, self).__init__(num_particles)
self.dataset = dataset
self.current_velocity = 0.0
self.current_steering = 0.0
self.add_controls_noise = False
self.velocity_noise_stdev = 0.1
self.steering_noise_stdev = 0.3
self.add_dynamics_noise = True
self.x_noise_stdev = 0.003
self.y_noise_stdev = 0.003
self.theta_noise_stdev = 0.0015
self.obs_cov_scale = 30.0
self.laser_angles = lasers.default_laser_angles()
self.laser_max_range = lasers.default_laser_max_range()
# Convert obstacles from list of (x, y) to array of (x, y, r).
obstacle_radius = 0.37
self.ground_obstacles = []
for x, y in dataset.ground_obstacles:
self.ground_obstacles.append((x, y, obstacle_radius))
self.ground_obstacles = np.array(self.ground_obstacles)
self.fig1 = plt.figure(figsize=(8, 12))
self.ax1 = self.fig1.add_subplot(211)
self.ax2 = self.fig1.add_subplot(413)
self.ax3 = self.fig1.add_subplot(414)
# Hack to display colorbar for angle.
plt.sca(self.ax1)
a = np.array([[-np.pi, np.pi]])
x = plt.pcolor(a)
self.ax1.clear()
plt.colorbar(x, ax=self.ax1)
# Hack to display legends for the bottom plots.
dummy_lines = []
dummy_lines += self.ax2.plot(
[0, 0], [0, 0], 'g', label='ground_gps_llik')
dummy_lines += self.ax2.plot(
[0, 0], [0, 0], 'r', label='best_particle_llik')
self.ax2.legend()
dummy_lines += self.ax3.plot([0, 0], [0, 0], label='error^2')
self.ax3.legend()
for line in dummy_lines:
line.remove()
# Plot ground-truth and dead-reckoning trajectories.
gps_traj = draw_dr.get_ground_truth_traj(dataset)
self.ax1.plot(gps_traj[:, 1], gps_traj[:, 2], label='ground')
dr_traj = draw_dr.get_dead_reckoning_traj(dataset, draw_dr.dynamics)
self.ax1.plot(dr_traj[:, 1], dr_traj[:, 2], label='dead-reckoning')
lasers.plot_obstacles(self.ground_obstacles, self.ax1)
self.ax1.set_xlim(X_MIN - 1, X_MAX + 1)
self.ax1.set_ylim(Y_MIN - 1, Y_MAX + 1)
self.ax1.legend()
plt.ion() # needed for running through cProfile
plt.show()
from ppaml_car import fast
from ppaml_car import lasers
from timeit import timeit
import matplotlib.pyplot as plt
import numpy as np
laser_angles = lasers.default_laser_angles()
laser_max_range = lasers.default_laser_max_range()
obstacles = np.array([
[-4.475, 1.45, 0.4],
[-1.3, 1.025, 0.4],
[-3.0, -1.55, 0.4],
[0.65, -1.95, 0.4],
[-1.95, -3.8, 0.4],
[0.15, -5.625, 0.4]
])
true_x = -6.1039
true_y = -0.0499926
true_theta = -0.14
# true_theta = -2.34159265359
def visual_test():
readings_old = readings_for_obstacles_old()
plt.figure('old')
lasers.plot_lasers(
true_x, true_y, true_theta,
laser_angles, laser_max_range,
obstacles, readings_old, plt.gca())
