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)