def launch_env(map_name,
randomize_maps_on_reset=False,
domain_rand=False,
frame_stacking=1):
environment = DuckietownEnv(domain_rand=domain_rand,
max_steps=math.inf,
map_name=map_name,
randomize_maps_on_reset=False)
tmp = environment._get_tile
if frame_stacking != 1:
environment = FrameStack(environment, frame_stacking)
environment._get_tile = tmp
environment.reset() # Force reset to get fake frame buffer
return environment
obs, reward, done, info = env.step([speed, steering])
total_reward += reward
print('Steps = %s, Timestep Reward=%.3f, Total Reward=%.3f' %
(env.step_count, reward, total_reward))
env.render()
i, j = env.get_grid_coords(env.cur_pos)
mask = cv2.inRange(obs, lower_red, upper_red)
#Immagine della maschera su schermo
if mask_enable:
cv2.imshow('Red_Lines_Mask', mask)
cv2.waitKey(1)
starting_angle = env.cur_angle
#Qui viene gestita la svolta a destra
while i == i_rounded and j == j_rounded:
tile = env._get_tile(i, j)
kind = tile['kind']
if kind.startswith('3way'):
if (env.cur_angle < (starting_angle - np.pi * 3 / 8)):
break
"""if kind.startswith('4way'):
if(env.cur_angle < (starting_angle - np.pi*7/20)):
break"""
lane_pose = env.get_right_turn(env.cur_pos,
env.cur_angle,
direction=direction2)
distance_to_road_center = lane_pose.dist
angle_from_straight_in_rads = lane_pose.angle_rad
steering = k_p * distance_to_road_center + k_d * angle_from_straight_in_rads
obs, reward, done, info = env.step([speed, steering])
total_reward += reward
obs = env.reset()
env.render()
total_reward = 0
while True:
lane_pose = env.get_lane_pos2(env.cur_pos, env.cur_angle)
distance_to_road_center = lane_pose.dist
angle_from_straight_in_rads = lane_pose.angle_rad
#####
info = env.get_agent_info()
i, j = env.get_grid_coords(info['Simulator']['cur_pos'])
print((i,j))
print(env._get_tile(i,j))
#####
###### Start changing the code here.
# TODO: Decide how to calculate the speed and direction.
k_p = 5#10
k_d = 2#1
# The speed is a value between [0, 1] (which corresponds to a real speed between 0m/s and 1.2m/s)
speed = 0.2 # TODO: You should overwrite this value
# angle of the steering wheel, which corresponds to the angular velocity in rad/s
steering = k_p*distance_to_road_center + k_d*angle_from_straight_in_rads # TODO: You should overwrite this value
for i in range(steps):
percent = round(i * 100 / steps, 2)
print(f'\rrunning: {percent} %', end='\r')
omega = expert.predict()[1]
omega_hat = model(obs)[0][0].numpy()
omega_err = abs(omega_hat - omega)
errors.append(omega_err)
labels.append(omega)
predictions.append(omega_hat)
kind = env._get_tile(env.cur_pos[0], env.cur_pos[2])['kind']
if kind.startswith('straight'):
straight_tile_errors.append(omega_err)
elif kind == 'curve_left':
curve_left_errors.append(omega_err)
elif kind == 'curve_right':
curve_right_errors.append(omega_err)
elif kind.startswith('3way'):
three_way_errors.append(omega_err)
elif kind.startswith('4way'):
four_way_errors.append(omega_err)
if nn_control:
correction = omega_hat
else:
"""YOUR CODE HERE"""
# using OpenCV erode the costmap x by a kernel size of args.radius
# apply a gaussian blur to smooth the costmap using cv2.BORDER_DEFAULT
"""END YOUR CODE HERE"""
cv2.imwrite(args.outfile, x)
lower_left = (999999, 999999)
upper_right = (0, 0)
for j in range(env.grid_height):
for i in range(env.grid_width):
tile = env._get_tile(i, j)
kind = tile['kind']
if (kind == "3way_left" or kind == "3way_right" or kind == "straight"
or kind == "curve_right" or kind == "curve_left"):
lower_left = (min(lower_left[0], i), min(lower_left[1], j))
upper_right = (max(upper_right[0], i), max(upper_right[1], j))
lower_left = (env.road_tile_size * lower_left[0], env.road_tile_size * lower_left[1])
upper_right = (env.road_tile_size * (upper_right[0] + 1), env.road_tile_size * (upper_right[1] + 1))
lower_left_image = (999999, 999999)
upper_right_image = (0, 0)
for x in contours:
