def calc_hybrid_astar_path(sx, sy, syaw, gx, gy, gyaw, ox, oy, xyreso,
yawreso):
syaw0 = rs_path.pi_2_pi(syaw)
gyaw0 = rs_path.pi_2_pi(gyaw)
global tox, toy
ox, oy = ox[:], oy[:]
tox, toy = ox[:], oy[:]
kdtree = KDTree(np.vstack((tox, toy)).T)
c = calc_config(ox, oy, xyreso, yawreso)
nstart = Node(round(sx / xyreso), round(sy / xyreso),
round(syaw0 / yawreso), True, [sx], [sy], [syaw0], [True],
0.0, 0.0, -1)
ngoal = Node(round(gx / xyreso), round(gy / xyreso),
round(gyaw0 / yawreso), True, [gx], [gy], [gyaw0], [True],
0.0, 0.0, -1)
h_dp = grid_a_star.calc_dist_policy(ngoal.x[-1], ngoal.y[-1], ox, oy,
xyreso, 1.0)
openset, closedset = {}, {}
fnode = None
openset[calc_index(nstart, c)] = nstart
pq = queue.PriorityQueue()
pq.put(calc_index(nstart, c), calc_cost(nstart, h_dp, ngoal, c))
u, d = calc_motion_inputs()
nmotion = len(u)
times = 0
while 1:
if len(openset) == 0:
print("Error: Cannot find path, No open set")
return []
c_id = min(
openset,
key=lambda o: openset[o].cost + calc_cost(nstart, h_dp, ngoal, c))
current = openset[c_id]
# move current node from open to closed
del openset[c_id]
closedset[c_id] = current
isupdated, fpath = update_node_with_analystic_expantion(
current, ngoal, c, ox, oy, kdtree)
if isupdated: # found
fnode = fpath
break
for i in range(nmotion):
node = calc_next_node(current, c_id, u[i], d[i], c)
if verify_index(node, c, ox, oy, kdtree) == False:
continue
node_ind = calc_index(node, c)
# If it is already in the closed set, skip it
if node_ind in closedset:
continue
if not node_ind in openset:
openset[node_ind] = node
pq.put(node_ind, calc_cost(nstart, h_dp, ngoal, c))
else:
if openset[node_ind].cost > node.cost:
# If so, update the node to have a new parent
openset[node_ind] = node
times = times + 1
print("final expand node:", len(openset) + len(closedset))
path = get_final_path(closedset, fnode, nstart, c)
return path
def calc_next_node(current, c_id, u, d, c):
arc_l = XY_GRID_RESOLUTION * 1.5
nlist = math.ceil(arc_l / MOTION_RESOLUTION) + 1
xlist, ylist, yawlist = [], [], []
xlist_0 = current.x[-1] + d * MOTION_RESOLUTION * math.cos(current.yaw[-1])
ylist_0 = current.y[-1] + d * MOTION_RESOLUTION * math.sin(current.yaw[-1])
yawlist_0 = rs_path.pi_2_pi(current.yaw[-1] +
d * MOTION_RESOLUTION / WB * math.tan(u))
xlist.append(xlist_0)
ylist.append(ylist_0)
yawlist.append(yawlist_0)
for i in range(1, int(nlist)):
xlist_i = xlist[i -
1] + d * MOTION_RESOLUTION * math.cos(yawlist[i - 1])
ylist_i = ylist[i -
1] + d * MOTION_RESOLUTION * math.sin(yawlist[i - 1])
yawlist_i = rs_path.pi_2_pi(yawlist[i - 1] +
d * MOTION_RESOLUTION / WB * math.tan(u))
xlist.append(xlist_i)
ylist.append(ylist_i)
yawlist.append(yawlist_i)
xind = round(xlist[-1] / c.xyreso)
yind = round(ylist[-1] / c.xyreso)
yawind = round(yawlist[-1] / c.yawreso)
addedcost = 0.0
if d > 0:
direction = True
addedcost += abs(arc_l)
else:
direction = False
addedcost += abs(arc_l) * BACK_COST
# swich back penalty
if direction != current.direction: # switch back penalty
addedcost += SB_COST
# steer penalyty
addedcost += STEER_COST * abs(u)
# steer change penalty
addedcost += STEER_CHANGE_COST * abs(current.steer - u)
cost = current.cost + addedcost
directions = [direction for i in range(len(xlist))]
node = Node(xind, yind, yawind, direction, xlist, ylist, yawlist,
directions, u, cost, c_id)
return node
def calc_next_node(current, c_id, u, d, c):
arc_l = XY_GRID_RESOLUTION*1.5
nlist = int(math.floor(arc_l/MOTION_RESOLUTION)) + 1
xlist = np.zeros(nlist)
ylist = np.zeros(nlist)
yawlist = np.zeros(nlist)
yaw1list = np.zeros(nlist)
xlist[0] = current.x[-1] + d * MOTION_RESOLUTION*math.cos(current.yaw[-1])
ylist[0] = current.y[-1] + d * MOTION_RESOLUTION*math.sin(current.yaw[-1])
yawlist[0] = rs_path.pi_2_pi(current.yaw[-1] + d*MOTION_RESOLUTION/WB * math.tan(u))
yaw1list[0] = rs_path.pi_2_pi(current.yaw1[-1] + d*MOTION_RESOLUTION/LT*math.sin(current.yaw[-1]-current.yaw1[-1]))
for i in range(nlist-1):
xlist[i+1] = xlist[i] + d * MOTION_RESOLUTION*math.cos(yawlist[i])
ylist[i+1] = ylist[i] + d * MOTION_RESOLUTION*math.sin(yawlist[i])
yawlist[i+1] = rs_path.pi_2_pi(yawlist[i] + d*MOTION_RESOLUTION/WB * math.tan(u))
yaw1list[i+1] = rs_path.pi_2_pi(yaw1list[i] + d*MOTION_RESOLUTION/LT*math.sin(yawlist[i]-yaw1list[i]))
xind = int(round(xlist[-1]/c.xyreso))
yind = int(round(ylist[-1]/c.xyreso))
yawind = int(round(yawlist[-1]/c.yawreso))
addedcost = 0.0
if d > 0:
direction = 1
addedcost += abs(arc_l)
else:
direction = 0
addedcost += abs(arc_l) * BACK_COST
# switch back penalty
if direction != current.direction: # switch back penalty
addedcost += SB_COST
# steer penalty
addedcost += STEER_COST*abs(u)
# steer change penalty
addedcost += STEER_CHANGE_COST*abs(current.steer - u)
# jacknif cost
addedcost += JACKKNIF_COST * sum([abs(rs_path.pi_2_pi(yawlist[i] - yaw1list[i])) for i in range(len(yawlist))])
cost = current.cost + addedcost
directions = [direction for i in range(len(xlist))]
node = Node(xind, yind, yawind, direction, xlist, ylist, yawlist, yaw1list, directions, u, cost, c_id)
return node
def calc_rs_path_cost(rspath, yaw1):
# calculate the path cost
cost = 0.0
for l in rspath.lengths:
if l >= 0: # forward
cost += l
else: # back
cost += abs(l) * BACK_COST
# swich back penalty
for i in range(len(rspath.lengths) - 1):
if rspath.lengths[i] * rspath.lengths[i+1] < 0.0: # switch back
cost += SB_COST
# steer penalyty
for ctype in rspath.ctypes:
if ctype != "S": # curve
cost += STEER_COST*abs(MAX_STEER)
# ==steer change penalty
# calc steer profile
nctypes = len(rspath.ctypes)
ulist = np.zeros(nctypes)
for i in range(nctypes):
if rspath.ctypes[i] == "R":
ulist[i] = - MAX_STEER
elif rspath.ctypes[i] == "L":
ulist[i] = MAX_STEER
for i in range(len(rspath.ctypes) - 1):
cost += STEER_CHANGE_COST*abs(ulist[i+1] - ulist[i])
# print ("cost: ", cost)
cost += JACKKNIF_COST * sum([abs(rs_path.pi_2_pi(rspath.yaw[i] - yaw1[i])) for i in range(len(yaw1))])
return cost
def update_node_with_analystic_expantion(current, ngoal, c, ox, oy, kdtree, gyaw1):
# use Reed-Shepp model to find a path between current node and goal node without obstacles, which will not run every time for computational reasons.
apath = analystic_expantion(current, ngoal, c, ox, oy, kdtree)
if apath != None:
fx = apath.x[1:]
fy = apath.y[1:]
fyaw = apath.yaw[1:]
steps = MOTION_RESOLUTION*apath.directions
yaw1 = tralierlib.calc_trailer_yaw_from_xyyaw(apath.x, apath.y, apath.yaw, current.yaw1[-1], steps)
if abs(rs_path.pi_2_pi(yaw1[-1] - gyaw1)) >= GOAL_TYAW_TH:
return 0, None #no update
fcost = current.cost + calc_rs_path_cost(apath, yaw1)
fyaw1 = yaw1[1:]
fpind = calc_index(current, c)
fd = []
for d in apath.directions[1:]:
if d >= 0:
fd.append(1)
else:
fd.append(0)
fsteer = 0.0
fpath = Node(current.xind, current.yind, current.yawind, current.direction, fx, fy, fyaw, fyaw1, fd, fsteer, fcost, fpind)
return 1, fpath
return 0, None #no update
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 calc_rs_path_cost(rspath, yaw1):
"""
This function calculates the total cost for a given RS path and yaw1
:param rspath: rs_path.Path, RS path
:param yaw1: radians, trailer yaw
:return: total cost
"""
# Initialize cost
cost = 0
# Update cost for length and direction of path
for l in rspath.lengths:
if l >= 0:
cost += l
else:
cost += abs(l) * BACK_COST
# Update cost if direction changes in the path (detected by change in sign of adjacent lengths)
for i in (range(len(rspath.lengths) - 1)):
if rspath.lengths[i] * rspath.lengths[i + 1] < 0:
cost += SB_COST
# Update cost for curved path
for ctype in rspath.ctypes:
if ctype != "S":
cost += STEER_COST * abs(MAX_STEER)
# Form list of steering inputs
nctypes = len(rspath.ctypes) # number of ctypes in the given path
ulist = [
] # list used to convert the string ctypes to numbers for cost calculation
for i in range(nctypes):
if rspath.ctypes[i] == "R":
ulist.append(-MAX_STEER)
elif rspath.ctypes[i] == "L":
ulist.append(MAX_STEER)
else:
ulist.append(0)
# Update cost for changing direction of turn
for i in range(nctypes - 2):
cost += STEER_CHANGE_COST * abs(ulist[i + 1] - ulist[i])
# Update cost to prevent jackknifes (the most the trailer folds toward the truck, the higher the cost)
cost += JACKKNIF_COST * sum(
np.absolute(rs_path.pi_2_pi(np.subtract(rspath.yaw, yaw1))))
return cost
def calc_hybrid_astar_path(sx, sy, syaw, gx, gy, gyaw, ox, oy, xyreso,
yawreso):
# sx: start x position[m]
# sy: start y position[m]
# gx: goal x position[m]
# gx: goal x position[m]
# ox: x position list of Obstacles[m]
# oy: y position list of Obstacles[m]
# xyreso: grid resolution[m]
# yawreso: yaw angle resolution[rad]
syaw0 = rs_path.pi_2_pi(syaw)
gyaw0 = rs_path.pi_2_pi(gyaw)
global tox, toy
ox, oy = ox[:], oy[:]
tox, toy = ox[:], oy[:]
kdtree = KDTree(np.vstack((tox, toy)).T)
# a = np.array((1,2,3))
# b = np.array((2,3,4))
# c = np.hstack((a,b))
# tree = spatial.KDTree((a,b),10)
# print tree.date
c = calc_config(ox, oy, xyreso, yawreso)
#c.prn_obj()
nstart = Node(round(sx / xyreso), round(sy / xyreso),
round(syaw0 / yawreso), True, [sx], [sy], [syaw0], [True],
0.0, 0.0, -1)
ngoal = Node(round(gx / xyreso), round(gy / xyreso),
round(gyaw0 / yawreso), True, [gx], [gy], [gyaw0], [True],
0.0, 0.0, -1)
h_dp = calc_holonomic_with_obstacle_heuristic(ngoal, ox, oy, xyreso)
openset, closedset = {}, {}
fnode = None
openset[calc_index(nstart, c)] = nstart
pq = Q.PriorityQueue()
pq.put(calc_index(nstart, c), calc_cost(nstart, h_dp, ngoal, c))
u, d = calc_motion_inputs()
nmotion = len(u)
if vehicle_lib.check_collision(ox, oy, [sx], [sy], [syaw0],
kdtree) == False:
print('1111111')
return []
if vehicle_lib.check_collision(ox, oy, [gx], [gy], [gyaw0],
kdtree) == False:
print('2222222')
return []
times = 0
while 1:
# if times >100:
# return []
if len(openset) == 0:
print("Error: Cannot find path, No open set")
return []
c_id = min(
openset,
key=lambda o: openset[o].cost + calc_cost(nstart, h_dp, ngoal, c))
#c_id = pq.get()
current = openset[c_id]
# move current node from open to closed
del openset[c_id]
closedset[c_id] = current
# visualize_x = []
# visualize_y = []
# for v in closedset.values():
# visualize_x.append(v.x[-1])
# visualize_y.append(v.y[-1])
# print visualize_x,visualize_y
# plt.plot(tox, toy, ".k")
# plt.plot(visualize_x,visualize_y,'.k')
# plt.pause(0.1)
isupdated, fpath = update_node_with_analystic_expantion(
current, ngoal, c, ox, oy, kdtree, True)
if isupdated: # found
fnode = fpath
break
for i in range(nmotion):
node = calc_next_node(current, c_id, u[i], d[i], c)
if verify_index(node, c, ox, oy, kdtree) == False:
continue
node_ind = calc_index(node, c)
#print 'expend node id:', node_ind
# If it is already in the closed set, skip it
if node_ind in closedset:
continue
if node_ind not in openset:
openset[node_ind] = node
pq.put(node_ind, calc_cost(nstart, h_dp, ngoal, c))
else:
if openset[node_ind].cost > node.cost:
# If so, update the node to have a new parent
openset[node_ind] = node
times = times + 1
print("final expand node:", len(openset) + len(closedset))
path = get_final_path(closedset, fnode, nstart, c)
return path
def calc_hybrid_astar_path(sx, sy, syaw, syaw1, gx, gy, gyaw, gyaw1, ox, oy,
xyreso, yawreso):
"""
:param sx: meters, start x position
:param sy: meters, start y position
:param syaw: radians, start tractor angle
:param syaw1: radians, start trailer angle
:param gx: meters, goal x position
:param gy: meters, goal y position
:param gyaw: radians, goal tractor angle
:param gyaw1: radians, goal trailer angle
:param ox: meters, list of x coordinates of each obstacle
:param oy: meters, list of y coordinates of each obstacle
:param xyreso: meters, xy grid resolution
:param yawreso: radians, yaw resolution
:return:
"""
syaw, gyaw = rs_path.pi_2_pi(syaw), rs_path.pi_2_pi(
gyaw) # function used to ensure yaw is between -pi and pi
oxy = [(ox[i], oy[i]) for i in range(0, len(ox))
] # forms a list of tuples from the two lists ox and oy for
# input to kd.create
# forms a kd tree from the xy points of the obstacles
print('Forming Obstacle KD Tree...')
kdtree = spatial.KDTree(oxy)
# Form the configuration space
c = calc_config(ox, oy, xyreso, yawreso)
# Define the starting node
nstart = Node(round(sx / xyreso), round(sy / xyreso),
round(syaw / yawreso), True, [sx], [sy], [syaw], [syaw1],
[True], 0, 0, -1)
# Define the goal node
ngoal = Node(round(gx / xyreso), round(gy / xyreso), round(gyaw / yawreso),
True, [gx], [gy], [gyaw], [gyaw1], [True], 0, 0, -1)
# Determine holonomic costs of each xy coordinate on the grid (numpy dependent)
print('Calculating Holonomic Costs...')
h_dp = calc_holonomic_with_obstacle_heuristics(ngoal, ox, oy, xyreso)
opened = {} # Dictionary to hold open nodes
closed = {} # Dictionary to hold closed nodes
pq = {
} # Dictionary to hold queue of indexes and costs, used to determine which node in open to expand next
fnode = []
# Add the start node to the dictionary of open nodes
opened[calc_index(nstart, c)] = nstart
# Add the start node to the dictionary of indexes and costs
pq[calc_index(nstart, c)] = calc_cost(nstart, h_dp, c)
u, d = calc_motion_inputs()
nmotion = len(u)
iteration = 1
print('Commencing Hybrid A* Search...')
while True:
if iteration % 10 == 0:
print(
f'{iteration} loops complete. {len(opened)} nodes opened. {len(closed)} nodes closed.'
)
iteration += 1
# Exit while loop and return nothing if open is expended and a solution is not found
if not opened:
print("Error: Cannot find path, No open nodes remaining.")
return []
# Obtain the index of the next node
c_id = min(pq.keys())
# Removed the obtained index from the queue
pq.pop(c_id)
# Get the node with the obtained index and remove it from open
current = opened.get(c_id)
opened.pop(c_id)
# Add the current node to the closed list
closed.update({c_id: current})
# get full data of current node and isupdated flag
isupdated, fpath = update_node_with_analystic_expantion(
current, ngoal, c, ox, oy, kdtree, gyaw1)
if isupdated: # goal has been found
fnode = fpath
break # exit the while loop
inityaw1 = current.yaw1[0]
# cycle through all inputs and check if each next node is in closed, else add it to open if not already there
for i in range(nmotion):
node = calc_next_node(current, c_id, u[i], d[i], c)
if not verify_index(node, c, ox, oy, inityaw1, kdtree):
continue # continue with next node since this node's configuration is invalid
node_ind = calc_index(node, c)
# Check if node is already in the closed dictionary
if node_ind in closed:
continue # continue with next node since this one is already closed
# Check if node is already in the open dictionary
if node_ind not in opened:
opened.update({node_ind: node}) # add the node to open
pq.update({node_ind: calc_cost(node, h_dp, c)
}) # add the node cost information to pq
else:
if opened[
node_ind].cost > node.cost: # if node is in the open dictionary, but the new node cost is
# lower, update the node in open
opened[node_ind] = node
print('Forming Final Path...')
path = get_final_path(closed, fnode, nstart, c)
return path
def calc_next_node(current, c_id, u, d, c):
"""
This function calculates the next node based on the current node and the given inputs.
:param current: Node, current node
:param c_id: index, index of current node
:param u: input, steering input
:param d: input, driving input
:param c: Config, Cspace
:return:
"""
arc_l = XY_GRID_RESOLUTION * 1.5
nlist = math.floor(arc_l / MOTION_RESOLUTION) + 1
# Initial motion from the current node
xlist = [current.x[-1] + d * MOTION_RESOLUTION * math.cos(current.yaw[-1])]
ylist = [current.y[-1] + d * MOTION_RESOLUTION * math.sin(current.yaw[-1])]
yawlist = [
rs_path.pi_2_pi(current.yaw[-1] +
d * MOTION_RESOLUTION * math.tan(u) / WB)
]
yaw1list = [
rs_path.pi_2_pi(current.yaw1[-1] + d * MOTION_RESOLUTION *
math.sin(current.yaw[-1] - current.yaw1[-1]) / LT)
]
# Discrete path from current node to the next node for the given inputs
for i in range(nlist - 1):
xlist.append(xlist[-1] + d * MOTION_RESOLUTION * math.cos(yawlist[-1]))
ylist.append(ylist[-1] + d * MOTION_RESOLUTION * math.sin(yawlist[-1]))
yawlist.append(
rs_path.pi_2_pi(yawlist[-1] +
d * MOTION_RESOLUTION * math.tan(u) / WB))
yaw1list.append(
rs_path.pi_2_pi(yaw1list[-1] + d * MOTION_RESOLUTION *
math.sin(yawlist[-1] - yaw1list[-1]) / LT))
# Determine the index in each dimension of the next node
xind = round(xlist[-1] / c.xyreso)
yind = round(ylist[-1] / c.xyreso)
yawind = round(yawlist[-1] / c.yawreso)
# Calculate cost to the next node
addedcost = 0 # Initialize cost of moving from current node to the next node
# Cost of traveling length in given direction
if d > 0:
direction = True
addedcost += abs(arc_l)
else:
direction = False
addedcost += abs(arc_l) * BACK_COST
# Cost of switching direction
if direction != current.direction: # A direction change occured
addedcost += SB_COST
# Cost of steering
addedcost += STEER_COST * abs(u)
# Cost of changing direction
addedcost += STEER_CHANGE_COST * abs(current.steer - u)
# Cost of acute angle between trailer and tractor
addedcost += JACKKNIF_COST * sum(
np.absolute(rs_path.pi_2_pi(np.subtract(yawlist, yaw1list))))
cost = current.cost + addedcost
# Form list of directions for the node class
directions = [direction for i in range(len(xlist))]
node = Node(xind, yind, yawind, direction, xlist, ylist, yawlist, yaw1list,
directions, u, cost, c_id)
return node
def calc_hybrid_astar_path(sx, sy, syaw, syaw1, gx, gy, gyaw, gyaw1, ox, oy, xyreso, yawreso):
"""
sx: start x position [m]
sy: start y position [m]
gx: goal x position [m]
gx: goal x position [m]
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
xyreso: grid resolution [m]
yawreso: yaw angle resolution [rad]
"""
ox = np.asarray(ox)
oy = np.asarray(oy)
syaw = rs_path.pi_2_pi(syaw)
gyaw = rs_path.pi_2_pi(gyaw)
kdtree = KDTree(np.asarray([ox, oy]).T)
# get the configuration of whole map (length, width. obstacles)
c = Config()
ox, oy = c.calc_config(ox, oy, xyreso, yawreso)
nstart = Node(int(round(sx/xyreso)), int(round(sy/xyreso)), int(round(syaw/yawreso)), 1, np.asarray([sx]), np.asarray([sy]), np.asarray([syaw]), np.asarray([syaw1]), np.asarray([1]), 0.0, 0.0, -1)
ngoal = Node(int(round(gx/xyreso)), int(round(gy/xyreso)), int(round(gyaw/yawreso)), 1, np.asarray([gx]), np.asarray([gy]), np.asarray([gyaw]), np.asarray([gyaw1]), np.asarray([1]), 0.0, 0.0, -1)
# To get a map with the h cost (distance to goal node) of each grid (without obstacle)
h_dp = calc_holonomic_with_obstacle_heuristic(ngoal, ox, oy, xyreso)
# print (h_dp)
open_set = {}
closed_set = {}
fnode = None
open_set[calc_index(nstart, c)] = nstart
pq =[]
heappush(pq, (calc_cost(nstart, h_dp, c), calc_index(nstart, c)))
u, d = calc_motion_inputs()
nmotion = len(u)
while (1):
starttime = time.time()
# print ("pq: ",pq)
print ("pq: ",len(pq))
# print ("open_set: ",open_set)
if len(open_set) == 0:
print("Error: Cannot find path, No open set")
return []
c_id = heappop(pq)[1]
current = open_set[c_id]
#move current node from open to closed
del open_set[c_id]
closed_set[c_id] = current
# use Reed-Shepp model to find a path between current node and goal node without obstacles, which will not run every time for computational reasons.
isupdated, fpath = update_node_with_analystic_expantion(current, ngoal, c, ox, oy, kdtree, gyaw1)
print ("isupdated: ", isupdated)
if isupdated: # found
fnode = fpath
break
inityaw1 = current.yaw1[0]
for i in range(nmotion):
# print ("current.xind: ", current.xind, " current.yind: ", current.yind, " current.cost: ", current.cost, " c_id: ", c_id, " u[i]: ", u[i], " d[i]: ", d[i])
# For each node, it will have multiple possible points but with one x,y index
node = calc_next_node(current, c_id, u[i], d[i], c)
# print ("node.xind: ", node.xind, " node.yind: ", node.yind, " node.cost: ", node.cost)
if not verify_index(node, c, ox, oy, inityaw1, kdtree):
continue
node_ind = calc_index(node, c)
# If it is already in the closed set, skip it
if node_ind in closed_set:
continue
if node_ind not in open_set:
open_set[node_ind] = node
heappush(pq, (calc_cost(node, h_dp, c), node_ind))
else:
if open_set[node_ind].cost > node.cost:
# If so, update the node to have a new parent
open_set[node_ind] = node
endtime = time.time()
print ("onetime: ", endtime-starttime)
print("final expand node:", len(open_set) + len(closed_set))
##################################
path = get_final_path(closed_set, fnode, nstart, c)
# ====Animation=====
animation.show_animation(path, ox, oy, sx, sy, syaw, syaw1, gx, gy, gyaw, gyaw1)
return path
