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 main():
s = [-14, 7, 0]
g = [0, 0, math.pi / 2]
ox, oy = make_ob()
path = planner.calc_hybrid_astar_path(s[0], s[1], s[2], g[0], g[1], g[2],
ox, oy, planner.XY_GRID_RESOLUTION,
planner.YAW_GRID_RESOLUTION)
fig, ax = plt.subplots()
plt.axis("equal")
plt.subplots_adjust(bottom=0.35)
ax_x = plt.axes([0.1, 0.25, 0.8, 0.03], facecolor='lightgoldenrodyellow')
ax_y = plt.axes([0.1, 0.20, 0.8, 0.03], facecolor='lightgoldenrodyellow')
ax_yaw = plt.axes([0.1, 0.15, 0.8, 0.03], facecolor='lightgoldenrodyellow')
ax_index = plt.axes([0.1, 0.10, 0.8, 0.03],
facecolor='lightgoldenrodyellow')
s_x = Slider(ax_x, 'x', -20.0, 20.0, valinit=s[0])
s_y = Slider(ax_y, 'y', 5.0, 10.0, valinit=s[1])
s_yaw = Slider(ax_yaw, 'yaw', -math.pi, math.pi, valinit=s[2])
s_index = Slider(ax_index, 'index', 0, 1.0, valinit=0)
ax.plot(ox, oy, 'ko')
plot_vehicle(ax, s[0], s[1], s[2])
plot_vehicle(ax, g[0], g[1], g[2])
def update_index(val):
global path
ax.cla()
ax.plot(ox, oy, 'ko')
x = s_x.val
y = s_y.val
yaw = s_yaw.val
index = s_index.val
plot_vehicle(ax, x, y, yaw)
plot_vehicle(ax, g[0], g[1], g[2])
_idx = max(min(int(len(path.x) * index), len(path.x) - 1), 0)
plot_vehicle(ax, path.x[_idx], path.y[_idx], path.yaw[_idx])
ax.plot(path.x, path.y, 'r-')
fig.canvas.draw_idle()
def update(val):
global path
ax.cla()
x = s_x.val
y = s_y.val
yaw = s_yaw.val
index = s_index.val
ax.plot(ox, oy, 'ko')
plot_vehicle(ax, x, y, yaw)
plot_vehicle(ax, g[0], g[1], g[2])
path = planner.calc_hybrid_astar_path(x, y, yaw, g[0], g[1], g[2], ox,
oy, planner.XY_GRID_RESOLUTION,
planner.YAW_GRID_RESOLUTION)
ax.plot(path.x, path.y, 'r-')
fig.canvas.draw_idle()
s_x.on_changed(update)
s_y.on_changed(update)
s_yaw.on_changed(update)
s_index.on_changed(update_index)
plt.show()
def update(val):
global path
ax.cla()
x = s_x.val
y = s_y.val
yaw = s_yaw.val
index = s_index.val
ax.plot(ox, oy, 'ko')
plot_vehicle(ax, x, y, yaw)
plot_vehicle(ax, g[0], g[1], g[2])
path = planner.calc_hybrid_astar_path(x, y, yaw, g[0], g[1], g[2], ox,
oy, planner.XY_GRID_RESOLUTION,
planner.YAW_GRID_RESOLUTION)
ax.plot(path.x, path.y, 'r-')
fig.canvas.draw_idle()
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)
print len(yield_x_2)
for mx, my, myaw in zip(yield_x_2, yield_y_2, yield_yaw_2):
oox, ooy = ox[:], oy[:]
states = States(final_mx_1, final_my_1, final_myaw_1, mx, my, np.deg2rad(myaw), ox, oy)
states_set_2[s_id] = states
s_id = s_id + 1
for i in range(len(states_set_2)):
s_id = min(
states_set_2, key=lambda o: states_set_2[o].estimated_cost)
states = states_set_2[s_id]
del states_set_2[s_id]
print 'mx:', states.mx, 'my:', states.my, 'myaw:', states.myaw, 'cost:', states.estimated_cost
final_mx_2, final_my_2, final_myaw_2 = main_middle(final_mx_1, final_my_1, final_myaw_1, states.mx, states.my, states.myaw, OTHER_OTHER_SX, OTHER_OTHER_SY, OTHER_OTHER_SYAW, OTHER_OTHER_GX, OTHER_OTHER_GY, OTHER_OTHER_GYAW, OTHER_GX, OTHER_GY, OTHER_GYAW)
if final_mx_2 != None and final_my_2 != None and final_myaw_2 != None:
break
oox = ox[:]
ooy = oy[:]
path3 = planner.calc_hybrid_astar_path(final_mx_2, final_my_2, final_myaw_2, GX, GY, GYAW, oox, ooy,
planner.XY_GRID_RESOLUTION,
planner.YAW_GRID_RESOLUTION)
show_animation(path3, ox, oy, OTHER_OTHER_GX, OTHER_OTHER_GY, OTHER_OTHER_GYAW, OTHER_GX, OTHER_GY, OTHER_GYAW)
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()
for i in range(61):
ox.append(i)
oy.append(60.0)
for i in range(61):
ox.append(0.0)
oy.append(i)
for i in range(20):
ox.append(i)
oy.append(30.0)
for i in range(45):
ox.append(i)
oy.append(40.0)
plt.grid(True)
plt.axis("equal")
plt.plot(ox, oy, '.k')
vehicle.plot_trailer(SX, SY, SYAW, 0)
path1 = planner.calc_hybrid_astar_path(SX, SY, SYAW, GX, GY, GYAW, ox, oy,
planner.XY_GRID_RESOLUTION,
planner.YAW_GRID_RESOLUTION)
#plt.scatter(path1.x, path1.y, "-r", label="Hybrid A* path")
show_animation(path1, ox, oy)
# #plt.scatter(x_path,y_path)
plt.axis([-40, 40, -40, 40])
plt.grid(True)
plt.axis("equal")
# plt.plot(ox, oy, '.k')
plt.show()