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 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)