def exec_waypoint_nav_demo(args):
""" Executes waypoint navigation demo.
"""
with make_carla_client(args.host, args.port) as client:
print('Carla client connected.')
settings = make_carla_settings(args)
# Now we load these settings into the server. The server replies
# with a scene description containing the available start spots for
# the player. Here we can provide a CarlaSettings object or a
# CarlaSettings.ini file as string.
scene = client.load_settings(settings)
# Refer to the player start folder in the WorldOutliner to see the
# player start information
player_start = PLAYER_START_INDEX
client.start_episode(player_start)
time.sleep(CLIENT_WAIT_TIME);
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print('Starting new episode at %r...' % scene.map_name)
client.start_episode(player_start)
#############################################
# Load Configurations
#############################################
# Load configuration file (options.cfg) and then parses for the various
# options. Here we have two main options:
# live_plotting and live_plotting_period, which controls whether
# live plotting is enabled or how often the live plotter updates
# during the simulation run.
config = configparser.ConfigParser()
config.read(os.path.join(
os.path.dirname(os.path.realpath(__file__)), 'options.cfg'))
demo_opt = config['Demo Parameters']
# Get options
enable_live_plot = demo_opt.get('live_plotting', 'true').capitalize()
enable_live_plot = enable_live_plot == 'True'
live_plot_period = float(demo_opt.get('live_plotting_period', 0))
# Set options
live_plot_timer = Timer(live_plot_period)
#############################################
# Load stop sign and parked vehicle parameters
# Convert to input params for LP
#############################################
# Stop sign (X(m), Y(m), Z(m), Yaw(deg))
stopsign_data = None
stopsign_fences = [] # [x0, y0, x1, y1]
with open(C4_STOP_SIGN_FILE, 'r') as stopsign_file:
next(stopsign_file) # skip header
stopsign_reader = csv.reader(stopsign_file,
delimiter=',',
quoting=csv.QUOTE_NONNUMERIC)
stopsign_data = list(stopsign_reader)
# convert to rad
for i in range(len(stopsign_data)):
stopsign_data[i][3] = stopsign_data[i][3] * np.pi / 180.0
# obtain stop sign fence points for LP
for i in range(len(stopsign_data)):
x = stopsign_data[i][0]
y = stopsign_data[i][1]
z = stopsign_data[i][2]
yaw = stopsign_data[i][3] + np.pi / 2.0 # add 90 degrees for fence
spos = np.array([
[0, 0 ],
[0, C4_STOP_SIGN_FENCELENGTH]])
rotyaw = np.array([
[np.cos(yaw), np.sin(yaw)],
[-np.sin(yaw), np.cos(yaw)]])
spos_shift = np.array([
[x, x],
[y, y]])
spos = np.add(np.matmul(rotyaw, spos), spos_shift)
stopsign_fences.append([spos[0,0], spos[1,0], spos[0,1], spos[1,1]])
# Parked car(s) (X(m), Y(m), Z(m), Yaw(deg), RADX(m), RADY(m), RADZ(m))
parkedcar_data = None
parkedcar_box_pts = [] # [x,y]
with open(C4_PARKED_CAR_FILE, 'r') as parkedcar_file:
next(parkedcar_file) # skip header
parkedcar_reader = csv.reader(parkedcar_file,
delimiter=',',
quoting=csv.QUOTE_NONNUMERIC)
parkedcar_data = list(parkedcar_reader)
# convert to rad
for i in range(len(parkedcar_data)):
parkedcar_data[i][3] = parkedcar_data[i][3] * np.pi / 180.0
# obtain parked car(s) box points for LP
for i in range(len(parkedcar_data)):
x = parkedcar_data[i][0]
y = parkedcar_data[i][1]
z = parkedcar_data[i][2]
yaw = parkedcar_data[i][3]
xrad = parkedcar_data[i][4]
yrad = parkedcar_data[i][5]
zrad = parkedcar_data[i][6]
cpos = np.array([
[-xrad, -xrad, -xrad, 0, xrad, xrad, xrad, 0 ],
[-yrad, 0, yrad, yrad, yrad, 0, -yrad, -yrad]])
rotyaw = np.array([
[np.cos(yaw), np.sin(yaw)],
[-np.sin(yaw), np.cos(yaw)]])
cpos_shift = np.array([
[x, x, x, x, x, x, x, x],
[y, y, y, y, y, y, y, y]])
cpos = np.add(np.matmul(rotyaw, cpos), cpos_shift)
for j in range(cpos.shape[1]):
parkedcar_box_pts.append([cpos[0,j], cpos[1,j]])
#############################################
# Load Waypoints
#############################################
# Opens the waypoint file and stores it to "waypoints"
waypoints_file = WAYPOINTS_FILENAME
waypoints_filepath =\
os.path.join(os.path.dirname(os.path.realpath(__file__)),
WAYPOINTS_FILENAME)
waypoints_np = None
with open(waypoints_filepath) as waypoints_file_handle:
waypoints = list(csv.reader(waypoints_file_handle,
delimiter=',',
quoting=csv.QUOTE_NONNUMERIC))
waypoints_np = np.array(waypoints)
#############################################
# Controller 2D Class Declaration
#############################################
# This is where we take the controller2d.py class
# and apply it to the simulator
controller = controller2d.Controller2D(waypoints)
#############################################
# Determine simulation average timestep (and total frames)
#############################################
# Ensure at least one frame is used to compute average timestep
num_iterations = ITER_FOR_SIM_TIMESTEP
if (ITER_FOR_SIM_TIMESTEP < 1):
num_iterations = 1
# Gather current data from the CARLA server. This is used to get the
# simulator starting game time. Note that we also need to
# send a command back to the CARLA server because synchronous mode
# is enabled.
measurement_data, sensor_data = client.read_data()
sim_start_stamp = measurement_data.game_timestamp / 1000.0
# Send a control command to proceed to next iteration.
# This mainly applies for simulations that are in synchronous mode.
send_control_command(client, throttle=0.0, steer=0, brake=1.0)
# Computes the average timestep based on several initial iterations
sim_duration = 0
for i in range(num_iterations):
# Gather current data
measurement_data, sensor_data = client.read_data()
# Send a control command to proceed to next iteration
send_control_command(client, throttle=0.0, steer=0, brake=1.0)
# Last stamp
if i == num_iterations - 1:
sim_duration = measurement_data.game_timestamp / 1000.0 -\
sim_start_stamp
# Outputs average simulation timestep and computes how many frames
# will elapse before the simulation should end based on various
# parameters that we set in the beginning.
SIMULATION_TIME_STEP = sim_duration / float(num_iterations)
print("SERVER SIMULATION STEP APPROXIMATION: " + \
str(SIMULATION_TIME_STEP))
TOTAL_EPISODE_FRAMES = int((TOTAL_RUN_TIME + WAIT_TIME_BEFORE_START) /\
SIMULATION_TIME_STEP) + TOTAL_FRAME_BUFFER
#############################################
# Frame-by-Frame Iteration and Initialization
#############################################
# Store pose history starting from the start position
measurement_data, sensor_data = client.read_data()
start_timestamp = measurement_data.game_timestamp / 1000.0
start_x, start_y, start_yaw = get_current_pose(measurement_data)
send_control_command(client, throttle=0.0, steer=0, brake=1.0)
x_history = [start_x]
y_history = [start_y]
yaw_history = [start_yaw]
time_history = [0]
speed_history = [0]
collided_flag_history = [False] # assume player starts off non-collided
#############################################
# Vehicle Trajectory Live Plotting Setup
#############################################
# Uses the live plotter to generate live feedback during the simulation
# The two feedback includes the trajectory feedback and
# the controller feedback (which includes the speed tracking).
lp_traj = lv.LivePlotter(tk_title="Trajectory Trace")
lp_1d = lv.LivePlotter(tk_title="Controls Feedback")
###
# Add 2D position / trajectory plot
###
trajectory_fig = lp_traj.plot_new_dynamic_2d_figure(
title='Vehicle Trajectory',
figsize=(FIGSIZE_X_INCHES, FIGSIZE_Y_INCHES),
edgecolor="black",
rect=[PLOT_LEFT, PLOT_BOT, PLOT_WIDTH, PLOT_HEIGHT])
trajectory_fig.set_invert_x_axis() # Because UE4 uses left-handed
# coordinate system the X
# axis in the graph is flipped
trajectory_fig.set_axis_equal() # X-Y spacing should be equal in size
# Add waypoint markers
trajectory_fig.add_graph("waypoints", window_size=waypoints_np.shape[0],
x0=waypoints_np[:,0], y0=waypoints_np[:,1],
linestyle="-", marker="", color='g')
# Add trajectory markers
trajectory_fig.add_graph("trajectory", window_size=TOTAL_EPISODE_FRAMES,
x0=[start_x]*TOTAL_EPISODE_FRAMES,
y0=[start_y]*TOTAL_EPISODE_FRAMES,
color=[1, 0.5, 0])
# Add starting position marker
trajectory_fig.add_graph("start_pos", window_size=1,
x0=[start_x], y0=[start_y],
marker=11, color=[1, 0.5, 0],
markertext="Start", marker_text_offset=1)
# Add end position marker
trajectory_fig.add_graph("end_pos", window_size=1,
x0=[waypoints_np[-1, 0]],
y0=[waypoints_np[-1, 1]],
marker="D", color='r',
markertext="End", marker_text_offset=1)
# Add car marker
trajectory_fig.add_graph("car", window_size=1,
marker="s", color='b', markertext="Car",
marker_text_offset=1)
# Add lead car information
trajectory_fig.add_graph("leadcar", window_size=1,
marker="s", color='g', markertext="Lead Car",
marker_text_offset=1)
# Add stop sign position
trajectory_fig.add_graph("stopsign", window_size=1,
x0=[stopsign_fences[0][0]], y0=[stopsign_fences[0][1]],
marker="H", color="r",
markertext="Stop Sign", marker_text_offset=1)
# Add stop sign "stop line"
trajectory_fig.add_graph("stopsign_fence", window_size=1,
x0=[stopsign_fences[0][0], stopsign_fences[0][2]],
y0=[stopsign_fences[0][1], stopsign_fences[0][3]],
color="r")
# Load parked car points
parkedcar_box_pts_np = np.array(parkedcar_box_pts)
trajectory_fig.add_graph("parkedcar_pts", window_size=parkedcar_box_pts_np.shape[0],
x0=parkedcar_box_pts_np[:,0], y0=parkedcar_box_pts_np[:,1],
linestyle="", marker="+", color='b')
# Add lookahead path
trajectory_fig.add_graph("selected_path",
window_size=INTERP_MAX_POINTS_PLOT,
x0=[start_x]*INTERP_MAX_POINTS_PLOT,
y0=[start_y]*INTERP_MAX_POINTS_PLOT,
color=[1, 0.5, 0.0],
linewidth=3)
# Add local path proposals
for i in range(NUM_PATHS):
trajectory_fig.add_graph("local_path " + str(i), window_size=200,
x0=None, y0=None, color=[0.0, 0.0, 1.0])
###
# Add 1D speed profile updater
###
forward_speed_fig =\
lp_1d.plot_new_dynamic_figure(title="Forward Speed (m/s)")
forward_speed_fig.add_graph("forward_speed",
label="forward_speed",
window_size=TOTAL_EPISODE_FRAMES)
forward_speed_fig.add_graph("reference_signal",
label="reference_Signal",
window_size=TOTAL_EPISODE_FRAMES)
# Add throttle signals graph
throttle_fig = lp_1d.plot_new_dynamic_figure(title="Throttle")
throttle_fig.add_graph("throttle",
label="throttle",
window_size=TOTAL_EPISODE_FRAMES)
# Add brake signals graph
brake_fig = lp_1d.plot_new_dynamic_figure(title="Brake")
brake_fig.add_graph("brake",
label="brake",
window_size=TOTAL_EPISODE_FRAMES)
# Add steering signals graph
steer_fig = lp_1d.plot_new_dynamic_figure(title="Steer")
steer_fig.add_graph("steer",
label="steer",
window_size=TOTAL_EPISODE_FRAMES)
# live plotter is disabled, hide windows
if not enable_live_plot:
lp_traj._root.withdraw()
lp_1d._root.withdraw()
#############################################
# Local Planner Variables
#############################################
wp_goal_index = 0
local_waypoints = None
path_validity = np.zeros((NUM_PATHS, 1), dtype=bool)
lp = local_planner.LocalPlanner(NUM_PATHS,
PATH_OFFSET,
CIRCLE_OFFSETS,
CIRCLE_RADII,
PATH_SELECT_WEIGHT,
TIME_GAP,
A_MAX,
SLOW_SPEED,
STOP_LINE_BUFFER,
PREV_BEST_PATH)
bp = behavioural_planner.BehaviouralPlanner(BP_LOOKAHEAD_BASE,
stopsign_fences,
LEAD_VEHICLE_LOOKAHEAD)
#############################################
# Scenario Execution Loop
#############################################
# Iterate the frames until the end of the waypoints is reached or
# the TOTAL_EPISODE_FRAMES is reached. The controller simulation then
# ouptuts the results to the controller output directory.
reached_the_end = False
skip_first_frame = True
# Initialize the current timestamp.
current_timestamp = start_timestamp
# Initialize collision history
prev_collision_vehicles = 0
prev_collision_pedestrians = 0
prev_collision_other = 0
for frame in range(TOTAL_EPISODE_FRAMES):
# Gather current data from the CARLA server
measurement_data, sensor_data = client.read_data()
# Update pose and timestamp
prev_timestamp = current_timestamp
current_x, current_y, current_yaw = \
get_current_pose(measurement_data)
current_speed = measurement_data.player_measurements.forward_speed
current_timestamp = float(measurement_data.game_timestamp) / 1000.0
# Wait for some initial time before starting the demo
if current_timestamp <= WAIT_TIME_BEFORE_START:
send_control_command(client, throttle=0.0, steer=0, brake=1.0)
continue
else:
current_timestamp = current_timestamp - WAIT_TIME_BEFORE_START
# Store history
x_history.append(current_x)
y_history.append(current_y)
yaw_history.append(current_yaw)
speed_history.append(current_speed)
time_history.append(current_timestamp)
# Store collision history
collided_flag,\
prev_collision_vehicles,\
prev_collision_pedestrians,\
prev_collision_other = get_player_collided_flag(measurement_data,
prev_collision_vehicles,
prev_collision_pedestrians,
prev_collision_other)
collided_flag_history.append(collided_flag)
###
# Local Planner Update:
# This will use the behavioural_planner.py and local_planner.py
# implementations that the learner will be tasked with in
# the Course 4 final project
###
# Obtain Lead Vehicle information.
lead_car_pos = []
lead_car_length = []
lead_car_speed = []
for agent in measurement_data.non_player_agents:
agent_id = agent.id
if agent.HasField('vehicle'):
lead_car_pos.append(
[agent.vehicle.transform.location.x,
agent.vehicle.transform.location.y])
lead_car_length.append(agent.vehicle.bounding_box.extent.x)
lead_car_speed.append(agent.vehicle.forward_speed)
# Execute the behaviour and local planning in the current instance
# Note that updating the local path during every controller update
# produces issues with the tracking performance (imagine everytime
# the controller tried to follow the path, a new path appears). For
# this reason, the local planner (LP) will update every X frame,
# stored in the variable LP_FREQUENCY_DIVISOR, as it is analogous
# to be operating at a frequency that is a division to the
# simulation frequency.
if frame % LP_FREQUENCY_DIVISOR == 0:
# TODO Once you have completed the prerequisite functions of each of these
# lines, you can uncomment the code below the dashed line to run the planner.
# Note that functions lower in this block often require outputs from the functions
# earlier in this block, so it may be easier to implement those first to
# get a more intuitive flow of the planner.
# In addition, some of these functions have already been implemented for you,
# but it is useful for you to understand what each function is doing.
# Before you uncomment a function, please take the time to take a look at
# it and understand what is going on. It will also help inform you on the
# flow of the planner, which in turn will help you implement the functions
# flagged for you in the TODO's.
# TODO: Uncomment each code block between the dashed lines to run the planner.
# --------------------------------------------------------------
# Compute open loop speed estimate.
print("================= start ======================")
open_loop_speed = lp._velocity_planner.get_open_loop_speed(current_timestamp - prev_timestamp)
# Calculate the goal state set in the local frame for the local planner.
# Current speed should be open loop for the velocity profile generation.
ego_state = [current_x, current_y, current_yaw, open_loop_speed]
# Set lookahead based on current speed.
bp.set_lookahead(BP_LOOKAHEAD_BASE + BP_LOOKAHEAD_TIME * open_loop_speed)
# Perform a state transition in the behavioural planner.
bp.transition_state(waypoints, ego_state, current_speed)
print("The current speed = %f" % current_speed)
# Check to see if we need to follow the lead vehicle.
bp.check_for_lead_vehicle(ego_state, lead_car_pos[1])
# Compute the goal state set from the behavioural planner's computed goal state.
goal_state_set = lp.get_goal_state_set(bp._goal_index, bp._goal_state, waypoints, ego_state)
# Calculate planned paths in the local frame.
paths, path_validity = lp.plan_paths(goal_state_set)
# Transform those paths back to the global frame.
paths = local_planner.transform_paths(paths, ego_state)
# Perform collision checking.
collision_check_array = lp._collision_checker.collision_check(paths, [parkedcar_box_pts])
# Compute the best local path.
best_index = lp._collision_checker.select_best_path_index(paths, collision_check_array, bp._goal_state)
print("The best_index = %d" % best_index)
# If no path was feasible, continue to follow the previous best path.
if best_index == None:
best_path = lp.prev_best_path
else:
best_path = paths[best_index]
lp.prev_best_path = best_path
# Compute the velocity profile for the path, and compute the waypoints.
# Use the lead vehicle to inform the velocity profile's dynamic obstacle handling.
# In this scenario, the only dynamic obstacle is the lead vehicle at index 1.
desired_speed = bp._goal_state[2]
print("The desired_speed = %f" % desired_speed)
lead_car_state = [lead_car_pos[1][0], lead_car_pos[1][1], lead_car_speed[1]]
decelerate_to_stop = bp._state == behavioural_planner.DECELERATE_TO_STOP
local_waypoints = lp._velocity_planner.compute_velocity_profile(best_path, desired_speed, ego_state, current_speed, decelerate_to_stop, lead_car_state, bp._follow_lead_vehicle)
print("================= end ======================")
# --------------------------------------------------------------
if local_waypoints != None:
# Update the controller waypoint path with the best local path.
# This controller is similar to that developed in Course 1 of this
# specialization. Linear interpolation computation on the waypoints
# is also used to ensure a fine resolution between points.
wp_distance = [] # distance array
local_waypoints_np = np.array(local_waypoints)
for i in range(1, local_waypoints_np.shape[0]):
wp_distance.append(
np.sqrt((local_waypoints_np[i, 0] - local_waypoints_np[i-1, 0])**2 +
(local_waypoints_np[i, 1] - local_waypoints_np[i-1, 1])**2))
wp_distance.append(0) # last distance is 0 because it is the distance
# from the last waypoint to the last waypoint
# Linearly interpolate between waypoints and store in a list
wp_interp = [] # interpolated values
# (rows = waypoints, columns = [x, y, v])
for i in range(local_waypoints_np.shape[0] - 1):
# Add original waypoint to interpolated waypoints list (and append
# it to the hash table)
wp_interp.append(list(local_waypoints_np[i]))
# Interpolate to the next waypoint. First compute the number of
# points to interpolate based on the desired resolution and
# incrementally add interpolated points until the next waypoint
# is about to be reached.
num_pts_to_interp = int(np.floor(wp_distance[i] /\
float(INTERP_DISTANCE_RES)) - 1)
wp_vector = local_waypoints_np[i+1] - local_waypoints_np[i]
wp_uvector = wp_vector / np.linalg.norm(wp_vector[0:2])
for j in range(num_pts_to_interp):
next_wp_vector = INTERP_DISTANCE_RES * float(j+1) * wp_uvector
wp_interp.append(list(local_waypoints_np[i] + next_wp_vector))
# add last waypoint at the end
wp_interp.append(list(local_waypoints_np[-1]))
# Update the other controller values and controls
controller.update_waypoints(wp_interp)
pass
###
# Controller Update
###
if local_waypoints != None and local_waypoints != []:
controller.update_values(current_x, current_y, current_yaw,
current_speed,
current_timestamp, frame)
controller.update_controls()
cmd_throttle, cmd_steer, cmd_brake = controller.get_commands()
else:
cmd_throttle = 0.0
cmd_steer = 0.0
cmd_brake = 0.0
# Skip the first frame or if there exists no local paths
if skip_first_frame and frame == 0:
pass
elif local_waypoints == None:
pass
else:
# Update live plotter with new feedback
trajectory_fig.roll("trajectory", current_x, current_y)
trajectory_fig.roll("car", current_x, current_y)
if lead_car_pos: # If there exists a lead car, plot it
trajectory_fig.roll("leadcar", lead_car_pos[1][0],
lead_car_pos[1][1])
forward_speed_fig.roll("forward_speed",
current_timestamp,
current_speed)
forward_speed_fig.roll("reference_signal",
current_timestamp,
controller._desired_speed)
throttle_fig.roll("throttle", current_timestamp, cmd_throttle)
brake_fig.roll("brake", current_timestamp, cmd_brake)
steer_fig.roll("steer", current_timestamp, cmd_steer)
# Local path plotter update
if frame % LP_FREQUENCY_DIVISOR == 0:
path_counter = 0
for i in range(NUM_PATHS):
# If a path was invalid in the set, there is no path to plot.
if path_validity[i]:
# Colour paths according to collision checking.
if not collision_check_array[path_counter]:
colour = 'r'
elif i == best_index:
colour = 'k'
else:
colour = 'b'
trajectory_fig.update("local_path " + str(i), paths[path_counter][0], paths[path_counter][1], colour)
path_counter += 1
else:
trajectory_fig.update("local_path " + str(i), [ego_state[0]], [ego_state[1]], 'r')
# When plotting lookahead path, only plot a number of points
# (INTERP_MAX_POINTS_PLOT amount of points). This is meant
# to decrease load when live plotting
wp_interp_np = np.array(wp_interp)
path_indices = np.floor(np.linspace(0,
wp_interp_np.shape[0]-1,
INTERP_MAX_POINTS_PLOT))
trajectory_fig.update("selected_path",
wp_interp_np[path_indices.astype(int), 0],
wp_interp_np[path_indices.astype(int), 1],
new_colour=[1, 0.5, 0.0])
# Refresh the live plot based on the refresh rate
# set by the options
if enable_live_plot and \
live_plot_timer.has_exceeded_lap_period():
lp_traj.refresh()
lp_1d.refresh()
live_plot_timer.lap()
# Output controller command to CARLA server
send_control_command(client,
throttle=cmd_throttle,
steer=cmd_steer,
brake=cmd_brake)
# Find if reached the end of waypoint. If the car is within
# DIST_THRESHOLD_TO_LAST_WAYPOINT to the last waypoint,
# the simulation will end.
dist_to_last_waypoint = np.linalg.norm(np.array([
waypoints[-1][0] - current_x,
waypoints[-1][1] - current_y]))
if dist_to_last_waypoint < DIST_THRESHOLD_TO_LAST_WAYPOINT:
reached_the_end = True
if reached_the_end:
break
# End of demo - Stop vehicle and Store outputs to the controller output
# directory.
if reached_the_end:
print("Reached the end of path. Writing to controller_output...")
else:
print("Exceeded assessment time. Writing to controller_output...")
# Stop the car
send_control_command(client, throttle=0.0, steer=0.0, brake=1.0)
# Store the various outputs
store_trajectory_plot(trajectory_fig.fig, 'trajectory.png')
store_trajectory_plot(forward_speed_fig.fig, 'forward_speed.png')
store_trajectory_plot(throttle_fig.fig, 'throttle_output.png')
store_trajectory_plot(brake_fig.fig, 'brake_output.png')
store_trajectory_plot(steer_fig.fig, 'steer_output.png')
write_trajectory_file(x_history, y_history, speed_history, time_history,
collided_flag_history)
write_collisioncount_file(collided_flag_history)
def exec_waypoint_nav_demo(args):
""" Executes waypoint navigation demo.
"""
with make_carla_client(args.host, args.port) as client:
print('Carla client connected.')
settings = make_carla_settings(args)
# Now we load these settings into the server. The server replies
# with a scene description containing the available start spots for
# the player. Here we can provide a CarlaSettings object or a
# CarlaSettings.ini file as string.
scene = client.load_settings(settings)
# Refer to the player start folder in the WorldOutliner to see the
# player start information
player_start = PLAYER_START_INDEX
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print('Starting new episode at %r...' % scene.map_name)
client.start_episode(player_start)
#############################################
# Load Configurations
#############################################
config = configparser.ConfigParser()
config.read(
os.path.join(os.path.dirname(os.path.realpath(__file__)),
'options.cfg'))
demo_opt = config['Demo Parameters']
# Get options
enable_live_plot = demo_opt.get('live_plotting', 'true').capitalize()
enable_live_plot = enable_live_plot == 'True'
live_plot_period = float(demo_opt.get('live_plotting_period', 0))
# Set options
# live plot timer
live_plot_timer = Timer(live_plot_period)
#############################################
# Load stop sign and parked vehicle parameters
# Convert to input params for LP
#############################################
# Stop sign (X(m), Y(m), Z(m), Yaw(deg))
stopsign_data = None
stopsign_fences = [] # [x0, y0, x1, y1]
with open(C4_STOP_SIGN_FILE, 'r') as stopsign_file:
next(stopsign_file) # skip header
stopsign_reader = csv.reader(stopsign_file,
delimiter=',',
quoting=csv.QUOTE_NONNUMERIC)
stopsign_data = list(stopsign_reader)
# convert to rad
for i in range(len(stopsign_data)):
stopsign_data[i][3] = stopsign_data[i][3] * np.pi / 180.0
# obtain stop sign fence points for LP
for i in range(len(stopsign_data)):
x = stopsign_data[i][0]
y = stopsign_data[i][1]
z = stopsign_data[i][2]
yaw = stopsign_data[i][3] + np.pi / 2.0 # add 90 degrees for fence
spos = np.array([[0, 0], [0, C4_STOP_SIGN_FENCELENGTH]])
rotyaw = np.array([[np.cos(yaw), np.sin(yaw)],
[-np.sin(yaw), np.cos(yaw)]])
spos_shift = np.array([[x, x], [y, y]])
spos = np.add(np.matmul(rotyaw, spos), spos_shift)
stopsign_fences.append(
[spos[0, 0], spos[1, 0], spos[0, 1], spos[1, 1]])
# Parked car(s) (X(m), Y(m), Z(m), Yaw(deg), RADX(m), RADY(m), RADZ(m))
parkedcar_data = None
parkedcar_box_pts = [] # [x,y]
with open(C4_PARKED_CAR_FILE, 'r') as parkedcar_file:
next(parkedcar_file) # skip header
parkedcar_reader = csv.reader(parkedcar_file,
delimiter=',',
quoting=csv.QUOTE_NONNUMERIC)
parkedcar_data = list(parkedcar_reader)
# convert to rad
for i in range(len(parkedcar_data)):
parkedcar_data[i][3] = parkedcar_data[i][3] * np.pi / 180.0
# obtain parked car(s) box points for LP
for i in range(len(parkedcar_data)):
x = parkedcar_data[i][0]
y = parkedcar_data[i][1]
z = parkedcar_data[i][2]
yaw = parkedcar_data[i][3]
xrad = parkedcar_data[i][4]
yrad = parkedcar_data[i][5]
zrad = parkedcar_data[i][6]
cpos = np.array([[-xrad, -xrad, -xrad, 0, xrad, xrad, xrad, 0],
[-yrad, 0, yrad, yrad, yrad, 0, -yrad, -yrad]])
rotyaw = np.array([[np.cos(yaw), np.sin(yaw)],
[-np.sin(yaw), np.cos(yaw)]])
cpos_shift = np.array([[x, x, x, x, x, x, x, x],
[y, y, y, y, y, y, y, y]])
cpos = np.add(np.matmul(rotyaw, cpos), cpos_shift)
for j in range(cpos.shape[1]):
parkedcar_box_pts.append([cpos[0, j], cpos[1, j]])
#############################################
# Load Waypoints
#############################################
waypoints_file = WAYPOINTS_FILENAME
with open(waypoints_file) as waypoints_file_handle:
waypoints = list(
csv.reader(waypoints_file_handle,
delimiter=',',
quoting=csv.QUOTE_NONNUMERIC))
#############################################
# Controller 2D Class Declaration
#############################################
# Initialize the controller.
controller = controller2d.Controller2D(waypoints)
#############################################
# Determine simulation average timestep (and total frames)
#############################################
# ensure at least one simulation is used
num_iterations = ITER_FOR_SIM_TIMESTEP
if (ITER_FOR_SIM_TIMESTEP < 1):
num_iterations = 1
measurement_data, sensor_data = client.read_data()
sim_start_stamp = measurement_data.game_timestamp / 1000.0
# send a control command to proceed to next iteration
send_control_command(client, throttle=0.0, steer=0, brake=1.0)
sim_duration = 0
for i in range(num_iterations):
# gather current data
measurement_data, sensor_data = client.read_data()
# send a control command to proceed to next iteration
send_control_command(client, throttle=0.0, steer=0, brake=1.0)
# last stamp
if i == num_iterations - 1:
sim_duration = measurement_data.game_timestamp / 1000.0 - \
sim_start_stamp
SIMULATION_TIME_STEP = sim_duration / float(num_iterations)
print("SERVER SIMULATION STEP APPROXIMATION: " + \
str(SIMULATION_TIME_STEP))
TOTAL_EPISODE_FRAMES = int((TOTAL_RUN_TIME + WAIT_TIME_BEFORE_START) /\
SIMULATION_TIME_STEP) + TOTAL_FRAME_BUFFER
#############################################
# Frame-by-Frame Iteration and Initialization
#############################################
# store pose history starting from the start position
measurement_data, sensor_data = client.read_data()
start_timestamp = measurement_data.game_timestamp / 1000.0
start_x, start_y, start_yaw = get_current_pose(measurement_data)
send_control_command(client, throttle=0.0, steer=0, brake=1.0)
x_history = [start_x]
y_history = [start_y]
yaw_history = [start_yaw]
time_history = [0]
speed_history = [0]
collided_flag_history = [False
] # assume player starts off non-collided
#############################################
# Vehicle Trajectory Live Plotting Setup
#############################################
lp_traj = lv.LivePlotter(tk_title="Trajectory Trace")
lp_1d = lv.LivePlotter(tk_title="Controls Feedback")
# Add 2D position / trajectory plot
trajectory_fig = lp_traj.plot_new_dynamic_2d_figure(
title='Vehicle Trajectory',
figsize=(FIGSIZE_X_INCHES, FIGSIZE_Y_INCHES),
edgecolor="black",
rect=[PLOT_LEFT, PLOT_BOT, PLOT_WIDTH, PLOT_HEIGHT])
trajectory_fig.set_invert_x_axis(
) # because UE4 - left-handed coord sys
trajectory_fig.set_axis_equal()
waypoints_np = np.array(waypoints)
trajectory_fig.add_graph("waypoints",
window_size=waypoints_np.shape[0],
x0=waypoints_np[:, 0],
y0=waypoints_np[:, 1],
linestyle="-",
marker="",
color='g')
trajectory_fig.add_graph("trajectory",
window_size=TOTAL_EPISODE_FRAMES,
x0=[start_x] * TOTAL_EPISODE_FRAMES,
y0=[start_y] * TOTAL_EPISODE_FRAMES,
color=[1, 0.5, 0])
trajectory_fig.add_graph("start_pos",
window_size=1,
x0=[start_x],
y0=[start_y],
marker=11,
color=[1, 0.5, 0],
markertext="Start",
marker_text_offset=1)
trajectory_fig.add_graph("end_pos",
window_size=1,
x0=[waypoints_np[-1, 0]],
y0=[waypoints_np[-1, 1]],
marker="D",
color='r',
markertext="End",
marker_text_offset=1)
trajectory_fig.add_graph("car",
window_size=1,
marker="s",
color='b',
markertext="Car",
marker_text_offset=1)
trajectory_fig.add_graph("leadcar",
window_size=1,
marker="s",
color='g',
markertext="Lead Car",
marker_text_offset=1)
trajectory_fig.add_graph("stopsign",
window_size=1,
x0=[stopsign_fences[0][0]],
y0=[stopsign_fences[0][1]],
marker="H",
color="r",
markertext="Stop Sign",
marker_text_offset=1)
trajectory_fig.add_graph(
"stopsign_fence",
window_size=1,
x0=[stopsign_fences[0][0], stopsign_fences[0][2]],
y0=[stopsign_fences[0][1], stopsign_fences[0][3]],
color="r")
# load parked car points
parkedcar_box_pts_np = np.array(parkedcar_box_pts)
trajectory_fig.add_graph("parkedcar_pts",
window_size=parkedcar_box_pts_np.shape[0],
x0=parkedcar_box_pts_np[:, 0],
y0=parkedcar_box_pts_np[:, 1],
linestyle="",
marker="+",
color='b')
for i in range(NUM_PATHS):
trajectory_fig.add_graph("local_path " + str(i),
window_size=200,
x0=None,
y0=None,
color=[0.0, 0.0, 1.0])
# Add 1D speed profile updater
forward_speed_fig =\
lp_1d.plot_new_dynamic_figure(title="Forward Speed (m/s)")
forward_speed_fig.add_graph("forward_speed",
label="forward_speed",
window_size=TOTAL_EPISODE_FRAMES)
forward_speed_fig.add_graph("reference_signal",
label="reference_Signal",
window_size=TOTAL_EPISODE_FRAMES)
# Add control signals graph
throttle_fig = lp_1d.plot_new_dynamic_figure(title="Throttle")
throttle_fig.add_graph("throttle",
label="throttle",
window_size=TOTAL_EPISODE_FRAMES)
# Add control signals graph
brake_fig = lp_1d.plot_new_dynamic_figure(title="Brake")
brake_fig.add_graph("brake",
label="brake",
window_size=TOTAL_EPISODE_FRAMES)
# Add control signals graph
steer_fig = lp_1d.plot_new_dynamic_figure(title="Steer")
steer_fig.add_graph("steer",
label="steer",
window_size=TOTAL_EPISODE_FRAMES)
# live plotter is disabled, hide windows
if not enable_live_plot:
lp_traj._root.withdraw()
lp_1d._root.withdraw()
# Initialize the planning variables.
wp_goal_index = 0
lp = local_planner.LocalPlanner(NUM_PATHS, PATH_OFFSET, CIRCLE_OFFSETS,
CIRCLE_RADII, PATH_SELECT_WEIGHT,
TIME_GAP, A_MAX, SLOW_SPEED,
STOP_LINE_BUFFER)
bp = behavioural_planner.BehaviouralPlanner(BP_LOOKAHEAD_BASE,
stopsign_fences,
LEAD_VEHICLE_LOOKAHEAD)
# Initialize the current timestamp.
current_timestamp = start_timestamp
# Iterate through the frames until the end of the waypoints are reached.
prev_collision_vehicles = 0
prev_collision_pedestrians = 0
prev_collision_other = 0
reached_the_end = False
skip_first_frame = True
local_waypoints = None
path_validity = np.zeros((NUM_PATHS, 1), dtype=bool)
for frame in range(TOTAL_EPISODE_FRAMES):
# Gather current measurement data.
measurement_data, sensor_data = client.read_data()
# Update pose and timestamp.
prev_timestamp = current_timestamp
current_timestamp = float(measurement_data.game_timestamp) / 1000.0
current_x, current_y, current_yaw = \
get_current_pose(measurement_data)
current_speed = measurement_data.player_measurements.forward_speed
# Wait for some initial time before starting the demo.
if current_timestamp <= WAIT_TIME_BEFORE_START:
send_control_command(client, throttle=0.0, steer=0, brake=1.0)
continue
# Store state history.
x_history.append(current_x)
y_history.append(current_y)
yaw_history.append(current_yaw)
speed_history.append(current_speed)
time_history.append(current_timestamp)
collided_flag,\
prev_collision_vehicles,\
prev_collision_pedestrians,\
prev_collision_other = get_player_collided_flag(measurement_data,
prev_collision_vehicles,
prev_collision_pedestrians,
prev_collision_other)
collided_flag_history.append(collided_flag)
# Obtain lead vehicle information.
lead_car_pos = []
lead_car_length = []
lead_car_speed = []
for agent in measurement_data.non_player_agents:
agent_id = agent.id # unique id of the agent
if agent.HasField('vehicle'):
lead_car_pos.append([
agent.vehicle.transform.location.x,
agent.vehicle.transform.location.y
])
lead_car_length.append(agent.vehicle.bounding_box.extent.x)
lead_car_speed.append(agent.vehicle.forward_speed)
if frame % 2 == 0:
# TODO Once you have completed the prerequisite functions of each of these
# lines, you can uncomment the code below to run the planner. Note that
# functions lower in this block often require outputs from the functions
# earlier in this block, so it may be easier to implement those first to
# get a more intuitive flow of the planner.
# In addition, some of these functions have already been implemented for you,
# but it is useful for you to understand what each function is doing.
# Before you uncomment a function, please take the time to take a look at
# it and understand what is going on. It will also help inform you on the
# flow of the planner, which in turn will help you implement the functions
# flagged for you in the TODO's.
# Compute open loop speed estimate.
open_loop_speed = lp._velocity_planner.get_open_loop_speed(
current_timestamp - prev_timestamp)
# Calculate the goal state set in the local frame for the local planner.
# Current speed should be open loop for the velocity profile generation.
ego_state = [
current_x, current_y, current_yaw, open_loop_speed
]
# Set lookahead based on current speed.
bp.set_lookahead(BP_LOOKAHEAD_BASE +
BP_LOOKAHEAD_TIME * open_loop_speed)
# Perform a state transition in the behavioural planner.
bp.transition_state(waypoints, ego_state, current_speed)
# Check to see if we need to follow the lead vehicle.
bp.check_for_lead_vehicle(ego_state, lead_car_pos[1])
# Compute the goal state set from the behavioural planner's computed goal state.
goal_state_set = lp.get_goal_state_set(bp._goal_index,
bp._goal_state,
waypoints, ego_state)
# Calculate planned paths in the local frame.
paths, path_validity = lp.plan_paths(goal_state_set)
# Transform those paths back to the global frame.
paths = local_planner.transform_paths(paths, ego_state)
# Perform collision checking.
collision_check_array = lp._collision_checker.collision_check(
paths, [parkedcar_box_pts])
# Compute the best local path.
best_index = lp._collision_checker.select_best_path_index(
paths, collision_check_array, bp._goal_state)
# If no path was feasible, continue to follow the previous best path.
if best_index == None:
best_path = lp._prev_best_path
else:
best_path = paths[best_index]
lp._prev_best_path = best_path
# Compute the velocity profile for the path, and compute the waypoints.
# Use the lead vehicle to inform the velocity profile's dynamic obstacle handling.
# In this scenario, the only dynamic obstacle is the lead vehicle at index 1.
desired_speed = bp._goal_state[2]
lead_car_state = [
lead_car_pos[1][0], lead_car_pos[1][1], lead_car_speed[1]
]
decelerate_to_stop = bp._state == behavioural_planner.DECELERATE_TO_STOP
local_waypoints = lp._velocity_planner.compute_velocity_profile(
best_path, desired_speed, ego_state, current_speed,
decelerate_to_stop, lead_car_state,
bp._follow_lead_vehicle)
# Update the controller waypoint path with the best local path.
# This controller is similar to that developed in Course 1 of this
# specialization.
controller.update_waypoints(local_waypoints)
# Update the controller with the sensor values, then compute the control commands
# for this iteration.
if local_waypoints != None:
controller.update_values(current_x, current_y, current_yaw,
current_speed, current_timestamp,
frame)
controller.update_controls()
cmd_throttle, cmd_steer, cmd_brake = controller.get_commands()
print('cmd_throttle, cmd_steer, cmd_brake: ', cmd_throttle,
cmd_steer, cmd_brake)
else:
cmd_throttle = 0.0
cmd_steer = 0.0
cmd_brake = 0.0
# Skip the first frame
if skip_first_frame and frame == 0:
pass
elif local_waypoints == None:
pass
else:
# Update plot
trajectory_fig.roll("trajectory", current_x, current_y)
trajectory_fig.roll("car", current_x, current_y)
if lead_car_pos:
trajectory_fig.roll("leadcar", lead_car_pos[1][0],
lead_car_pos[1][1])
forward_speed_fig.roll("forward_speed", current_timestamp,
current_speed)
forward_speed_fig.roll("reference_signal", current_timestamp,
controller._desired_speed)
throttle_fig.roll("throttle", current_timestamp, cmd_throttle)
brake_fig.roll("brake", current_timestamp, cmd_brake)
steer_fig.roll("steer", current_timestamp, cmd_steer)
if frame % 2 == 0:
path_counter = 0
for i in range(NUM_PATHS):
# If a path was invalid in the set, there is no path to plot.
if path_validity[i]:
# Colour paths according to collision checking.
if not collision_check_array[path_counter]:
colour = 'r'
elif i == best_index:
colour = 'k'
else:
colour = 'b'
trajectory_fig.update("local_path " + str(i),
paths[path_counter][0],
paths[path_counter][1],
colour)
path_counter += 1
else:
trajectory_fig.update("local_path " + str(i),
[ego_state[0]],
[ego_state[1]], 'r')
if enable_live_plot and \
live_plot_timer.has_exceeded_lap_period():
lp_traj.refresh()
lp_1d.refresh()
live_plot_timer.lap()
# Output controller command to CARLA server
send_control_command(client,
throttle=cmd_throttle,
steer=cmd_steer,
brake=cmd_brake)
# Find if reached the end of waypoint
dist_to_last_waypoint = np.linalg.norm(
np.array([
waypoints[-1][0] - current_x, waypoints[-1][1] - current_y
]))
if dist_to_last_waypoint < DIST_THRESHOLD_TO_LAST_WAYPOINT:
reached_the_end = True
if reached_the_end:
break
# End of demo - stop vehicle and store outputs
if reached_the_end:
print("Reached the end of path. Writing to controller_output...")
else:
print("Exceeded assessment time. Writing to controller_output...")
send_control_command(client, throttle=0.0, steer=0.0, brake=1.0)
store_trajectory_plot(trajectory_fig.fig, 'trajectory.png')
store_trajectory_plot(forward_speed_fig.fig, 'forward_speed.png')
store_trajectory_plot(throttle_fig.fig, 'throttle_output.png')
store_trajectory_plot(brake_fig.fig, 'brake_output.png')
store_trajectory_plot(steer_fig.fig, 'steer_output.png')
write_trajectory_file(x_history, y_history, speed_history,
time_history, collided_flag_history)
write_collisioncount_file(collided_flag_history)
