class Simulator:
def __init__(self,
max_sensor_range=50,
sensor_std=0.0,
num_particles=50,
gps_noise_var=10.0,
gps_noise_width=20):
self.racetrack = load_racetrack("data/racetrack.p")
self.car = Car(max_sensor_range=max_sensor_range,
sensor_std=sensor_std,
gps_noise_var=gps_noise_var,
gps_noise_width=gps_noise_width)
self.max_sensor_range = max_sensor_range
self.sensor_std = sensor_std
self.num_particles = num_particles
self.do_particle_filtering = False
self.particle_filter = None
self.x_est = None
self.y_est = None
self.orient_est = None
self.do_kalman_filtering = False
self.kalman_filter = None
self.kf_state = None
self.gps_noise_var = gps_noise_var
self.gps_noise_width = gps_noise_width
self.gps_noise_dist = "gaussian"
self.r_count = 0
self.cur_rightness = 0.5
self.lap_data = []
self.crossed_start = False
self.lap_data_old = np.load("data/lap_data.npy")
self.cur_i = 2
self.recording = False
self.replaying = False
def init_particles(self):
self.do_particle_filtering = True
self.particle_filter = ParticleFilter(self.num_particles, 0,
WORLD_WIDTH, 0, WORLD_HEIGHT)
def stop_particles(self):
self.do_particle_filtering = False
self.particle_filter = None
def init_kalman(self):
self.do_kalman_filtering = True
self.kalman_filter = KalmanFilter(self.car.gps_noise_var,
self.gps_noise_width)
def stop_kalman(self):
self.do_kalman_filtering = False
self.kalman_filter = None
def toggle_particles(self):
if self.do_particle_filtering:
self.stop_particles()
else:
self.init_particles()
def toggle_kalman(self):
if self.do_kalman_filtering:
self.stop_kalman()
else:
self.init_kalman()
def toggle_gps_noise_dist(self):
if self.gps_noise_dist == "gaussian":
self.gps_noise_dist = "uniform"
elif self.gps_noise_dist == "uniform":
self.gps_noise_dist = "gaussian"
else:
raise ValueError
def toggle_replay(self):
self.replaying = not self.replaying
def loop(self):
if self.recording:
self.lap_data.append(np.append(self.car.pos, self.car.orient))
datum = self.racetrack.progress(self.car)
progress = datum[0]
if 1.1 < progress and progress < 1.5 and not self.crossed_start:
self.crossed_start = True
if 0.5 < progress and progress < 1.0 and self.crossed_start:
np.save("data/lap_data.npy", np.array(self.lap_data))
print("finished")
self.crossed_start = False
if self.replaying:
if self.cur_i == len(self.lap_data_old):
print("replay finished")
return False
dp = self.lap_data_old[self.cur_i]
pos = dp[0:2]
orient = dp[2:]
dp_1 = self.lap_data_old[self.cur_i - 1]
pos_1 = dp_1[0:2]
dp_1 = self.lap_data_old[self.cur_i - 2]
pos_2 = dp_1[0:2]
self.car.pos = pos
self.car.vel = pos - pos_1
self.car.old_vel = pos_1 - pos_2
self.car.orient = orient
self.cur_i += 1
if not self.replaying:
self.car.update(self.racetrack.contour_inner,
self.racetrack.contour_outer)
d_orient = angle_bw(self.car.vel, self.car.old_vel) * np.pi / 180.0
#######################################################################################################################################
# particle filtering
self.car.measure_sensor_dists(self.racetrack)
if self.do_particle_filtering:
sensor = self.racetrack.read_distances
max_sensor_range = self.car.max_sensor_range
sensor_std = self.car.sensor_std
evidence = self.car.sensor_dists
delta_angle = d_orient
speed = np.linalg.norm(self.car.vel)
self.x_est, self.y_est, self.orient_est = self.particle_filter.filtering_and_estimation(
sensor, max_sensor_range, sensor_std, evidence, delta_angle,
speed)
#######################################################################################################################################
# kalman filter
if self.do_kalman_filtering:
self.gps_measurement = self.car.measure_gps(
noise_dist=self.gps_noise_dist)
self.kf_state = self.kalman_filter.predict_and_update(
self.gps_measurement, self.gps_noise_dist)
return True
