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)