def highlight_edges( self, graph_base: graph_ltpl.data_objects.GraphBase.GraphBase, passed_nodes: list, color_str='m') -> None: """ Highlight a sequence of edges in the graph plot with another color (specified by node list). :param graph_base: reference to the class object holding all graph relevant information :param passed_nodes: a list of nodes (layer and node id tuple), which define the path to be highlighted :param color_str: character specifying the color of the generated line """ # generate coordinates for highlight edges spline_coord_x = [] spline_coord_y = [] previous_node = None for current_node in passed_nodes: if previous_node is not None: spline_param = graph_base.get_edge( start_layer=previous_node[0], start_node=previous_node[1], end_layer=current_node[0], end_node=current_node[1])[1] # Add the coordinates to a single array with "None" values separating the splines spline_coord_x.extend(spline_param[:, 0]) # x spline_coord_x.append(None) spline_coord_y.extend(spline_param[:, 1]) # y spline_coord_y.append(None) previous_node = current_node # plot the spline self.highlight_line(line_coords_x=spline_coord_x, line_coords_y=spline_coord_y, color_str=color_str)
def gen_node_skeleton(graph_base: graph_ltpl.data_objects.GraphBase.GraphBase, length_raceline: list, var_heading=True, closed: bool = True) -> list: """ Generate a node skeleton for a graph structure based on center line, track width and race line. :param graph_base: reference to the GraphBase object instance holding all graph relevant information: * track_width: track width at given center line coordinates in meters * normvec_normalized: x and y components of normized normal vector at given center line pos * alpha: alpha parameter (percentage of track width) for min global curvature * veh_width: width of the vehicle (i.e. distance of CG to wall) :param length_raceline: list holding element lengths of the raceline :param var_heading: flag, defining whether the heading of each node is linearly sampled between the bounds orientation and the race line heading (else: all points with same heading as raceline) :param closed: if true, a closed circuit is assumed :returns: * **state_pos** - stacked list of x and y coordinates of all nodes generated :Authors: * Tim Stahl <*****@*****.**> :Created on: 26.09.2018 """ # closed index - include last length element for closed tracks if closed: closed_idx = None else: closed_idx = -1 # ------------------------------------------------------------------------------------------------------------------ # PREPARE / EXTRACT DATA ------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # extract data stored in the graph_base object normvec_normalized = graph_base.normvec_normalized alpha = graph_base.alpha track_width_right = graph_base.track_width_right track_width_left = graph_base.track_width_left # calculate raceline points raceline_points = graph_base.refline + normvec_normalized * alpha[:, np. newaxis] # ------------------------------------------------------------------------------------------------------------------ # EXTRACT ORIENTATION ---------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # heading of race line points in global coord (subtract pi/2, since zero is north) psi = tph.calc_head_curv_num.calc_head_curv_num( path=raceline_points, el_lengths=np.array(length_raceline[:closed_idx]), is_closed=closed)[0] # if variable heading, generate heading of outer bounds psi_bound_l = None psi_bound_r = None if var_heading: # get bounds # bound_l = graph_base.refline - normvec_normalized * [1, 1] # bound_r = graph_base.refline + normvec_normalized * [1, 1] bound_r = graph_base.refline + normvec_normalized * np.expand_dims( track_width_right, 1) bound_l = graph_base.refline - normvec_normalized * np.expand_dims( track_width_left, 1) # get distance between bound elements d = np.diff(np.vstack((bound_l, bound_l[0])), axis=0) len_bl = np.hypot(d[:, 0], d[:, 1]) d = np.diff(np.vstack((bound_r, bound_r[0])), axis=0) len_br = np.hypot(d[:, 0], d[:, 1]) psi_bound_l = tph.calc_head_curv_num.calc_head_curv_num( path=bound_l, el_lengths=np.array(len_bl[:closed_idx]), is_closed=closed)[0] psi_bound_r = tph.calc_head_curv_num.calc_head_curv_num( path=bound_r, el_lengths=np.array(len_br[:closed_idx]), is_closed=closed)[0] # ------------------------------------------------------------------------------------------------------------------ # GENERATE SAMPLES ---------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ state_pos = [] raceline_index_array = [] # Check if safety margin is valid (paired with raceline) margin_left = min(track_width_left - graph_base.veh_width / 2 + alpha) margin_right = min(track_width_right - graph_base.veh_width / 2 - alpha) if (margin_left < 0.0) or (margin_right < 0.0): # calculate maximum vehicle width max_veh_width = graph_base.veh_width + min(margin_left, margin_right) * 2 print("#############################\n") print( "Specified vehicle width is too wide in order to follow the provided raceline! The maximum possible " "vehicle width for this raceline is %.3fm!" % max_veh_width) print( "-> In order to specify the vehicle width of the local planner, change 'veh_width' in " "'/params/graph_config_offline.ini'") print("\n#############################") raise ValueError( "Provided raceline holds points outside the safety margin! " "Reduce the vehicle width or adapt the race line (## SEE DETAILS ABOVE ##)." ) # for each normvect for i in range(len(normvec_normalized)): # determine raceline index in array to be generated raceline_index = int( np.floor( (track_width_left[i] - graph_base.veh_width / 2 + alpha[i]) / graph_base.lat_resolution)) raceline_index_array.append(raceline_index) # determine start point on normal relative to ideal raceline point s = alpha[i] - raceline_index * graph_base.lat_resolution # spread points on normal with defined lateral offset and calculated starting point temp_alphas = np.arange( s, track_width_right[i] - graph_base.veh_width / 2, graph_base.lat_resolution) # transfer alphas to cartesian space temp_pos = np.repeat( graph_base.refline[i][None, :], len(temp_alphas), axis=0 ) + np.repeat(normvec_normalized[i][None, :], len(temp_alphas), axis=0) * temp_alphas[:, np.newaxis] # calculate psi for each coordinate if var_heading: # since psi is defined in [-pi <-> pi], we need to check, which heading path is the shortest if abs(psi_bound_l[i] - psi[i]) < np.pi: psi1 = np.linspace(psi_bound_l[i], psi[i], num=raceline_index + 1)[:-1] else: temp_bl = psi_bound_l[i] + 2 * np.pi * (psi_bound_l[i] < 0) temp_psi = psi[i] + 2 * np.pi * (psi[i] < 0) psi1 = tph.normalize_psi.\ normalize_psi(np.linspace(temp_bl, temp_psi, num=raceline_index + 1)[:-1]) if abs(psi_bound_r[i] - psi[i]) < np.pi: psi2 = np.linspace(psi[i], psi_bound_r[i], num=len(temp_alphas) - raceline_index) else: temp_br = psi_bound_r[i] + 2 * np.pi * (psi_bound_r[i] < 0) temp_psi = psi[i] + 2 * np.pi * (psi[i] < 0) psi2 = tph.normalize_psi. \ normalize_psi(np.linspace(temp_psi, temp_br, num=len(temp_alphas) - raceline_index)) temp_psi = np.append(psi1, psi2) else: temp_psi = np.repeat(psi[i], len(temp_alphas), axis=0) # store info in local variable list.append(state_pos, [temp_pos, temp_psi]) # store node information in data object graph_base.add_layer(layer=i, pos_multi=temp_pos, psi=temp_psi, raceline_index=raceline_index) # store race line index array in graph base graph_base.raceline_index = raceline_index_array return state_pos
def main_online_path_gen( graph_base: graph_ltpl.data_objects.GraphBase.GraphBase, start_node: tuple, obj_veh: list, obj_zone: list, action_sets=True, last_action_id: str = None, max_solutions=1, const_path_seg: np.ndarray = None, pos_est: np.ndarray = None, last_solution_nodes: list = None, w_last_edges: list = ()) -> tuple: """ The main function to be called iteratively in an online environment to calculate path solutions. The following steps are executed in an iterative manner: * Represent all objects (e.g. vehicles) within the planning range in the graph * Define action sets to be generated by the planner (e.g. keep straight, overtake on the left, ...) and modify graph for each action primitive correspondingly * Execute path search for each action set * Calculate curvature continuous spline for each solution :param graph_base: reference to the GraphBase object instance holding relevant parameters :param start_node: tuple of layer and node id (a tracking of the position and fixed trajectories should be handled in the main script :param obj_veh: list of objects holding info like "pos", "ids", "vel", "radius" :param obj_zone: list of objects holding information about the blocked zones :param action_sets: flag, whether action sets should be used or not (else just a single solution) :param last_action_id: string holding the previously executed action id :param max_solutions: maximum number of path solutions to be returned (NOTE: 1 is the fastest!) :param const_path_seg: path segment (x,y,psi,kappa,s) in the path planning process, which remains constant :param pos_est: estimated position of the ego vehicle :param last_solution_nodes: nodes of the last valid solution -> used to reduce cost along last valid path :param w_last_edges: online cost reduction per previously planned path segment :returns: * **action_set_nodes** - dict for each action containing a list with one or multiple arrays holding tuples of node id sequences for determined local solutions * **action_set_node_idx** - dict for each action containing a list with one or multiple arrays holding the indexes of the node candidates in the coords arrays * **action_set_coeff** - dict for each action containing a list with one or multiple arrays holding the spline coefficients Nx8 (N spline segments) for the determined local solutions * **action_set_path_param** - dict for each action containing a numpy array with one or multiple arrays holding the x-, y-coordinates, psi, kappa and el-length for the determined local solutions * **closest_obj_index** - index of the object in the provided array, which is closest upfront ego-vehicle ("None", if no object in planning range) :Authors: * Tim Stahl <*****@*****.**> :Created on: 07.11.2018 """ # ------------------------------------------------------------------------------------------------------------------ # - Handle objects within the planning horizon --------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # Generate local node template for the current action_set (block edges occupied by objects) end_layer, closest_obj_index, closest_obj_node = graph_ltpl.online_graph.src.gen_local_node_template.\ gen_local_node_template(graph_base=graph_base, start_node=start_node, obj_veh=obj_veh, obj_zone=obj_zone, last_solution_nodes=last_solution_nodes, w_last_edges=w_last_edges) # check for objects in constant path segment (if provided) obj_in_const_path = False object_besides_const_path = False if const_path_seg is not None and np.size(const_path_seg, axis=0) >= 2: # get start pos (use pos_est if provided, else use start of constant path [may contain passed path]) if pos_est is not None: pos_start = pos_est else: pos_start = const_path_seg[0, 0:2] s_start, _ = graph_ltpl.helper_funcs.src.get_s_coord.get_s_coord( ref_line=graph_base.raceline, pos=pos_start, s_array=graph_base.s_raceline, closed=True) s_end, _ = graph_ltpl.helper_funcs.src.get_s_coord.get_s_coord( ref_line=graph_base.raceline, pos=const_path_seg[-1, 0:2], s_array=graph_base.s_raceline, closed=True) smallest_obj_dist = np.Inf for obj_idx, vehicle in enumerate(obj_veh): s_obj, _ = graph_ltpl.helper_funcs.src.get_s_coord.get_s_coord( ref_line=graph_base.raceline, pos=vehicle.get_pos(), s_array=graph_base.s_raceline, closed=True) # if object within constant path segment if s_start <= s_obj <= s_end or ( s_start > s_end and (s_obj > s_start or s_obj < s_end)): object_besides_const_path = True # calculate object distance if s_obj < s_start: obj_dist = s_obj + graph_base.s_raceline[-1] - s_start else: obj_dist = s_obj - s_start if closest_obj_index is None or obj_dist < smallest_obj_dist: closest_obj_index = obj_idx smallest_obj_dist = obj_dist # check if constant path intersects with object obstacle_ref = np.power( vehicle.get_radius() + graph_base.veh_width / 2, 2) distances2 = np.power( const_path_seg[:, 0] - vehicle.get_pos()[0], 2) + np.power( const_path_seg[:, 1] - vehicle.get_pos()[1], 2) if any(distances2 <= obstacle_ref): obj_in_const_path = True # -- DEFINE ACTION SETS TO BE GENERATED ---------------------------------------------------------------------------- # NOTE: Ensure to first list the straight/follow options (if the planning horizon is reduced there, it will be also # reduced for the other primitives # if an object is located in the constant path section (either overtaking or follow mode) if action_sets and (obj_in_const_path or object_besides_const_path): action_set_filters = ["planning_range"] action_set_goal_layer = end_layer action_set_names = ["follow"] # if currently in an overtaking maneuver-> offer straight with overtaking action ID if not obj_in_const_path and (last_action_id == "left" or last_action_id == "right"): action_set_filters.append("default") action_set_names.append(last_action_id) # if object only besides path, but no overtaking active -> offer left and right elif not obj_in_const_path: # array of all filters action_set_filters.extend(["default", "default"]) action_set_names.extend(["left", "right"]) # if action sets enabled and a vehicle in the planning horizon elif action_sets and closest_obj_index is not None and closest_obj_node is not None: # -- generate node filters for each action set (overtake left, overtake right, follow) -- # overtake left block_set = list( range(closest_obj_node[1], graph_base.nodes_in_layer[closest_obj_node[0]])) graph_base.remove_nodes_filter(layer_ids=[closest_obj_node[0]] * len(block_set), node_ids=block_set, applied_filter="overtake_left_filter", base="default") # overtake right block_set = list(range(0, closest_obj_node[1])) graph_base.remove_nodes_filter(layer_ids=[closest_obj_node[0]] * len(block_set), node_ids=block_set, applied_filter="overtake_right_filter", base="default") # straight # block_set = list(range(0, graph_base.nodes_in_layer[closest_obstacle_node[0]])) # block_filter = copy.deepcopy(local_node_filter) # graph_base.update_node_filter(layer_ids=[closest_obstacle_node[0]] * len(block_set), # node_ids=block_set, # applied_filter=block_filter) # array of all filters action_set_filters = [ "planning_range", "overtake_left_filter", "overtake_right_filter" ] action_set_goal_layer = end_layer action_set_names = ["follow", "left", "right"] else: action_set_filters = ["default"] action_set_goal_layer = end_layer action_set_names = ["straight"] # ------------------------------------------------------------------------------------------------------------------ # - Path search and assembly of spline segments -------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # Init output variables action_set_nodes = dict() action_set_node_idx = dict() action_set_coeff = dict() action_set_path_param = dict( ) # x, y, psi, kappa, el length (tailing zero) action_set_red_len = dict( ) # dict holding a True value for primitives with a reduced planning horizon mod_action_set_goal_layer = action_set_goal_layer for action_set_filter, action_set_name in \ zip(action_set_filters, action_set_names): # activate action_set_filter graph_base.activate_filter(applied_filter=action_set_filter) # Check if object is not on planning start layer (except for follow) # if closest_obj_node is not None and start_node[0] == closest_obj_node[0] and action_set_name != "follow": # lat_displ = abs(start_node[1] - closest_obj_node[1]) * graph_base.lat_resolution - \ # graph_base.veh_width / 2 - obj_veh[closest_obj_index].get_radius() # if lat_displ < 0: # continue loc_path_nodes_list = None while True: # check if start layer is equal to goal layer if mod_action_set_goal_layer == start_node[0]: break # Trigger graph search (if goal node exists) _, loc_path_nodes_list = graph_base.search_graph_layer( start_layer=start_node[0], start_node=start_node[1], end_layer=mod_action_set_goal_layer, max_solutions=max_solutions) # exit if solution is found or when planning a special maneuver (e.g. overtaking -> use same goal as s/f) if loc_path_nodes_list is not None or not ( action_set_name == "follow" or action_set_name == "straight"): break else: mod_action_set_goal_layer -= 1 if mod_action_set_goal_layer < 0: mod_action_set_goal_layer = graph_base.num_layers - 1 # If planning horizon was reduced (either by vehicles or end of open track reached) reduced_horizon = (mod_action_set_goal_layer != action_set_goal_layer or (not graph_base.closed and action_set_goal_layer == graph_base.num_layers - 1)) if reduced_horizon: # Check if vehicle is still in range obj_in_mod_range = (closest_obj_node is not None and ( (start_node[0] <= closest_obj_node[0] <= mod_action_set_goal_layer) or (start_node[0] > mod_action_set_goal_layer and (closest_obj_node[0] >= start_node[0] or closest_obj_node[0] <= mod_action_set_goal_layer)))) if not obj_in_const_path and closest_obj_node is not None and not obj_in_mod_range: # rename follow mode to straight and remove overtaking options if action_set_name == "follow" or action_set_name == "straight": action_set_name = "straight" logging.getLogger("local_trajectory_logger"). \ info("No feasible solution for '" + action_set_name + "'! Reduced planning horizon!") else: # remove all other options loc_path_nodes_list = None else: logging.getLogger("local_trajectory_logger"). \ info("No feasible solution for '" + action_set_name + "'! Reduced planning horizon!") # If no solution is found (blocked graph), end iteration and leave action set list empty if loc_path_nodes_list is None: logging.getLogger("local_trajectory_logger").\ debug("Action set '" + action_set_name + "' is empty! No path solution was found.") else: # Store node sequence in action set action_set_nodes[action_set_name] = loc_path_nodes_list # init dict entry for action set (in order to allow multiple trajectories per set) action_set_node_idx[action_set_name] = [] action_set_coeff[action_set_name] = [] action_set_path_param[action_set_name] = [] action_set_red_len[action_set_name] = [] # For every path in list for loc_path_nodes in loc_path_nodes_list: loc_path_node_idx = [0] # spline_coeff_mat = np.zeros((len(loc_path_nodes)-1, 8)) # Initialize list for all path components (since length is not beforehand, lists are faster than numpy) spline_param_fuse = np.empty((0, 5)) dists = np.empty(0) previous_node = None # Assemble data from path segments in one array per path for count, current_node in enumerate(loc_path_nodes): if previous_node is not None: spline_coeff, spline_param, offline_cost, spline_length = \ graph_base.get_edge(start_layer=previous_node[0], start_node=previous_node[1], end_layer=current_node[0], end_node=current_node[1]) # Sort spline coefficients in large numpy matrix Nx8 # spline_coeff_mat[count-1, :] = np.array(spline_coeff).flatten() # Remove last value of coordinate, psi and curvature, since they appear twice at transitions # between two spline segments -> indicator that is one except in the last iteration if count != len(loc_path_nodes) - 1: rmv_el = -1 else: rmv_el = None # Add the coordinates to a single array spline_param_fuse = np.append(spline_param_fuse, spline_param[:rmv_el, :], axis=0) dists = np.append(dists, spline_length) # Provide indexes of loc_path_nodes in loc_path_coords loc_path_node_idx.append( np.size(spline_param_fuse, axis=0) - 1 * (rmv_el is None)) previous_node = current_node # Calculate curvature continous spline if const_path_seg is not None: psi_s = const_path_seg[-1, 2] else: psi_s = spline_param_fuse[0, 2] spline_coeff_mat = np.column_stack( tph.calc_splines.calc_splines( path=spline_param_fuse[loc_path_node_idx, 0:2], psi_s=psi_s, psi_e=spline_param_fuse[-1, 2], el_lengths=dists)[0:2]) # Calculate new curvature, coordinates and headings based on splines spline_param_fuse[:, 0:2], tmp_spline_inds, tmp_t_values, _ = \ tph.interp_splines.interp_splines(coeffs_x=spline_coeff_mat[:, :4], coeffs_y=spline_coeff_mat[:, 4:], incl_last_point=True, stepnum_fixed=(np.diff(loc_path_node_idx) + 1).tolist()) spline_param_fuse[:, 2], spline_param_fuse[:, 3] = tph.calc_head_curv_an.\ calc_head_curv_an(coeffs_x=spline_coeff_mat[:, :4], coeffs_y=spline_coeff_mat[:, 4:], ind_spls=tmp_spline_inds, t_spls=tmp_t_values) # add trajectory candidate to action set action_set_coeff[action_set_name].append(spline_coeff_mat) action_set_node_idx[action_set_name].append(loc_path_node_idx) action_set_path_param[action_set_name].append( spline_param_fuse) action_set_red_len[action_set_name].append(reduced_horizon) # deactivate present filters graph_base.deactivate_filter() return (action_set_nodes, action_set_node_idx, action_set_coeff, action_set_path_param, action_set_red_len, closest_obj_index)
def gen_offline_cost(graph_base: graph_ltpl.data_objects.GraphBase.GraphBase, cost_config_path: str): """ Generate offline cost for given edges (in a GraphBase object instance). :param graph_base: reference to the GraphBase object instance holding all graph relevant information :param cost_config_path: path pointing to the configuration file, parameterizing the cost generation :Authors: * Tim Stahl <*****@*****.**> :Created on: 10.10.2018 """ # ------------------------------------------------------------------------------------------------------------------ # PREPARE DATA ----------------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # Read cost configuration from provided datafile cost_config = configparser.ConfigParser() if not cost_config.read(cost_config_path): raise ValueError( 'Specified graph config file does not exist or is empty!') if 'COST' not in cost_config: raise ValueError( 'Specified graph config file does not hold the expected data!') # ------------------------------------------------------------------------------------------------------------------ # GENERATE OFFLINE COST -------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # sample a path for each edge in the graph base tic = time.time() edges = graph_base.get_edges() i = 0 for edge in edges: tph.progressbar.progressbar(i, len(edges) - 1, prefix="Generating cost ") # retrieve stored data for given edge spline_coeff, spline_param, offline_cost, spline_len = graph_base.get_edge( edge[0], edge[1], edge[2], edge[3]) # Calculate offline cost / init offline_cost = 0.0 # average curvature (div. by #elements and multiplied by length (to be independent of coord.-resolution) offline_cost += cost_config.getfloat('COST', 'w_curv_avg') * np.power( sum(abs(spline_param[:, 3])) / float(len(spline_param[:, 3])), 2) * spline_len # peak curvature offline_cost += cost_config.getfloat('COST', 'w_curv_peak') * np.power( abs(max(spline_param[:, 3]) - min(spline_param[:, 3])), 2) * spline_len # Path length offline_cost += cost_config.getfloat('COST', 'w_length') * spline_len # Raceline cost (normed by distance) raceline_dist = abs(graph_base.raceline_index[edge[2]] - edge[3]) * graph_base.lat_resolution offline_cost += min( cost_config.getfloat('COST', 'w_raceline') * spline_len * raceline_dist, cost_config.getfloat('COST', 'w_raceline_sat') * spline_len) # store determined value along with graph edge graph_base.update_edge(start_layer=edge[0], start_node=edge[1], end_layer=edge[2], end_node=edge[3], offline_cost=offline_cost) i += 1 toc = time.time() print("Cost generation took " + '%.3f' % (toc - tic) + "s")
def plot_graph_base(self, graph_base: graph_ltpl.data_objects.GraphBase.GraphBase, cost_dep_color: bool = True, plot_edges: bool = True) -> None: """ Plot the major components stored in the graph_base object :param graph_base: reference to the GraphBase object instance holding all graph relevant information :param cost_dep_color: boolean flag, specifying, whether to plot edges with a variable color (depending on cost) or not (Note: cost dependent plotting is drastically slower) :param plot_edges: boolean flag, specifying, whether the edges should be included in the plot """ # refline plt_refline, = plt.plot(graph_base.refline[:, 0], graph_base.refline[:, 1], "k--", linewidth=1.4, label="Refline") # track bounds # bound1 = graph_base.refline + graph_base.normvec_normalized * graph_base.track_width[:, np.newaxis] / 2 # bound2 = graph_base.refline - graph_base.normvec_normalized * graph_base.track_width[:, np.newaxis] / 2 bound1 = graph_base.refline + graph_base.normvec_normalized * np.expand_dims(graph_base.track_width_right, 1) bound2 = graph_base.refline - graph_base.normvec_normalized * np.expand_dims(graph_base.track_width_left, 1) x = list(bound1[:, 0]) y = list(bound1[:, 1]) x.append(None) y.append(None) x.extend(list(bound2[:, 0])) y.extend(list(bound2[:, 1])) plt_bounds, = self.__main_ax.plot(x, y, "k-", linewidth=1.4, label="Bounds") # norm vecs x = [] y = [] for i in range(bound1.shape[0]): temp = np.vstack((bound1[i], bound2[i])) x.extend(temp[:, 0]) y.extend(temp[:, 1]) x.append(None) y.append(None) plt_normals, = plt.plot(x, y, color=TUM_colors['TUM_blue_dark'], linestyle="-", linewidth=0.7, label="Normals") # raceline points rlpt = graph_base.refline + graph_base.normvec_normalized * graph_base.alpha[:, np.newaxis] plt_raceline, = self.__main_ax.plot(rlpt[:, 0], rlpt[:, 1], color=TUM_colors['TUM_blue'], linestyle="-", linewidth=1.4, label="Raceline") # plot state poses nodes = graph_base.get_nodes() i = 0 x = [] y = [] for node in nodes: tph.progressbar.progressbar(i, len(nodes) - 1, prefix="Plotting nodes ") # Try to get node info (if filtered, i.e. online graph, this will fail) try: node_pos = graph_base.get_node_info(node[0], node[1])[0] x.append(node_pos[0]) y.append(node_pos[1]) except ValueError: pass i += 1 plt_nodes, = self.__main_ax.plot(x, y, "x", color=TUM_colors['TUM_blue'], markersize=3, label="Nodes") if plot_edges: # plot edges edges = graph_base.get_edges() i = 0 if not cost_dep_color: x = [] y = [] color_spline = TUM_colors['TUM_blue_light'] # (0, 1, 0) min_cost = None max_cost = None else: # get maximum and minimum cost in all provided edges min_cost = 9999.9 max_cost = -9999.9 for edge in edges: try: edge_cost = graph_base.get_edge(edge[0], edge[1], edge[2], edge[3])[2] min_cost = min(min_cost, edge_cost) max_cost = max(max_cost, edge_cost) except ValueError: pass color_spline = None plt_edges = None for edge in edges: tph.progressbar.progressbar(i, len(edges) - 1, prefix="Plotting edges ") # Try to get edge (if filtered, i.e. online graph, this will fail) try: spline = graph_base.get_edge(edge[0], edge[1], edge[2], edge[3]) spline_coords = spline[1][:, 0:2] spline_cost = spline[2] # cost dependent color if cost_dep_color: color_spline = (round(min(1, (spline_cost - min_cost) / (max_cost - min_cost)), 2), round(max(0, 1 - (spline_cost - min_cost) / (max_cost - min_cost)), 2), 0) self.__main_ax.plot(spline_coords[:, 0], spline_coords[:, 1], "-", color=color_spline, linewidth=0.7) else: # Faster plot method (but for now, no individual color shading) x.extend(spline_coords[:, 0]) x.append(None) y.extend(spline_coords[:, 1]) y.append(None) except ValueError: pass i += 1 # plt.pause(0.000001) # Live plotting -> caution: slows down drastically! plt_edges = None if not cost_dep_color: plt_edges, = self.__main_ax.plot(x, y, "-", color=color_spline, linewidth=0.7, label="Edges") # properties leg = self.__main_ax.legend(loc='upper left') if plot_edges and not cost_dep_color: elements = [plt_refline, plt_bounds, plt_normals, plt_raceline, plt_nodes, plt_edges] else: elements = [plt_refline, plt_bounds, plt_normals, plt_raceline, plt_nodes] elementd = dict() # couple legend entry to real line for leg_element, orig_element in zip(leg.get_lines(), elements): leg_element.set_pickradius(10) # 5 pts tolerance elementd[leg_element] = orig_element # line picking self.__fig.canvas.mpl_connect('pick_event', lambda event: self.__eh.onpick(event=event, elementd=elementd)) # detail information node_plot_marker, = self.__main_ax.plot([], [], 'o', color=TUM_colors['TUM_orange']) edge_plot_marker, = self.__main_ax.plot([], [], '-', color=TUM_colors['TUM_orange']) annotation = self.__main_ax.annotate('', xy=[0, 0], xytext=(0, 0), arrowprops={'arrowstyle': "->"}) self.__eh.set_graph_markers(node_plot_marker=node_plot_marker, edge_plot_marker=edge_plot_marker, annotation=annotation) self.__fig.canvas.mpl_connect('motion_notify_event', lambda event: self.__eh.onhover(event=event, graph_base=graph_base)) self.__text_display = self.__main_ax.text(0.02, 0.95, "", transform=plt.gcf().transFigure) self.__text_display2 = self.__main_ax.text(0.8, 0.9, "", transform=plt.gcf().transFigure) if type(self.__time_ax) is not str: self.__time_annotation = self.__time_ax.annotate("", xy=(0, 0), xytext=(0.05, 0.90), textcoords='figure fraction', bbox=dict(boxstyle="round", fc="w"), arrowprops=dict(arrowstyle="->"))
def gen_edges(state_pos: np.ndarray, graph_base: graph_ltpl.data_objects.GraphBase.GraphBase, stepsize_approx: float, min_vel_race: float = 0.0, closed: bool = True) -> None: """ Generate edges for a given node skeleton. :param state_pos: stacked list of x and y coordinates of all nodes to be considered for edge generation :param graph_base: reference to the class object holding all graph relevant information :param stepsize_approx: number of samples to be generated for each spline (every n meter one sample) :param min_vel_race: min. race speed compared to global race line (in percent), all splines not allowing this velocity will be removed --> set this value to 0.0 in order to allow all splines :param closed: if true, the track is assumed to be a closed circuit :Authors: * Tim Stahl <*****@*****.**> :Created on: 28.09.2018 """ # ------------------------------------------------------------------------------------------------------------------ # PREPARE DATA ----------------------------------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ if graph_base.lat_offset <= 0: raise ValueError( 'Requested to small lateral offset! A lateral offset larger than zero must be allowed!' ) # ------------------------------------------------------------------------------------------------------------------ # DEFINE EDGES AND SAMPLE SPLINE COEFFICIENTS ---------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ # calculate splines for race-line raceline_cl = np.vstack((graph_base.raceline, graph_base.raceline[0])) x_coeff_r, y_coeff_r, _, _ = tph.calc_splines.calc_splines( path=raceline_cl) tic = time.time() # loop over start_layers for i in range(len(state_pos)): tph.progressbar.progressbar(i, len(state_pos) - 1, prefix="Calculate splines") start_layer = i end_layer = i + 1 # if requested end-layer exceeds number of available layers if end_layer >= len(state_pos): if closed: # if closed, connect to first layers end_layer = end_layer - len(state_pos) else: break # loop over nodes in start_layer for start_n in range(len(state_pos[start_layer][0])): # get end node reference (node with same offset to race line) end_n_ref = graph_base.raceline_index[ end_layer] + start_n - graph_base.raceline_index[start_layer] # determine allowed lateral offset # -> get distance between start node and (if possible) central (same index) goal node d_start = state_pos[start_layer][0][start_n, :] d_end = state_pos[end_layer][0][ max(0, min(len(state_pos[end_layer][0]) - 1, end_n_ref)), :] dist = np.sqrt( np.power(d_end[0] - d_start[0], 2) + np.power(d_end[1] - d_start[1], 2)) # -> get number of lateral steps based on distance, lateral resolution and allowed lateral offset p. m. lat_steps = int( round(dist * graph_base.lat_offset / graph_base.lat_resolution)) # loop over nodes in end_layer (clipped to the specified lateral offset) for end_n in range( max(0, end_n_ref - lat_steps), min(len(state_pos[end_layer][0]), end_n_ref + lat_steps + 1)): if (graph_base.raceline_index[end_layer] == end_n) and \ (graph_base.raceline_index[start_layer] == start_n): # if race-line element -> use race-line coeffs x_coeff = x_coeff_r[start_layer, :] y_coeff = y_coeff_r[start_layer, :] else: x_coeff, y_coeff, _, _ = tph.calc_splines.\ calc_splines(path=np.vstack((state_pos[start_layer][0][start_n, :], state_pos[end_layer][0][end_n, :])), psi_s=state_pos[start_layer][1][start_n], psi_e=state_pos[end_layer][1][end_n]) # add calculated edge to graph graph_base.add_edge(start_layer=start_layer, start_node=start_n, end_layer=end_layer, end_node=end_n, spline_coeff=[x_coeff, y_coeff]) toc = time.time() print("Spline generation and edge definition took " + '%.3f' % (toc - tic) + "s") # ------------------------------------------------------------------------------------------------------------------ # SAMPLE PATH EDGES (X, Y COORDINATES) ----------------------------------------------------------------------------- # ------------------------------------------------------------------------------------------------------------------ tic = time.time() rmv_cnt = 0 edge_cnt = 0 for i in range(graph_base.num_layers): tph.progressbar.progressbar(i, len(state_pos) - 1, prefix="Sampling splines ") start_layer = i for s in range(graph_base.nodes_in_layer[start_layer]): pos, psi, raceline, children, _ = graph_base.get_node_info( layer=start_layer, node_number=s, return_child=True) # loop over child-nodes for node in children: edge_cnt += 1 end_layer = node[0] e = node[1] spline = graph_base.get_edge(start_layer=start_layer, start_node=s, end_layer=end_layer, end_node=e)[0] x_coeff = np.atleast_2d(spline[0]) y_coeff = np.atleast_2d(spline[1]) spline_sample, inds, t_values, _ = tph.interp_splines.interp_splines( coeffs_x=x_coeff, coeffs_y=y_coeff, stepsize_approx=stepsize_approx, incl_last_point=True) psi, kappa = tph.calc_head_curv_an.calc_head_curv_an( coeffs_x=x_coeff, coeffs_y=y_coeff, ind_spls=inds, t_spls=t_values) # Extract race speed from global race line vel_rl = graph_base.vel_raceline[i] * min_vel_race # calculate min. allowed corner radius, when assuming 10m/s² lateral acceleration min_turn = np.power(vel_rl, 2) / 10.0 # check if spline violates vehicle turn radius (race line is guaranteed to be among graph set) if (all(abs(kappa) <= 1 / graph_base.veh_turn) and all(abs(kappa) <= 1 / min_turn)) or \ (raceline and graph_base.get_node_info(layer=end_layer, node_number=e)[2]): graph_base.update_edge( start_layer=start_layer, start_node=s, end_layer=end_layer, end_node=e, spline_x_y_psi_kappa=np.column_stack( (spline_sample, psi, kappa))) else: graph_base.remove_edge(start_layer=start_layer, start_node=s, end_layer=end_layer, end_node=e) rmv_cnt += 1 toc = time.time() print("Spline sampling took " + '%.3f' % (toc - tic) + "s") print("Added %d splines to the graph!" % edge_cnt) if rmv_cnt > 0: print( "Removed %d splines due to violation of the specified vehicle's turn radius or velocity aims!" % rmv_cnt)
def prune_graph(graph_base: graph_ltpl.data_objects.GraphBase.GraphBase, closed: bool = True) -> None: """ Prune graph - remove nodes and edges that are not reachable within the cyclic graph. :param graph_base: reference to the GraphBase object instance holding all graph relevant information :param closed: if false, an un-closed track is assumed, i.e. last layer nodes will not be pruned :Authors: * Tim Stahl <*****@*****.**> :Created on: 28.09.2018 """ j = 0 rmv_cnt_tot = 0 nodes = graph_base.get_nodes() while True: rmv_cnt = 0 for i, node in enumerate(nodes): tph.progressbar.progressbar(min(j * len(nodes) + i, len(nodes) * 10 - 2), len(nodes) * 10 - 1, prefix="Pruning graph ") # if not closed, keep all nodes in start and end-layer if not closed and (node[0] == graph_base.num_layers - 1 or node[0] == 0): continue # get children and parents of node _, _, _, children, parents = graph_base.get_node_info(layer=node[0], node_number=node[1], return_child=True, return_parent=True) # remove edges (removing nodes may destroy indexing conventions) if not children or not parents: # if no children or no parents, remove all connecting edges if not children: for parent in parents: rmv_cnt += 1 graph_base.remove_edge(start_layer=parent[0], start_node=parent[1], end_layer=node[0], end_node=node[1]) else: for child in children: rmv_cnt += 1 graph_base.remove_edge(start_layer=node[0], start_node=node[1], end_layer=child[0], end_node=child[1]) if rmv_cnt == 0: break else: rmv_cnt_tot += rmv_cnt j += 1 tph.progressbar.progressbar(100, 100, prefix="Pruning graph ") if rmv_cnt_tot > 0: print("Removed %d edges, identified as dead ends!" % rmv_cnt_tot)