def analystic_expantion(n, ngoal, ox, oy, kdtree):
sx = n.x[-1]
sy = n.y[-1]
syaw = n.yaw[-1]
max_curvature = math.tan(MAX_STEER) / WB
paths = rs_path.calc_paths(sx,
sy,
syaw,
ngoal.x[-1],
ngoal.y[-1],
ngoal.yaw[-1],
max_curvature,
step_size=MOTION_RESOLUTION)
if len(paths) == 0:
return None
pathset = {}
path_id = 0
for path in paths:
pathset[path_id] = path
path_id = path_id + 1
ind = 0
for i in range(len(pathset)):
p_id = min(pathset, key=lambda o: calc_rs_path_cost(pathset[o]))
path = pathset[p_id]
if vehicle_lib.check_collision(ox, oy, path.x, path.y, path.yaw,
kdtree):
return path
return None
def main_middle(sx,
sy,
syaw,
mx,
my,
myaw,
other_sx,
other_sy,
other_syaw,
other_gx,
other_gy,
other_gyaw,
other_other_x=None,
other_other_y=None,
other_other_yaw=None):
# set obstacles
global ox, oy
oox = ox[:]
ooy = oy[:]
tox, toy = ox[:], oy[:]
kdtree = planner.KDTree(np.vstack((tox, toy)).T)
if vehicle_lib.check_collision(oox, ooy, [mx], [my], [myaw],
kdtree) == False:
return None, None, None
vehicle_x, vehicle_y = planner.vehicle.plot_trailer(
mx, my, myaw, 0.0, True)
oox_add, ooy_add = add_obstacle(vehicle_x, vehicle_y, oox, ooy)
path2 = planner.calc_hybrid_astar_path(other_sx, other_sy, other_syaw,
other_gx, other_gy, other_gyaw,
oox_add, ooy_add,
planner.XY_GRID_RESOLUTION,
planner.YAW_GRID_RESOLUTION)
if path2 == []:
print 'The middle state is invalid(2)'
return None, None, None
# path generation
path1 = planner.calc_hybrid_astar_path(sx, sy, syaw, mx, my, myaw, oox,
ooy, planner.XY_GRID_RESOLUTION,
planner.YAW_GRID_RESOLUTION)
if path1 == []:
print 'Cannot find path.(1)'
return None, None, None
vehicle.plot_trailer(other_sx, other_sy, other_syaw, 0)
show_animation(path1, ox, oy, other_sx, other_sy, other_syaw,
other_other_x, other_other_y, other_other_yaw)
show_animation(path2, ox, oy, mx, my, myaw, other_other_x, other_other_y,
other_other_yaw)
plt.pause(0.01)
return mx, my, myaw
def verify_index(node, c, ox, oy, kdtree):
if (node.xind - c.minx) >= c.xw or (node.xind - c.minx) <= 0 or (
node.yind - c.miny) >= c.yw or (node.yind - c.miny) <= 0:
return False
if vehicle_lib.check_collision(ox, oy, node.x, node.y, node.yaw,
kdtree) == False:
return False
return True
def analystic_expantion(n, ngoal, ox, oy, kdtree, draw):
sx = n.x[-1]
sy = n.y[-1]
syaw = n.yaw[-1]
max_curvature = math.tan(MAX_STEER) / WB
paths = rs_path.calc_paths(sx,
sy,
syaw,
ngoal.x[-1],
ngoal.y[-1],
ngoal.yaw[-1],
max_curvature,
step_size=MOTION_RESOLUTION)
if len(paths) == 0:
return None
#pathqueue = PriorityQueue{rs_path.Path}
#pathqueue = Queue.PriorityQueue()
pathset = {}
path_id = 0
for path in paths: #yaw1 = vehicle_lib.calc_trailer_yaw_from_xyyaw(path.x, path.y, path.yaw, n.yaw1[-1], steps)
#pathqueue.put(path, calc_rs_path_cost(path))
pathset[path_id] = path
path_id = path_id + 1
ind = 0
for i in range(len(pathset)):
#path = pathqueue.get()
p_id = min(pathset, key=lambda o: calc_rs_path_cost(pathset[o]))
path = pathset[p_id]
# if draw == True:
# plt.grid(True)
# plt.axis("equal")
# plt.plot(path.x, path.y, linewidth = '0.3', color= 'red')
# plt.pause(0.01)
if vehicle_lib.check_collision(ox, oy, path.x, path.y, path.yaw,
kdtree):
#plt.plot(path.x, path.y, "-^b")
return path # path is ok
return None
def verify_index(node, c, ox, oy, kdtree):
# overflow map
if (node.xind - c.minx) >= c.xw:
return False
elif (node.xind - c.minx) <= 0:
return False
if (node.yind - c.miny) >= c.yw:
return False
elif (node.yind - c.miny) <= 0:
return False
# check collisiton
#steps = MOTION_RESOLUTION*node.directions
#yaw1 = vehicle_lib.calc_trailer_yaw_from_xyyaw(node.x, node.y, node.yaw, inityaw1, steps)
#ind = 1:SKIP_COLLISION_CHECK:length(node.x)
#if !trailerlib.check_trailer_collision(ox, oy, node.x[ind], node.y[ind], node.yaw[ind], yaw1[ind], kdtree = kdtree):
#return False
if vehicle_lib.check_collision(ox, oy, node.x, node.y, node.yaw, kdtree) == False:
return False
return True #index is ok"
def main(x, y, yaw):
sx = SX # [m]
sy = SY # [m]
syaw0 = SYAW
# goal state
gx = GX # [m]
gy = GY # [m]
gyaw0 = GYAW
other_sx = OTHER_SX # [m]
other_sy = OTHER_SY # [m]
other_syaw0 = OTHER_SYAW
other_gx = OTHER_GX # [m]
other_gy = OTHER_GY # [m]
other_gyaw0 = OTHER_GYAW
mx = x # [m]
my = y # [m]
myaw0 = yaw
# set obstacles
global ox, oy
oox = ox[:]
ooy = oy[:]
tox, toy = ox[:], oy[:]
kdtree = planner.KDTree(np.vstack((tox, toy)).T)
if vehicle_lib.check_collision(oox, ooy, [mx], [my], [myaw0],
kdtree) == False:
return
# path generation
vehicle_x, vehicle_y = planner.vehicle.plot_trailer(
mx, my, myaw0, 0.0, True)
oox_add, ooy_add = add_obstacle(vehicle_x, vehicle_y, oox, ooy)
path2 = planner.calc_hybrid_astar_path(other_sx, other_sy, other_syaw0,
other_gx, other_gy, other_gyaw0,
oox_add, ooy_add,
planner.XY_GRID_RESOLUTION,
planner.YAW_GRID_RESOLUTION)
if path2 == []:
print('The middle state is invalid(2)')
return
path1 = planner.calc_hybrid_astar_path(sx, sy, syaw0, mx, my, myaw0, oox,
ooy, planner.XY_GRID_RESOLUTION,
planner.YAW_GRID_RESOLUTION)
if path1 == []:
print('Cannot find path.(1)')
return
path3 = planner.calc_hybrid_astar_path(mx, my, myaw0, gx, gy, gyaw0, oox,
ooy, planner.XY_GRID_RESOLUTION,
planner.YAW_GRID_RESOLUTION)
if path3 == []:
print('Cannot find path.(3)')
return
# global total_cost
# global final_path1
# global final_path2
# global final_path3,final_mx,final_my,final_myaw0
#
# if path1.cost + path2.cost + path3.cost < total_cost:
# final_path1 = path1
# final_path2 = path2
# final_path3 = path3
# final_mx = mx
# final_my = my
# final_myaw0 = myaw0
plt.grid(True)
plt.axis("equal")
plt.plot(ox, oy, '.k')
plt.pause(5)
show_animation(path1, ox, oy, other_sx, other_sy, other_syaw0)
show_animation(path2, ox, oy, mx, my, myaw0)
show_animation(path3, ox, oy, other_gx, other_gy, other_gyaw0)
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 main(x, y, yaw):
sx = SX # [m]
sy = SY # [m]
syaw0 = SYAW
# goal state
gx = GX # [m]
gy = GY # [m]
gyaw0 = GYAW
other_sx = OTHER_SX # [m]
other_sy = OTHER_SY # [m]
other_syaw0 = OTHER_SYAW
other_gx = OTHER_GX # [m]
other_gy = OTHER_GY # [m]
other_gyaw0 = OTHER_GYAW
mx = x # [m]
my = y # [m]
myaw0 = np.deg2rad(yaw)
# set obstacles
ox = []
oy = []
# for i in range(-8, 8):
# ox.append(i)
# oy.append(12.0)
for i in range(-25, -8):
ox.append(i)
oy.append(9.0)
for i in range(8, 25):
ox.append(i)
oy.append(9.0)
for i in range(-25, -8):
ox.append(i)
oy.append(4.0)
for i in range(-3, 5):
ox.append(-8.0)
oy.append(i)
for i in range(-3, 5):
ox.append(8.0)
oy.append(i)
for i in range(8, 25):
ox.append(i)
oy.append(4.0)
# for i in range(-8, 8):
# ox.append(i)
# oy.append(1.0)
for i in range(-8, 8):
ox.append(i)
oy.append(12.0)
for i in range(4, 9):
ox.append(-25)
oy.append(i)
for i in range(4, 9):
ox.append(25)
oy.append(i)
oox = ox[:]
ooy = oy[:]
tox, toy = ox[:], oy[:]
kdtree = planner.KDTree(np.vstack((tox, toy)).T)
# plt.plot(ox,oy,'.k')
# plt.show()
if vehicle_lib.check_collision(oox, ooy, [mx], [my], [myaw0],
kdtree) == False:
return
# path generation
path1 = planner.calc_hybrid_astar_path(sx, sy, syaw0, mx, my, myaw0, ox,
oy, planner.XY_GRID_RESOLUTION,
planner.YAW_GRID_RESOLUTION)
if path1 == []:
print 'Cannot find path.(1)'
return
vehicle_x, vehicle_y = planner.vehicle.plot_trailer(
mx, my, myaw0, 0.0, True)
oox_add, ooy_add = add_obstacle(vehicle_x, vehicle_y, oox, ooy)
path2 = planner.calc_hybrid_astar_path(other_sx, other_sy, other_syaw0,
other_gx, other_gy, other_gyaw0,
oox_add, ooy_add,
planner.XY_GRID_RESOLUTION,
planner.YAW_GRID_RESOLUTION)
if path2 == []:
print 'The middle state is invalid(2)'
return
path3 = planner.calc_hybrid_astar_path(mx, my, myaw0, gx, gy, gyaw0, ox,
oy, planner.XY_GRID_RESOLUTION,
planner.YAW_GRID_RESOLUTION)
if path3 == []:
print 'Cannot find path.(3)'
return
# global total_cost
# global final_path1
# global final_path2
# global final_path3,final_mx,final_my,final_myaw0
#
# if path1.cost + path2.cost + path3.cost < total_cost:
# final_path1 = path1
# final_path2 = path2
# final_path3 = path3
# final_mx = mx
# final_my = my
# final_myaw0 = myaw0
vehicle.plot_trailer(other_sx, other_sy, other_syaw0, 0)
show_animation(path1, oox, ooy, other_sx, other_sy, other_syaw0)
show_animation(path2, oox, ooy, mx, my, myaw0)
show_animation(path3, oox, ooy, other_gx, other_gy, other_gyaw0)
plt.show()
