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 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()) readings_new = readings_for_obstacles_new() plt.figure('new') lasers.plot_lasers( true_x, true_y, true_theta, laser_angles, laser_max_range, obstacles, readings_new, plt.gca()) plt.show()
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)
# LW MAP, state noise 0.01, laser noise 0.01, 100K samples, trial 2: x = -6.111312885927913 y = -0.11534201599817488 theta = 0.01650033592056109 # Compute laser location from vehicle (rear-axle) location: a = 0.299541 b = 0.0500507 laser_x, laser_y, laser_theta = car_loc_to_laser_loc(x, y, theta, a, b) laser_angles = np.arange(-90, 90.5, 0.5) * np.pi / 180 laser_max_range = 10 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] ]) plt.figure(figsize=(8, 8)) plot_lasers( laser_x, laser_y, laser_theta, laser_angles, laser_max_range, obstacles, lasers, plt.gca()) plt.plot([-7, -7, 7, 7, -7], [-7, 7, 7, -7, -7], 'k') plt.xlim(-15, 15) plt.ylim(-15, 15) plt.show()