def produce_pd_images(self):
obs = self.reset()
for i in range(self.set_size):
percent = round(i * 100 / self.set_size, 2)
print(f'\rgenerating set: {percent} %', end='\r')
lp = get_lane_pos(self)
while lp.dist_to_edge < 0:
obs = self.reset()
lp = get_lane_pos(self)
self.images[i] = obs
self.labels[i] = np.array(
[lp.dist_to_edge * 100, lp.angle_deg,
self.get_tile_kind()])
steering = self.k_p * lp.dist + self.k_d * lp.angle_rad
obs, reward, done, info = self.step(
np.array([self.action_speed, steering]))
if done:
self.reset()
def produce_rand_pd_images(self, reset_steps=10):
"""
Do n steps by pd controller then reset to random pose.
"""
reset_counter = 0
obs = self.reset()
for i in range(self.set_size):
percent = round(i * 100 / self.set_size, 2)
print(f'\rgenerating set: {percent} %', end='\r')
lp = get_lane_pos(self)
while lp.dist_to_edge < 0:
obs = self.reset()
lp = get_lane_pos(self)
self.images[i] = obs
self.labels[i] = np.array(
[lp.dist_to_edge * 100, lp.angle_deg,
self.get_tile_kind()])
steering = self.k_p * lp.dist + self.k_d * lp.angle_rad
obs, reward, done, info = self.step(
np.array([self.action_speed, steering]))
reset_counter += 1
if done or reset_counter == reset_steps:
self.reset()
reset_counter = 0
def update(dt):
"""
This function is called at every frame to handle
movement/stepping and redrawing
"""
action = np.array([0.0, 0.0])
if key_handler[key.UP]:
action = np.array([0.44, 0.0])
if key_handler[key.DOWN]:
action = np.array([-0.44, 0])
if key_handler[key.LEFT]:
action = np.array([0.35, +1])
if key_handler[key.RIGHT]:
action = np.array([0.35, -1])
if key_handler[key.SPACE]:
action = np.array([0, 0])
# Speed boost
if key_handler[key.LSHIFT]:
action *= 1.5
obs, reward, done, info = env.step(action)
lane_pose = get_lane_pos(env)
distance_to_road_edge = lane_pose.dist_to_edge * 100
distance_to_road_center = lane_pose.dist
angle_from_straight_in_rad = lane_pose.angle_rad
angle_from_straight_in_deg = lane_pose.angle_deg
y_hat = model(preprocess(obs))
d = y_hat[0][0].numpy()
a = y_hat[0][1].numpy()
dist_err = abs(distance_to_road_edge - d)
angle_err = abs(angle_from_straight_in_deg - a)
print(
f"\ractu: {round(distance_to_road_edge, 1)}, {round(angle_from_straight_in_deg, 1)}, pred: {round(d, 1)}, {round(a, 1)}, error: {round(dist_err, 1)}, {round(angle_err, 1)}",
end='\r')
if done:
print('done!')
env.reset()
env.render()
env.render()
def update(dt):
global obs
global correction
lane_pos = get_lane_pos(env)
dist_to_road_edge = lane_pos.dist_to_edge
pred_dist = model.predict(obs)[0][0]
if not MANUAL_CONTROL:
# correction = pid.update(dist_to_road_edge * 100)
correction = pid.update(pred_dist)
action = np.array([speed, correction])
else:
action = np.array([0.0, 0.0])
if key_handler[key.UP]:
action = np.array([0.44, 0.0])
if key_handler[key.DOWN]:
action = np.array([-0.44, 0])
if key_handler[key.LEFT]:
action = np.array([0.35, +1])
if key_handler[key.RIGHT]:
action = np.array([0.35, -1])
if DEBUG:
print()
print("pred_dist: ", pred_dist)
print("dist_to_road_edge: ", dist_to_road_edge)
print("correction: ", correction)
# print("out_lim: ", out_lim)
# print("signed_dist: ", lane_pos.dist)
# print("dot_dir: ", lane_pos.dot_dir)
# print("angle_deg: ", lane_pos.angle_deg)
print("dist_err: ", abs(dist_to_road_edge * 100 - pred_dist))
obs, _, _, _ = env.step(action)
obs = preprocess(obs)
env.render()
def get_integration_dataset(num_samples):
period = 10
data = np.zeros(shape=(num_samples, 1, 480, 640, 3))
labels = np.zeros(shape=(num_samples, 1))
k_p = 1
k_d = 1
speed = 0.2
env = DuckietownEnv(domain_rand=False, draw_bbox=False)
iterations = env.max_steps = num_samples * period
env.reset()
for i in range(iterations):
percent = round(i * 100 / iterations, 2)
print(f'\rsimulator running: {percent} %', end='\r')
lane_pose = get_lane_pos(env)
distance_to_road_center = lane_pose.dist
angle_from_straight_in_rad = lane_pose.angle_rad
steering = k_p * distance_to_road_center + k_d * angle_from_straight_in_rad
command = np.array([speed, steering])
obs, _, done, _ = env.step(command)
obs = integration_preprocess(obs)
if i % period == 0:
data[i // period, 0, :, :, :] = obs
labels[i // period][0] = lane_pose.dist_to_edge * 100
if done:
env.reset()
return list(zip(data, labels))
crashes = 0
obs = env.reset()
obs = preprocess(obs)
if visual:
env.render()
t0 = time.perf_counter()
for i in range(steps):
percent = round(i * 100 / steps, 2)
print(f'\rrunning: {percent} %', end='\r')
lane_pose = get_lane_pos(env)
distance_to_road_edge = lane_pose.dist_to_edge * 100
distance_to_road_center = lane_pose.dist
angle_from_straight_in_rad = lane_pose.angle_rad
angle_from_straight_in_deg = lane_pose.angle_deg
y_hat = model(obs)
d = y_hat[0][0].numpy()
a = y_hat[0][1].numpy()
dist_err = abs(distance_to_road_edge - d)
angle_err = abs(angle_from_straight_in_deg - a)
errors.append([dist_err, angle_err])
labels.append([distance_to_road_edge, angle_from_straight_in_deg])