def nearest(vertex_rand, current_vertex, vertex_nearest,
k): # recursively exhaustively search tree for nearest node
current_vertex.k_nearest = k
if dist(vertex_rand, current_vertex) < dist(
vertex_rand, vertex_nearest
) and current_vertex.at_goal_set is False: # if distance to next vertex in tree is smaller
vertex_nearest = current_vertex # update it as the nearest - is this accidentally changing root node value?
for i in range(len(current_vertex.children)): # for all children vertices
if current_vertex.children[
i].k_nearest != k: # if next vertex not checked yet
vertex_nearest = nearest(vertex_rand, current_vertex.children[i],
vertex_nearest,
k) # call nearest function again
return vertex_nearest
def path_generation(vertex):
#print 'running path gen 1', 'vertex parents:', vertex.parents
paths_parents = [] # list of paths, path = list of vertices
costs_parents = [] # list of costs associated with some paths
paths = []
costs = []
for i in range(len(vertex.parents)): # for all parents
parent = vertex.parents[i]
pths, csts = path_generation2(
parent) # get all paths and costs for that parent
paths_parents.append(pths) # append paths to path list
costs_parents.append(csts) # append costs to cost list
# separating a list of lists of paths into one list of paths:
for i in range(len(paths_parents)
): # for number of lists of paths (number of parents)
distance = dist(
vertex,
vertex.parents[i]) # calculate distance for each new parent
for j in range(len(paths_parents[i])
): # for number of paths in one of those lists
paths.append(
paths_parents[i][j]) # append that path to new total list
costs.append(
costs_parents[i][j] + distance
) # append that parent cost + distance to new total cost list
for i in range(len(paths)): # for every path in paths
paths[i].append(vertex) # add the current vertex to that path
if len(paths) == 0: # if this vertex is the root node
paths = [[vertex]] # only "path" is itself, and has zero cost
costs = [0]
return paths, costs
def get_time(
path, robo_num, times
): # simple version for holonomic, velocity is controlled / constant-magnitude
if len(times) == 0:
times.append(0)
for i in range(len(path) - 1):
add_time = dist(path[i],
path[i + 1]) / Settings.robo_velocities[robo_num]
times.append(times[i] + add_time)
# else times is already calculated
return
def near_vertices(vertex_new, current_vertex, k,
vertices_near): # return all vertices within near_radius
current_vertex.k_near = k # for avoiding checking one vertex multiple times
if k == 1:
near_radius = Dimensions.eta
else:
near_radius = min(Dimensions.gamma * sqrt(log(k) / k),
Dimensions.eta) # function of numPoints
if dist(vertex_new, current_vertex) <= near_radius and (
current_vertex.at_goal_set is False): # if within radius
vertices_near.append(current_vertex) # add to list of near vertices
for i in range(len(
current_vertex.children)): # for all children of this vertex
if current_vertex.children[
i].k_near != k: # if that child not checked yet
vertices_near = near_vertices(vertex_new,
current_vertex.children[i], k,
vertices_near) # call function again
return vertices_near