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)