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()
#!/usr/bin/env python3
import numpy as np
import os
from gym_duckietown.envs import DuckietownEnv
import pyglet
from pyglet.window import key
from neural_perception.util.pid_controller import PID, calculate_out_lim
from neural_perception.util.util import preprocess, get_lane_pos
import tensorflow as tf
env = DuckietownEnv(domain_rand=False, draw_bbox=False)
obs = env.reset()
obs = preprocess(obs)
env.render()
os.environ["CUDA_VISIBLE_DEVICES"] = "-1"
@env.unwrapped.window.event
def on_key_press(symbol, modifiers):
global DEBUG
global MANUAL_CONTROL
if symbol == key.BACKSPACE:
print("RESET")
env.reset()
env.render()
elif symbol == key.ESCAPE:
env.close()
exit(0)