def update_node_with_analystic_expantion(current, ngoal, c, ox, oy, kdtree,
gyaw1):
"""
This function updates the current node with "analystic" data
:param current: Node, current node
:param ngoal: Node, goal node
:param c: Config, configuration space
:param ox: meters, list of x positions of each obstacle
:param oy: meters, list of y positions of each obstacle
:param kdtree: kdtree, KD tree made from ox and oy
:param gyaw1: radians, goal position trailer yaw
:return:
"""
apath = analystic_expantion(current, ngoal, c, ox, oy, kdtree)
# Formulate data for the final node
if apath: # if apath = [], skip the if statement since there is no path (ignore the unresolved attribute reference
# errors as they won't happen since apath is only a list if it is empty and then the if statement is false and
# the block doesn't execute)
fx = apath.x[1:-1]
fy = apath.y[1:-1]
fyaw = apath.yaw[1:-1]
steps = [MOTION_RESOLUTION * direct for direct in apath.directions]
yaw1 = trailerlib.calc_trailer_yaw_from_xyyaw(apath.x, apath.y,
apath.yaw,
current.yaw1[-1], steps)
# check if the trailer yaw is outside the trailer yaw threshold
if abs(rs_path.pi_2_pi(yaw1[-1] - gyaw1)) >= GOAL_TYAW_TH:
return False, [
] # current node is not the goal node based on trailer
if abs(fx[-1] - ngoal.x[-1]) >= XY_GRID_RESOLUTION or abs(
fy[-1] - ngoal.y[-1]) >= XY_GRID_RESOLUTION:
return False, [
] # current node is not the goal node based on xy location
fcost = current.cost + calc_rs_path_cost(apath,
yaw1) # cost to next node
fyaw1 = yaw1[1:-1] # array of trailer yaws
fpind = calc_index(current, c) # index of parent node
fd = [] # array of directions
for d in apath.directions[1:-1]:
if d >= 0:
fd.append(True)
else:
fd.append(False)
fsteer = 0
fpath = Node(current.xind, current.yind, current.yawind,
current.direction, fx, fy, fyaw, fyaw1, fd, fsteer, fcost,
fpind)
return True, fpath
return False, []
def verify_index(node, c, ox, oy, inityaw1, kdtree):
"""
This function verifies that the x and y index of the given node are valid and collision free
:param node: Node, node to check x and y index of
:param c: Config, Cspace
:param ox: meters, list of x positions of obstacles
:param oy: meters, list of y positions of obstacles
:param inityaw1: radians, first increment of yaw1 from the current node configuration
:param kdtree: scipy.spatial.KDTree, KD Tree of obstacles
:return: boolean, validity of the x and y indexes
"""
# Check if the x index is valid
if (node.xind - c.minx) >= c.xw: # x index is too high
return False
elif (node.xind - c.minx) <= 0: # x index is too low
return False
if (node.yind - c.miny) >= c.yw: # y index is too high
return False
elif (node.yind - c.miny) <= 0: # y index is too low
return False
# Check if the node collides with an obstacle
steps = MOTION_RESOLUTION * node.directions
yaw1 = trailerlib.calc_trailer_yaw_from_xyyaw(node.x, node.y, node.yaw,
inityaw1, steps)
ind = [i for i in np.arange(1, len(node.x), SKIP_COLLISION_CHECK)]
if not trailerlib.check_trailer_collision(ox,
oy,
node.x[ind],
node.y[ind],
node.yaw[ind],
yaw1[ind],
kdtree=kdtree):
return False
# If none of the above returned false, return true
return True
def analystic_expantion(n, ngoal, c, ox, oy, kdtree):
"""
This function determines the least costly path to the next node that doesn't collide with an obstacle
TODO verify function purpose
:param n: Node, current node
:param ngoal: Node, goal node
:param c: Config, Cspace
:param ox: meters, list of x positions of obstacles
:param oy: meters, list of y positions of obstacles
:param kdtree: kdtree, KD Tree
:return: an RS path leading to the next least costly node that doesn't collide with an obstacle
"""
# Extract the x,y, and yaw info from the current node
sx = n.x[-1]
sy = n.y[-1]
syaw = n.yaw[-1]
# Calculate the sharpest turn possible
max_curvature = math.tan(MAX_STEER) / WB
# Call rs_path to determine the paths available based on current and goal node TODO verify integration with rs_path
# TODO determine structure of paths from rs_path.calc_paths
paths = rs_path.calc_paths(sx,
sy,
syaw,
ngoal.x[-1],
ngoal.y[-1],
ngoal.yaw[-1],
max_curvature,
step_size=MOTION_RESOLUTION)
# If there is no available path, stop and return nothing
if not paths:
return []
pathqueue = {} # Dictionary for holding the path and yaw1 information
# Put the rs path and yaw1 combinations into pathqueue
for path in paths:
steps = MOTION_RESOLUTION * path.directions
yaw1 = trailerlib.calc_trailer_yaw_from_xyyaw(path.x, path.y, path.yaw,
n.yaw1[-1], steps)
pathqueue.update({path: calc_rs_path_cost(path, yaw1)})
# Go through each path, starting with the lowest cost, and check for collisions, return the first viable path
for i in range(len(pathqueue)):
path = min(pathqueue.iteritems(), key=operator.itemgetter(1))[
0] # extract path with lowest cost # TODO see error
pathqueue.pop(path) # remove the lowest cost path from the queue
# TODO description insert here
steps = MOTION_RESOLUTION * path.directions
yaw1 = trailerlib.calc_trailer_yaw_from_xyyaw(path.x, path.y, path.yaw,
n.yaw1[-1], steps)
ind = [i for i in np.arange(1, len(path.x), SKIP_COLLISION_CHECK)]
if trailerlib.check_trailer_collision(ox,
oy,
path.x[ind],
path.y[ind],
path.yaw[ind],
yaw1[ind],
kdtree=kdtree):
return path
return []