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