def draw_trajectory(planning_route):
"""
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
waypoints_x = list()
waypoints_y = list()
for w in planning_route:
pos = w.transform.location
waypoints_x.append(pos.x)
waypoints_y.append(pos.y)
trajectory_fig.add_graph("waypoints",
window_size=len(waypoints_x),
x0=waypoints_x,
y0=waypoints_y,
linestyle="-",
marker="",
color='g')
# Add trajectory markers
start_x, start_y = waypoints_x[0], waypoints_y[0]
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_x[-1]],
y0=[waypoints_y[-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 1D speed profile updater pass: forward speed, throttle, steer --- #
forward_speed_fig = lp_1d.plot_new_dynamic_figure(
title="Forward Speed (km/h)")
forward_speed_fig.add_graph("forward_speed",
label="forward_speed",
window_size=TOTAL_EPISODE_FRAMES)
# # each waypoints should have the reference signal ...
# 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 steering signals graph
steer_fig = lp_1d.plot_new_dynamic_figure(title="Steer (Degree)")
steer_fig.add_graph("steer",
label="steer",
window_size=TOTAL_EPISODE_FRAMES)
return lp_traj, lp_1d, trajectory_fig, forward_speed_fig, throttle_fig, steer_fig
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
#############################################
# 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 Waypoints
#############################################
# Opens the waypoint file and stores it to "waypoints"
waypoints_file = WAYPOINTS_FILENAME
waypoints_np = None
with open(waypoints_file) as waypoints_file_handle:
waypoints = list(
csv.reader(waypoints_file_handle,
delimiter=',',
quoting=csv.QUOTE_NONNUMERIC))
waypoints_np = np.array(waypoints)
# Because the waypoints are discrete and our controller performs better
# with a continuous path, here we will send a subset of the waypoints
# within some lookahead distance from the closest point to the vehicle.
# Interpolating between each waypoint will provide a finer resolution
# path and make it more "continuous". A simple linear interpolation
# is used as a preliminary method to address this issue, though it is
# better addressed with better interpolation methods (spline
# interpolation, for example).
# More appropriate interpolation methods will not be used here for the
# sake of demonstration on what effects discrete paths can have on
# the controller. It is made much more obvious with linear
# interpolation, because in a way part of the path will be continuous
# while the discontinuous parts (which happens at the waypoints) will
# show just what sort of effects these points have on the controller.
# Can you spot these during the simulation? If so, how can you further
# reduce these effects?
# Linear interpolation computations
# Compute a list of distances between waypoints
wp_distance = [] # distance array
for i in range(1, waypoints_np.shape[0]):
wp_distance.append(
np.sqrt((waypoints_np[i, 0] - waypoints_np[i - 1, 0])**2 +
(waypoints_np[i, 1] - 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])
wp_interp_hash = [] # hash table which indexes waypoints_np
# to the index of the waypoint in wp_interp
interp_counter = 0 # counter for current interpolated point index
for i in range(waypoints_np.shape[0] - 1):
# Add original waypoint to interpolated waypoints list (and append
# it to the hash table)
wp_interp.append(list(waypoints_np[i]))
wp_interp_hash.append(interp_counter)
interp_counter += 1
# 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 = waypoints_np[i + 1] - waypoints_np[i]
wp_uvector = wp_vector / np.linalg.norm(wp_vector)
for j in range(num_pts_to_interp):
next_wp_vector = INTERP_DISTANCE_RES * float(j +
1) * wp_uvector
wp_interp.append(list(waypoints_np[i] + next_wp_vector))
interp_counter += 1
# add last waypoint at the end
wp_interp.append(list(waypoints_np[-1]))
wp_interp_hash.append(interp_counter)
interp_counter += 1
#############################################
# 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_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]
#############################################
# 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 lookahead path
#trajectory_fig.add_graph("lookahead_path",
# window_size=INTERP_MAX_POINTS_PLOT,
# x0=[start_x]*INTERP_MAX_POINTS_PLOT,
# y0=[start_y]*INTERP_MAX_POINTS_PLOT,
# color=[0, 0.7, 0.7],
# linewidth=4)
# 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 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()
# 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
closest_index = 0 # Index of waypoint that is currently closest to
# the car (assumed to be the first index)
closest_distance = 0 # Closest distance of closest waypoint to car
for frame in range(TOTAL_EPISODE_FRAMES):
# Gather current data from the CARLA server
measurement_data, sensor_data = client.read_data()
# Update pose, 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)
###
# Controller update (this uses the controller2d.py implementation)
###
# To reduce the amount of waypoints sent to the controller,
# provide a subset of waypoints that are within some
# lookahead distance from the closest point to the car. Provide
# a set of waypoints behind the car as well.
# Find closest waypoint index to car. First increment the index
# from the previous index until the new distance calculations
# are increasing. Apply the same rule decrementing the index.
# The final index should be the closest point (it is assumed that
# the car will always break out of instability points where there
# are two indices with the same minimum distance, as in the
# center of a circle)
closest_distance = np.linalg.norm(
np.array([
waypoints_np[closest_index, 0] - current_x,
waypoints_np[closest_index, 1] - current_y
]))
new_distance = closest_distance
new_index = closest_index
while new_distance <= closest_distance:
closest_distance = new_distance
closest_index = new_index
new_index += 1
if new_index >= waypoints_np.shape[0]: # End of path
break
new_distance = np.linalg.norm(
np.array([
waypoints_np[new_index, 0] - current_x,
waypoints_np[new_index, 1] - current_y
]))
new_distance = closest_distance
new_index = closest_index
while new_distance <= closest_distance:
closest_distance = new_distance
closest_index = new_index
new_index -= 1
if new_index < 0: # Beginning of path
break
new_distance = np.linalg.norm(
np.array([
waypoints_np[new_index, 0] - current_x,
waypoints_np[new_index, 1] - current_y
]))
# Once the closest index is found, return the path that has 1
# waypoint behind and X waypoints ahead, where X is the index
# that has a lookahead distance specified by
# INTERP_LOOKAHEAD_DISTANCE
waypoint_subset_first_index = closest_index - 1
if waypoint_subset_first_index < 0:
waypoint_subset_first_index = 0
waypoint_subset_last_index = closest_index
total_distance_ahead = 0
while total_distance_ahead < INTERP_LOOKAHEAD_DISTANCE:
total_distance_ahead += wp_distance[waypoint_subset_last_index]
waypoint_subset_last_index += 1
if waypoint_subset_last_index >= waypoints_np.shape[0]:
waypoint_subset_last_index = waypoints_np.shape[0] - 1
break
# Use the first and last waypoint subset indices into the hash
# table to obtain the first and last indicies for the interpolated
# list. Update the interpolated waypoints to the controller
# for the next controller update.
new_waypoints = \
wp_interp[wp_interp_hash[waypoint_subset_first_index]:\
wp_interp_hash[waypoint_subset_last_index] + 1]
controller.update_waypoints(new_waypoints)
# Update the other controller values and controls
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()
# Skip the first frame (so the controller has proper outputs)
if skip_first_frame and frame == 0:
pass
else:
# Update live plotter with new feedback
#trajectory_fig.roll("trajectory", current_x, current_y)
#trajectory_fig.roll("car", current_x, current_y)
# 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
new_waypoints_np = np.array(new_waypoints)
path_indices = np.floor(
np.linspace(0, new_waypoints_np.shape[0] - 1,
INTERP_MAX_POINTS_PLOT))
#trajectory_fig.update("lookahead_path",
# new_waypoints_np[path_indices.astype(int), 0],
# new_waypoints_np[path_indices.astype(int), 1],
# new_colour=[0, 0.7, 0.7])
#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)
# 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)
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)
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, camera2 = 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
#############################################
# 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 Waypoints
#############################################
# Opens the waypoint file and stores it to "waypoints"
camera_to_car_transform = camera2.get_unreal_transform()
carla_utils_obj = segmentationobj.carla_utils(intrinsic)
measurement_data, sensor_data = client.read_data()
measurements = measurement_data
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
image_depth = to_rgb_array(sensor_data['CameraDepth'])
world_transform = Transform(measurements.player_measurements.transform)
im_bgr = cv2.cvtColor(image_RGB, cv2.COLOR_RGB2BGR)
pos_vector = ([
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
])
fdfd = str(world_transform)
sdsd = fdfd[1:-1].split('\n')
new = []
for i in sdsd:
dd = i[:-1].lstrip('[ ')
ff = re.sub("\s+", ",", dd.strip())
gg = ff.split(',')
new.append([float(i) for i in gg])
newworld = np.array(new)
fdfd = str(camera_to_car_transform)
sdsd = fdfd[1:-1].split('\n')
new = []
for i in sdsd:
dd = i[:-1].lstrip('[ ')
ff = re.sub("\s+", ",", dd.strip())
gg = ff.split(',')
new.append([float(i) for i in gg])
newcam = np.array(new)
extrinsic = np.matmul(newworld, newcam)
#print("dfjdfjkjfdkf",extrinsic)
get_2d_point, pointsmid, res_img = carla_utils_obj.run_seg(
im_bgr, extrinsic, pos_vector)
#print(get_2d_point)
#dis1=((get_2d_point[1]-pointsmid[0][1]),(get_2d_point[0]-pointsmid[0][0]))
#dis2=((get_2d_point[1]-pointsmid[1][1]),(get_2d_point[0]-pointsmid[1][0]))
#dis3=((get_2d_point[1]-pointsmid[2][1]),(get_2d_point[0]-pointsmid[2][0]))
#dis4=((get_2d_point[1]-pointsmid[3][1]),(get_2d_point[0]-pointsmid[3][0]))
#flagbox=darknet_proper_fps.performDetect(im_bgr,thresh,configPath, weightPath, metaPath, showImage, makeImageOnly, initOnly,pointsmid)
depth1 = image_depth[int(pointsmid[0][0]), int(pointsmid[0][1])]
depth2 = image_depth[int(pointsmid[1][0]), int(pointsmid[1][1])]
depth3 = image_depth[int(pointsmid[2][0]), int(pointsmid[2][1])]
depth4 = image_depth[int(pointsmid[3][0]), int(pointsmid[3][1])]
scale1 = depth1[0] + depth1[1] * 256 + depth1[2] * 256 * 256
scale1 = scale1 / ((256 * 256 * 256) - 1)
depth1 = scale1 * 1000
pos2d1 = np.array([(WINDOW_WIDTH - pointsmid[0][1]) * depth1,
(WINDOW_HEIGHT - pointsmid[0][0]) * depth1, depth1])
first1 = np.dot(np.linalg.inv(intrinsic), pos2d1)
first1 = np.append(first1, 1)
sec1 = np.matmul((extrinsic), first1)
scale2 = depth2[0] + depth2[1] * 256 + depth2[2] * 256 * 256
scale2 = scale2 / ((256 * 256 * 256) - 1)
depth2 = scale2 * 1000
pos2d2 = np.array([(WINDOW_WIDTH - pointsmid[1][1]) * depth2,
(WINDOW_HEIGHT - pointsmid[1][0]) * depth2, depth2])
first2 = np.dot(np.linalg.inv(intrinsic), pos2d2)
first2 = np.append(first2, 1)
sec2 = np.matmul((extrinsic), first2)
scale3 = depth3[0] + depth3[1] * 256 + depth3[2] * 256 * 256
scale3 = scale3 / ((256 * 256 * 256) - 1)
depth3 = scale3 * 1000
pos2d3 = np.array([(WINDOW_WIDTH - pointsmid[2][1]) * depth3,
(WINDOW_HEIGHT - pointsmid[2][0]) * depth3, depth3])
first3 = np.dot(np.linalg.inv(intrinsic), pos2d3)
first3 = np.append(first3, 1)
sec3 = np.matmul((extrinsic), first3)
scale4 = depth4[0] + depth4[1] * 256 + depth4[2] * 256 * 256
scale4 = scale4 / ((256 * 256 * 256) - 1)
depth4 = scale4 * 1000
pos2d4 = np.array([(WINDOW_WIDTH - pointsmid[3][1]) * depth4,
(WINDOW_HEIGHT - pointsmid[3][0]) * depth4, depth4])
first4 = np.dot(np.linalg.inv(intrinsic), pos2d4)
first4 = np.append(first4, 1)
sec4 = np.matmul((extrinsic), first4)
depth = image_depth[int(get_2d_point[0]), int(get_2d_point[1])]
scale = depth[0] + depth[1] * 256 + depth[2] * 256 * 256
scale = scale / ((256 * 256 * 256) - 1)
depth = scale * 1000
pos2d = np.array([(WINDOW_WIDTH - get_2d_point[1]) * depth,
(WINDOW_HEIGHT - get_2d_point[0]) * depth, depth])
first = np.dot(np.linalg.inv(intrinsic), pos2d)
first = np.append(first, 1)
sec = np.matmul((extrinsic), first)
print("present", pos_vector)
print('goal-', sec)
pos_vector[2] = 4.0
ini = pos_vector
goal = list(sec)
goal[2] = 1.08
aa = ini[0]
dec = abs(ini[1] - goal[1]) / abs(aa - (goal[0]))
f1 = open(WAYPOINTS_FILENAME, 'a+')
for i in range(int(goal[0]), int(aa)):
# print(int(goal[0])-i)
if abs((int(aa) - i)) < 10:
ini = [ini[0] - 1, ini[1] + dec, ini[2] - 0.03]
else:
ini = [ini[0] - 1, ini[1] + dec, ini[2]]
#if i<int(aa)-1:
f1.write(
str(ini[0]) + ',' + str(ini[1]) + ',' + str(ini[2]) + '\n')
#else:
#f1.write(str(ini[0])+','+str(ini[1])+','+str(ini[2]))
waypoints_file = WAYPOINTS_FILENAME
waypoints_np = None
with open(waypoints_file) as waypoints_file_handle:
waypoints = list(
csv.reader(waypoints_file_handle,
delimiter=',',
quoting=csv.QUOTE_NONNUMERIC))
waypoints_np = np.array(waypoints)
#print("dfjdjfhjdhfjdfhjdfdjdfdufh",waypoints_np)
print((waypoints_np))
# Because the waypoints are discrete and our controller performs better
# with a continuous path, here we will send a subset of the waypoints
# within some lookahead distance from the closest point to the vehicle.
# Interpolating between each waypoint will provide a finer resolution
# path and make it more "continuous". A simple linear interpolation
# is used as a preliminary method to address this issue, though it is
# better addressed with better interpolation methods (spline
# interpolation, for example).
# More appropriate interpolation methods will not be used here for the
# sake of demonstration on what effects discrete paths can have on
# the controller. It is made much more obvious with linear
# interpolation, because in a way part of the path will be continuous
# while the discontinuous parts (which happens at the waypoints) will
# show just what sort of effects these points have on the controller.
# Can you spot these during the simulation? If so, how can you further
# reduce these effects?
# Linear interpolation computations
# Compute a list of distances between waypoints
wp_distance = [] # distance array
for i in range(1, waypoints_np.shape[0]):
wp_distance.append(
np.sqrt((waypoints_np[i, 0] - waypoints_np[i - 1, 0])**2 +
(waypoints_np[i, 1] - 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])
wp_interp_hash = [] # hash table which indexes waypoints_np
# to the index of the waypoint in wp_interp
interp_counter = 0 # counter for current interpolated point index
for i in range(waypoints_np.shape[0] - 1):
# Add original waypoint to interpolated waypoints list (and append
# it to the hash table)
wp_interp.append(list(waypoints_np[i]))
wp_interp_hash.append(interp_counter)
interp_counter += 1
# 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 = waypoints_np[i + 1] - waypoints_np[i]
wp_uvector = wp_vector / np.linalg.norm(wp_vector)
for j in range(num_pts_to_interp):
next_wp_vector = INTERP_DISTANCE_RES * float(j +
1) * wp_uvector
wp_interp.append(list(waypoints_np[i] + next_wp_vector))
interp_counter += 1
# add last waypoint at the end
wp_interp.append(list(waypoints_np[-1]))
wp_interp_hash.append(interp_counter)
interp_counter += 1
#############################################
# 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.
sim_start_stamp = measurement_data.game_timestamp / 1000.0
# Send a control command to proceed to next iteration.
#print("dddddddddddddddddddddddddddddddddddddddddddddd",sim_start_stamp)
# 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_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]
#############################################
# 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 lookahead path
trajectory_fig.add_graph("lookahead_path",
window_size=INTERP_MAX_POINTS_PLOT,
x0=[start_x] * INTERP_MAX_POINTS_PLOT,
y0=[start_y] * INTERP_MAX_POINTS_PLOT,
color=[0, 0.7, 0.7],
linewidth=4)
# 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 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()
# 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
closest_index = 0 # Index of waypoint that is currently closest to
# the car (assumed to be the first index)
closest_distance = 0 # Closest distance of closest waypoint to car
counter = 0
#print("dssssssssssssssssssssssssssssssssssssssssssssssssssssssss",TOTAL_EPISODE_FRAMES)
for frame in range(TOTAL_EPISODE_FRAMES):
# Gather current data from the CARLA server
measurement_data, sensor_data = client.read_data()
#print("lllllllllllllllllllllllllll",len(waypoints_np),waypoints_np[-1])
#update_pts=list(waypoints_np[-1])
# Update pose, 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
#print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx",current_timestamp)
# 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)
counter += 1
#flagbox=darknet_proper_fps.performDetect(res_img,thresh,configPath, weightPath, metaPath, showImage, makeImageOnly, initOnly,pointsmid)
continue
else:
current_timestamp = current_timestamp - WAIT_TIME_BEFORE_START
#print("sdmskdkfjdkfjdjksf",counter)
#for i in range(1,5):
update_pts = [list(sec1), list(sec2), list(sec3), list(sec4)]
pts_2d_ls = []
for i in range(len(update_pts)):
world_coord = np.asarray(update_pts[i]).reshape(4, -1)
# print("world_coord.shape",world_coord.shape)
# world_coord = np.array([[250.0 ,129.0 ,38.0 ,1.0]]).reshape(4,-1)
world_transform = Transform(
measurement_data.player_measurements.transform)
fdfd = str(world_transform)
sdsd = fdfd[1:-1].split('\n')
new = []
for i in sdsd:
dd = i[:-1].lstrip('[ ')
ff = re.sub("\s+", ",", dd.strip())
gg = ff.split(',')
new.append([float(i) for i in gg])
newworld = np.array(new)
extrinsic = np.matmul(newworld, newcam)
cam_coord = np.linalg.inv(extrinsic) @ world_coord
img_coord = intrinsic @ cam_coord[:3]
img_coord = np.array([
img_coord[0] / img_coord[2], img_coord[1] / img_coord[2],
img_coord[2]
])
if (img_coord[2] > 0):
x_2d = WINDOW_WIDTH - img_coord[0]
y_2d = WINDOW_HEIGHT - img_coord[1]
pts_2d_ls.append([x_2d, y_2d])
#x_diff=(pts_2d_ls[0]-get_2d_point[1])
#y_diff=(pts_2d_ls[1]-get_2d_point[0])
#print("sdsdjsdsjdksjdskdjk",x_diff,y_diff,pts_2d_ls,get_2d_point)
#get_2d_point=[pts_2d_ls[1],pts_2d_ls[0]]
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
im_bgr = cv2.cvtColor(image_RGB, cv2.COLOR_RGB2BGR)
image_depth = to_rgb_array(sensor_data['CameraDepth'])
counter += 1
if counter == 0:
flagbox = False
else:
#pointsmid[0][1]=pointsmid[0][1]+x_diff
#pointsmid[0][0]=pointsmid[0][0]+y_diff
#pointsmid[1][1]=pointsmid[1][1]+x_diff
#pointsmid[1][0]=pointsmid[1][0]+y_diff
#pointsmid[2][1]=pointsmid[2][1]+x_diff
#pointsmid[2][0]=pointsmid[2][0]+y_diff
#pointsmid[3][1]=pointsmid[3][1]+x_diff
#pointsmid[3][0]=pointsmid[3][0]+y_diff
try:
pointsmid[0][1] = pts_2d_ls[0][0]
pointsmid[0][0] = pts_2d_ls[0][1]
pointsmid[1][1] = pts_2d_ls[1][0]
pointsmid[1][0] = pts_2d_ls[1][1]
pointsmid[2][1] = pts_2d_ls[2][0]
pointsmid[2][0] = pts_2d_ls[2][1]
pointsmid[3][1] = pts_2d_ls[3][0]
pointsmid[3][0] = pts_2d_ls[3][1]
disbox = False
flagbox = darknet_proper_fps.performDetect(
im_bgr, thresh, configPath, weightPath, metaPath,
showImage, makeImageOnly, initOnly, pointsmid, disbox)
except Exception as e:
disbox = True
flagbox = darknet_proper_fps.performDetect(
im_bgr, thresh, configPath, weightPath, metaPath,
showImage, makeImageOnly, initOnly, pointsmid, disbox)
if flagbox != False:
midofpts = [(pointsmid[1][1] + pointsmid[0][1]) / 2,
(pointsmid[1][0] + pointsmid[0][0]) / 2]
depthflag = image_depth[int(flagbox[1]),
int(flagbox[0])]
depthpts = image_depth[int(midofpts[1]),
int(midofpts[0])]
print(depthflag, depthpts)
#cv2.circle(im_bgr,(int(flagbox[0]),int(flagbox[1])), 5, (255,0,255), -1)
#cv2.circle(im_bgr,(int(midofpts[0]),int(midofpts[1])), 5, (0,0,255), -1)
cv2.imwrite('./seg_out/{}_zz.jpg'.format(frame),
im_bgr)
scalenew = depthflag[
0] + depthflag[1] * 256 + depthflag[2] * 256 * 256
scalenew = scalenew / ((256 * 256 * 256) - 1)
depthflag = scalenew * 1000
scalenew = depthpts[
0] + depthpts[1] * 256 + depthpts[2] * 256 * 256
scalenew = scalenew / ((256 * 256 * 256) - 1)
depthpts = scalenew * 1000
diff = abs(depthflag - depthpts)
print("entereeeeeeeeeeeeeeeeeeeeeeeeeeeeherree", diff)
if diff < 10:
flagbox = True
else:
flagbox = False
print(e)
print("fffffffffffffffffff", flagbox)
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)
if flagbox == False:
# Store history
###
# Controller update (this uses the controller2d.py implementation)
###
# To reduce the amount of waypoints sent to the controller,
# provide a subset of waypoints that are within some
# lookahead distance from the closest point to the car. Provide
# a set of waypoints behind the car as well.
# Find closest waypoint index to car. First increment the index
# from the previous index until the new distance calculations
# are increasing. Apply the same rule decrementing the index.
# The final index should be the closest point (it is assumed that
# the car will always break out of instability points where there
# are two indices with the same minimum distance, as in the
# center of a circle)
closest_distance = np.linalg.norm(
np.array([
waypoints_np[closest_index, 0] - current_x,
waypoints_np[closest_index, 1] - current_y
]))
new_distance = closest_distance
new_index = closest_index
while new_distance <= closest_distance:
closest_distance = new_distance
closest_index = new_index
new_index += 1
if new_index >= waypoints_np.shape[0]: # End of path
break
new_distance = np.linalg.norm(
np.array([
waypoints_np[new_index, 0] - current_x,
waypoints_np[new_index, 1] - current_y
]))
new_distance = closest_distance
new_index = closest_index
while new_distance <= closest_distance:
closest_distance = new_distance
closest_index = new_index
new_index -= 1
if new_index < 0: # Beginning of path
break
new_distance = np.linalg.norm(
np.array([
waypoints_np[new_index, 0] - current_x,
waypoints_np[new_index, 1] - current_y
]))
# Once the closest index is found, return the path that has 1
# waypoint behind and X waypoints ahead, where X is the index
# that has a lookahead distance specified by
# INTERP_LOOKAHEAD_DISTANCE
waypoint_subset_first_index = closest_index - 1
if waypoint_subset_first_index < 0:
waypoint_subset_first_index = 0
waypoint_subset_last_index = closest_index
total_distance_ahead = 0
while total_distance_ahead < INTERP_LOOKAHEAD_DISTANCE:
total_distance_ahead += wp_distance[
waypoint_subset_last_index]
waypoint_subset_last_index += 1
if waypoint_subset_last_index >= waypoints_np.shape[0]:
waypoint_subset_last_index = waypoints_np.shape[0] - 1
break
# Use the first and last waypoint subset indices into the hash
# table to obtain the first and last indicies for the interpolated
# list. Update the interpolated waypoints to the controller
# for the next controller update.
new_waypoints = \
wp_interp[wp_interp_hash[waypoint_subset_first_index]:\
wp_interp_hash[waypoint_subset_last_index] + 1]
controller.update_waypoints(new_waypoints)
# Update the other controller values and controls
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()
# Skip the first frame (so the controller has proper outputs)
if skip_first_frame and frame == 0:
pass
else:
# Update live plotter with new feedback
trajectory_fig.roll("trajectory", current_x, current_y)
trajectory_fig.roll("car", current_x, current_y)
# 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
new_waypoints_np = np.array(new_waypoints)
path_indices = np.floor(
np.linspace(0, new_waypoints_np.shape[0] - 1,
INTERP_MAX_POINTS_PLOT))
trajectory_fig.update(
"lookahead_path",
new_waypoints_np[path_indices.astype(int), 0],
new_waypoints_np[path_indices.astype(int), 1],
new_colour=[0, 0.7, 0.7])
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)
# 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, ###########changed waypoints[-1]
waypoints[-1][1] - current_y
]))
if dist_to_last_waypoint < DIST_THRESHOLD_TO_LAST_WAYPOINT:
reached_the_end = True
if reached_the_end:
break
else:
send_control_command(client,
throttle=0.0,
steer=0.0,
brake=1.0)
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...")
send_control_command(client, throttle=0.0, steer=0.0, brake=1.0)
else:
print("stop!!!!.")
# 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)
def __init__(self, waypoints_np, start_x, start_y, TOTAL_EPISODE_FRAMES,
INTERP_MAX_POINTS_PLOT, enable_live_plot):
self.waypoints_np = waypoints_np
self.start_y = start_y
self.start_x = start_x
self.TOTAL_EPISODE_FRAMES = TOTAL_EPISODE_FRAMES
self.INTERP_MAX_POINTS_PLOT = INTERP_MAX_POINTS_PLOT
self.FIGSIZE_X_INCHES = 8 # x figure size of feedback in inches
self.FIGSIZE_Y_INCHES = 8 # y figure size of feedback in inches
self.PLOT_LEFT = 0.1 # in fractions of figure width and height
self.PLOT_BOT = 0.1
self.PLOT_WIDTH = 0.8
self.PLOT_HEIGHT = 0.8
self.lp_traj = lv.LivePlotter(tk_title="Trajectory Trace")
self.lp_1d = lv.LivePlotter(tk_title="Controls Feedback")
###
# Add 2D position / trajectory plot
###
self.trajectory_fig = self.lp_traj.plot_new_dynamic_2d_figure(
title='Vehicle Trajectory',
figsize=(self.FIGSIZE_X_INCHES, self.FIGSIZE_Y_INCHES),
edgecolor="black",
rect=[
self.PLOT_LEFT, self.PLOT_BOT, self.PLOT_WIDTH,
self.PLOT_HEIGHT
])
self.trajectory_fig.set_invert_x_axis() # Because UE4 uses left-handed
# coordinate system the X
# axis in the graph is flipped
self.trajectory_fig.set_axis_equal(
) # X-Y spacing should be equal in size
# Add waypoint markers
self.trajectory_fig.add_graph("waypoints",
window_size=self.waypoints_np.shape[0],
x0=self.waypoints_np[:, 0],
y0=self.waypoints_np[:, 1],
linestyle="-",
marker="",
color='g')
# Add trajectory markers
self.trajectory_fig.add_graph(
"trajectory",
window_size=self.TOTAL_EPISODE_FRAMES,
x0=[self.start_x] * self.TOTAL_EPISODE_FRAMES,
y0=[self.start_y] * self.TOTAL_EPISODE_FRAMES,
color=[1, 0.5, 0])
# Add lookahead path
self.trajectory_fig.add_graph(
"lookahead_path",
window_size=self.INTERP_MAX_POINTS_PLOT,
x0=[start_x] * self.INTERP_MAX_POINTS_PLOT,
y0=[start_y] * self.INTERP_MAX_POINTS_PLOT,
color=[0, 0.7, 0.7],
linewidth=4)
# Add starting position marker
self.trajectory_fig.add_graph("start_pos",
window_size=1,
x0=[self.start_x],
y0=[self.start_y],
marker=11,
color=[1, 0.5, 0],
markertext="Start",
marker_text_offset=1)
# Add end position marker
self.trajectory_fig.add_graph("end_pos",
window_size=1,
x0=[self.waypoints_np[-1, 0]],
y0=[self.waypoints_np[-1, 1]],
marker="D",
color='r',
markertext="End",
marker_text_offset=1)
# Add car marker
self.trajectory_fig.add_graph("car",
window_size=1,
marker="s",
color='b',
markertext="Car",
marker_text_offset=1)
###
# Add 1D speed profile updater
###
self.forward_speed_fig =\
self.lp_1d.plot_new_dynamic_figure(title="Forward Speed (m/s)")
self.forward_speed_fig.add_graph("forward_speed",
label="forward_speed",
window_size=self.TOTAL_EPISODE_FRAMES)
self.forward_speed_fig.add_graph("reference_signal",
label="reference_Signal",
window_size=self.TOTAL_EPISODE_FRAMES)
# Add throttle signals graph
self.throttle_fig = self.lp_1d.plot_new_dynamic_figure(
title="Throttle")
self.throttle_fig.add_graph("throttle",
label="throttle",
window_size=self.TOTAL_EPISODE_FRAMES)
# Add brake signals graph
self.brake_fig = self.lp_1d.plot_new_dynamic_figure(title="Brake")
self.brake_fig.add_graph("brake",
label="brake",
window_size=self.TOTAL_EPISODE_FRAMES)
# Add steering signals graph
self.steer_fig = self.lp_1d.plot_new_dynamic_figure(title="Steer")
self.steer_fig.add_graph("steer",
label="steer",
window_size=self.TOTAL_EPISODE_FRAMES)
# live plotter is disabled, hide windows
if not enable_live_plot:
self.lp_traj._root.withdraw()
self.lp_1d._root.withdraw()
