def senarios_1():
# database connection
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
# plot
fig = plt.figure()
# ax1 = fig.add_subplot(111, projection='3d')
ax1 = fig.add_subplot(111)
# ax2 = fig.add_subplot(212)
# road center line points
p = (0.01, 0.0070893847415232263, 0.0056488099243383414, -0.01, 109.61234595301809)
center_line = TG.spiral3_calc(p, s=100.,q=(3.,25.,0.))
# p = (0.,0.,0.,0.,90.) # (p0~p3, sg)
# center_line = TG.spiral3_calc(p, q=(5.,50.,0.))
# np.savetxt('scenario_1/road_center_line.txt', center_line, delimiter='\t')
# print(center_line)
# road
road = Road(center_line)
ax1.plot(center_line[:,1], center_line[:,2], color='maroon', linestyle='--', linewidth=1.)
ax1.plot(road.lateral_lines[:,0], road.lateral_lines[:,1],color='green', linewidth=1.)
ax1.plot(road.lateral_lines[:,-2], road.lateral_lines[:,-1],color='green', linewidth=1.)
for i in range(road.grid_num_lateral+1):
if (i % road.grid_num_per_lane) == 0:
ax1.plot(road.longitudinal_lines[:,2*i], road.longitudinal_lines[:,2*i+1], color='green', linewidth=1.)
# else:
# ax1.plot(road.longitudinal_lines[:,2*i], road.longitudinal_lines[:,2*i+1], color='black', linewidth=0.3)
# for i in range(road.grid_num_longitudinal+1):
# ax1.plot(road.lateral_lines[:,2*i], road.lateral_lines[:,2*i+1],color='black', linewidth=0.3)
# vehicle
cfg0 = road.sl2xy(5.,0.)
cfg5 = road.sl2xy(90.,0.)
veh = Vehicle(trajectory=np.array([[-1.,-1.,cfg0[0], cfg0[1], cfg0[2], cfg0[3], 0.,5.,0.]]))
veh5 = Vehicle(trajectory=np.array([[-1.,-1.,cfg5[0], cfg5[1], cfg5[2], cfg5[3], 0.,5.,0.]]))
# ax1.plot(cfg0[0], cfg0[1], 'ko')
# ax1.text(cfg0[0], cfg0[1]+0.4, 'Start')
# # ax1.plot(cfg5[0], cfg5[1], 'ko')
# ax1.text(cfg5[0], cfg5[1]+0.4, 'Goal')
codes6 = [Path.MOVETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.CLOSEPOLY,
]
# verts0 = [tuple(veh.vertex[i]) for i in range(6)]
# verts0.append(verts0[0])
# ax1.add_patch(patches.PathPatch(Path(verts0, codes6), facecolor='green', alpha=0.5))
# verts5 = [tuple(veh5.vertex[i]) for i in range(6)]
# verts5.append(verts5[0])
# ax1.add_patch(patches.PathPatch(Path(verts5, codes6), facecolor='red', alpha=0.5))
# workspace
ws = Workspace(vehicle=veh, road=road)
road_lane_bitmap0 = ws.lane_grids[0]
road_lane_bitmap1 = ws.lane_grids[1]
road_lane_bitmap2 = ws.lane_grids[2]
# write the lane bitmaps into files
# np.savetxt('road_lane_bitmap0.txt', road_lane_bitmap0, fmt='%i',delimiter=' ')
# np.savetxt('road_lane_bitmap1.txt', road_lane_bitmap1, fmt='%i',delimiter=' ')
# np.savetxt('road_lane_bitmap2.txt', road_lane_bitmap2, fmt='%i',delimiter=' ')
# road bitmap
road_bitmap = road_lane_bitmap0 + road_lane_bitmap1 + road_lane_bitmap2
road_bitmap = np.where(road_bitmap>1.e-6, 1., 0.)
# np.savetxt('road_bitmap.txt', road_bitmap, fmt='%i', delimiter=' ')
# base bitmap
base = 1. - road_bitmap
# base = np.where(base>1.e-6, np.inf, 0)
# np.savetxt('base_bitmap.txt', base, fmt='%i', delimiter=' ')
# static obstacles
cfg1 = road.sl2xy(30., 0.)
cfg2 = road.sl2xy(30., -road.lane_width)
cfg3 = road.sl2xy(65.,0.)
cfg4 = road.sl2xy(65., road.lane_width)
obst1 = Vehicle(trajectory=np.array([[-1.,-1.,cfg1[0], cfg1[1], cfg1[2], cfg1[3], 0.,0.,0.]]))
obst2 = Vehicle(trajectory=np.array([[-1.,-1.,cfg2[0], cfg2[1], cfg2[2], cfg2[3], 0.,0.,0.]]))
obst3 = Vehicle(trajectory=np.array([[-1.,-1.,cfg3[0], cfg3[1], cfg3[2], cfg3[3], 0.,0.,0.]]))
obst4 = Vehicle(trajectory=np.array([[-1.,-1.,cfg4[0], cfg4[1], cfg4[2], cfg4[3], 0.,0.,0.]]))
verts1 = [tuple(obst1.vertex[i]) for i in range(6)]
verts1.append(verts1[0])
ax1.add_patch(patches.PathPatch(Path(verts1, codes6), facecolor='cyan'))
verts2 = [tuple(obst2.vertex[i]) for i in range(6)]
verts2.append(verts2[0])
ax1.add_patch(patches.PathPatch(Path(verts2, codes6), facecolor='cyan'))
verts3 = [tuple(obst3.vertex[i]) for i in range(6)]
verts3.append(verts3[0])
ax1.add_patch(patches.PathPatch(Path(verts3, codes6), facecolor='cyan'))
verts4 = [tuple(obst4.vertex[i]) for i in range(6)]
verts4.append(verts4[0])
ax1.add_patch(patches.PathPatch(Path(verts4, codes6), facecolor='cyan'))
base += ws.grids_occupied_by_polygon(obst1.vertex)
base += ws.grids_occupied_by_polygon(obst2.vertex)
base += ws.grids_occupied_by_polygon(obst3.vertex)
base += ws.grids_occupied_by_polygon(obst4.vertex)
base = np.where(base>1.e-6, 1.,0.)
# np.savetxt('scenario_1/static_bitmap.txt', base, fmt='%i', delimiter=' ')
# collision map
collision_map = cv2.filter2D(base, -1, ws.collision_filter)
collision_map = np.where(collision_map>1.e-6, 1., 0.)
# np.savetxt('scenario_1/collision_bitmap.txt', collision_map, fmt='%i', delimiter=' ')
# cost map
cost_map = cv2.filter2D(collision_map, -1, ws.cost_filter)
cost_map += collision_map
cost_map = np.where(cost_map>1., np.inf, cost_map)
cost_map = np.where(cost_map<1.e-8, 0., cost_map)
# costmap_save = np.where( cost_map >1., -1., cost_map)
# np.savetxt('scenario_1/cost_map.txt', costmap_save, delimiter='\t')
# plot
# costmap_plot = np.where( cost_map >1., 1., cost_map)
# ax1.imshow(costmap_plot, cmap=plt.cm.Reds, origin="lower",extent=(0.,ws.resolution*ws.row,0.,ws.resolution*ws.column))
# heuristic map
goal_state = State(road=road, r_s=90., r_l=0., v=10.)
# ax1.scatter(goal_state.x, goal_state.y, c='r')
ax1.plot(goal_state.x, goal_state.y, 'rs')
heuristic_map = heuristic_map_constructor(goal_state, cost_map)
# hm_save = np.where(heuristic_map > np.finfo('d').max, -1., heuristic_map)
# np.savetxt('scenario_1/heuristic_map.txt', hm_save, delimiter='\t')
start_state = State(time=0., length=0., road=road, r_s=5., r_l=0., v=10.,cost=0., heuristic_map=heuristic_map)
# ax1.scatter(start_state.x, start_state.y, c='r')
ax1.plot(start_state.x, start_state.y, 'rs')
# ax1.imshow(heuristic_map, cmap=plt.cm.Reds, origin="lower",extent=(0.,ws.resolution*ws.row,0.,ws.resolution*ws.column))
# # weights: weights for (k, dk, v, a, a_c, l, env, j, t, s)
weights = np.array([5., 10., -0.1, 10., 0.1, 0.1, 50., 5, 40., -4.])
starttime = datetime.datetime.now()
res, state_dict, traj_dict = Astar(start_state, goal_state, road, cost_map, veh, heuristic_map, cursor, weights=weights)
endtime = datetime.datetime.now()
print((endtime - starttime).total_seconds()*1000) # 2.6s
print(res)
print(len(state_dict)) #66
print(len(traj_dict))
# print(goal_state.time, goal_state.length, goal_state.cost, start_state.heuristic, goal_state.heuristic)
# # True
# 168
# 175
# 8.78814826688 76.409797813 2701.06684421 1559.33663366 0.0
# for _ , traj in traj_dict.items():
# ax1.plot(traj[:,2], traj[:,3], traj[:,0], color='navy', linewidth=0.3)
# ax1.plot(traj[:,2], traj[:,3], color='navy', linewidth=1.)
# for _, state in state_dict.items():
# if state != start_state and state != goal_state:
# ax1.plot(state.x, state.y, 'go')
# ax1.text(state.x, state.y,'{0:.2f}'.format(state.cost))
state = goal_state
rows = 0
while state.parent is not None:
traj = traj_dict[(state.parent, state)]
ax1.plot(traj[:,2], traj[:,3], color='magenta', linewidth=3.)
rows += traj.shape[0]
ax1.plot(state.x, state.y, 'go')
ax1.plot(state.parent.x, state.parent.y, 'go')
state = state.parent
final_traj=np.zeros((rows,9))
state = goal_state
while state.parent is not None:
traj = traj_dict[(state.parent, state)]
final_traj[(rows-traj.shape[0]):rows,:] = traj
rows -= traj.shape[0]
state = state.parent
# with open('scenario_1/final_traj.pickle','wb') as f3:
# pickle.dump(final_traj, f3)
for i in range(31):
state1 = trajectory_interp(final_traj, i*goal_state.time/30)
if state1 is not None:
obst_d1 = Vehicle(trajectory=np.array([[-1.,-1.,state1[2], state1[3], state1[4], 0., 0.,0.,0.]]))
verts_d1 = [tuple(obst_d1.vertex[i]) for i in range(6)]
verts_d1.append(verts_d1[0])
ax1.add_patch(patches.PathPatch(Path(verts_d1, codes6), facecolor='blue', alpha=0.1+0.03*i))
# close database connection
cursor.close()
conn.close()
#
# plt.legend()
plt.axis('equal')
# plt.savefig('scenario_1/astar_3.png', dpi=600)
plt.show()
def senarios_4():
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
fig = plt.figure()
ax = fig.add_subplot(111)
p = (0.01, 0.0070893847415232263, 0.0056488099243383414, -0.01, 109.61234595301809)
center_line = TG.spiral3_calc(p, s=100.,q=(3.,25.,0.))
road = Road(center_line, ref_grid_width=0.5, ref_grid_length=1.)
# ax.plot(center_line[:,1], center_line[:,2], color='red', linewidth=1.)
ax.plot(road.lateral_lines[:,0], road.lateral_lines[:,1],color='green', linewidth=1.)
ax.plot(road.lateral_lines[:,-2], road.lateral_lines[:,-1],color='green', linewidth=1.)
for i in range(road.grid_num_lateral+1):
if (i % road.grid_num_per_lane) == 0:
ax.plot(road.longitudinal_lines[:,2*i], road.longitudinal_lines[:,2*i+1], color='green', linewidth=1.)
#
# ws = Workspace(road=road, lane_costs=[0.4,0.1,0.2])
# cost_map_base = ws.lane_map
# ax.imshow(ws.lane_map, cmap=plt.cm.Blues, origin='lower', extent=(0,100,0,100))
s1 = State(time=0.,length=0.,road=road,r_s=25.,r_l=road.lane_width, v=15.)
g1 = State(road=road,r_s=95.,r_l=road.lane_width,v=15.)
traj1 = trajectory_forward(s1,g1,cursor)
# print(traj1[-1,:])
# ax.plot(traj1[:,2], traj1[:,3], color='navy', linewidth=2.)
s2 = State(time=0.,length=0.,road=road,r_s=20.,r_l=0.,v=6.)
g2 = State(road=road,r_s=95.,r_l=0.,v=6.)
traj2 = trajectory_forward(s2,g2,cursor)
# print(traj2[-1,:])
# ax.plot(traj2[:,2], traj2[:,3], color='navy', linewidth=2.)
cfg3 = road.sl2xy(30.,-road.lane_width)
obst_s = Vehicle(trajectory=np.array([[-1.,-1.,cfg3[0], cfg3[1], cfg3[2], cfg3[3], 0.,0.,0.]]))
cfg0 = road.sl2xy(5.,0.)
veh0 = Vehicle(trajectory=np.array([[-1.,-1.,cfg0[0], cfg0[1], cfg0[2], cfg0[3], 0.,0.,0.]]))
cfg1 = road.sl2xy(90.,0.)
veh1 = Vehicle(trajectory=np.array([[-1.,-1.,cfg1[0], cfg1[1], cfg1[2], cfg1[3], 0.,0.,0.]]))
ax.plot(cfg0[0], cfg0[1], 'ko')
ax.text(cfg0[0], cfg0[1]+0.4, 'Start')
ax.plot(cfg1[0], cfg1[1], 'ko')
ax.text(cfg1[0], cfg1[1]+0.4, 'Goal')
#
codes6 = [Path.MOVETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.CLOSEPOLY,
]
verts_s = [tuple(obst_s.vertex[i]) for i in range(6)]
verts_s.append(verts_s[0])
ax.add_patch(patches.PathPatch(Path(verts_s, codes6), facecolor='cyan'))
verts0 = [tuple(veh0.vertex[i]) for i in range(6)]
verts0.append(verts0[0])
ax.add_patch(patches.PathPatch(Path(verts0, codes6), facecolor='green', alpha=0.5))
verts1 = [tuple(veh1.vertex[i]) for i in range(6)]
verts1.append(verts1[0])
ax.add_patch(patches.PathPatch(Path(verts1, codes6), facecolor='red', alpha=0.5))
for i in range(20):
state1 = trajectory_interp(traj1, i*0.2)
state2 = trajectory_interp(traj2, i*0.2)
if state1 is not None:
obst_d1 = Vehicle(trajectory=np.array([[-1.,-1.,state1[2], state1[3], state1[4], 0., 0.,0.,0.]]))
verts_d1 = [tuple(obst_d1.vertex[i]) for i in range(6)]
verts_d1.append(verts_d1[0])
ax.add_patch(patches.PathPatch(Path(verts_d1, codes6), facecolor='cyan', alpha=(i+1)/20.))
if state2 is not None:
obst_d2 = Vehicle(trajectory=np.array([[-1.,-1.,state2[2], state2[3], state2[4], 0., 0.,0.,0.]]))
verts_d2 = [tuple(obst_d2.vertex[i]) for i in range(6)]
verts_d2.append(verts_d2[0])
ax.add_patch(patches.PathPatch(Path(verts_d2, codes6), facecolor='cyan', alpha=(i+1)/20.))
#
# cost_maps = np.zeros((150,500,500))
# grids_s = ws.grids_occupied_by_polygon(obst_s.vertex)
# lane_grids = sum(ws.lane_grids)
# lane_grids = np.where(lane_grids>1.,1., lane_grids)
# off_road_map = 1. - lane_grids
# grids_s += off_road_map
# for i in range(150):
# obst_map = np.zeros((500,500))
# obst_map += grids_s
# state1 = trajectory_interp(traj1, i/10.)
# state2 = trajectory_interp(traj2, i/10.)
# if state1 is not None:
# obst_d1 = Vehicle(trajectory=np.array([[-1.,-1.,state1[2], state1[3], state1[4], 0., 0.,0.,0.]]))
# grids_d1 = ws.grids_occupied_by_polygon(obst_d1.vertex)
# obst_map += grids_d1
# if state2 is not None:
# obst_d2 = Vehicle(trajectory=np.array([[-1.,-1.,state2[2], state2[3], state2[4], 0., 0.,0.,0.]]))
# grids_d2 = ws.grids_occupied_by_polygon(obst_d2.vertex)
# obst_map += grids_d2
# collision_map = cv2.filter2D(obst_map, -1, ws.collision_filter)
# collision_map = np.where(collision_map>1.e-6, 1., 0.)
# cost_map = cv2.filter2D(collision_map, -1, ws.cost_filter)
# cost_map += collision_map
# cost_map = np.where(cost_map>1., np.inf, cost_map)
# cost_map = np.where(cost_map<1.e-8, 0., cost_map)
# cost_map += cost_map_base
# cost_maps[i,:,:] = cost_map
# with open('scenario_4/cost_maps.pickle','wb') as f1:
# pickle.dump(cost_maps, f1)
# plt.xlabel('$x (m)$', fontsize=20)
# plt.ylabel('$y (m)$', fontsize=20)
plt.axis('equal')
# plt.axis('off')
# plt.savefig('scenario_4/obstacles2.png', dpi=600)
plt.show()
cursor.close()
conn.close()
def scenario_4_sim():
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
cost_maps = np.zeros((150,500,500))
with open('scenario_4/cost_maps.pickle', 'rb') as f1:
cost_maps = pickle.load(f1)
heuristic_maps = np.zeros((150,500,500))
with open('scenario_4/heuristic_maps.pickle', 'rb') as f2:
heuristic_maps = pickle.load(f2)
fig = plt.figure()
ax = fig.add_subplot(111)
p = (0.01, 0.0070893847415232263, 0.0056488099243383414, -0.01, 109.61234595301809)
center_line = TG.spiral3_calc(p, s=100.,q=(3.,25.,0.))
road = Road(center_line, ref_grid_width=0.5, ref_grid_length=1.)
# ax.plot(center_line[:,1], center_line[:,2], color='red', linewidth=1.)
ax.plot(road.lateral_lines[:,0], road.lateral_lines[:,1],color='green', linewidth=1.)
ax.plot(road.lateral_lines[:,-2], road.lateral_lines[:,-1],color='green', linewidth=1.)
for i in range(road.grid_num_lateral+1):
if (i % road.grid_num_per_lane) == 0:
ax.plot(road.longitudinal_lines[:,2*i], road.longitudinal_lines[:,2*i+1], color='green', linewidth=1.)
goal = State(road=road, r_s=90., r_l=0., v=10.,static=False)
start = State(time=0., length=0., road=road, r_s=5., r_l=0., v=10.,cost=0.,heuristic_map=heuristic_maps, static=False)
veh = Vehicle(trajectory=np.array([[-1.,-1.,start.x, start.y, start.theta, start.k, 0.,0.,0.]]))
# # weights: weights for (k, dk, v, a, a_c, l, env, j, t, s)
weights = np.array([5., 10., -0.1, 10., 0.1, 1., 50., 5, 30., -2.])
starttime = datetime.datetime.now()
res, state_dict, traj_dict = Astar(start, goal, road, cost_maps, veh, heuristic_maps, cursor, static=False, weights=weights)
endtime = datetime.datetime.now()
print((endtime - starttime).total_seconds()*1000) # 4.8s
print(res)
print(len(state_dict)) #96
print(len(traj_dict))
# for _ , traj in traj_dict.items():
# ax.plot(traj[:,2], traj[:,3], traj[:,0], color='navy', linewidth=0.3)
# ax.plot(traj[:,2], traj[:,3], color='blue', linewidth=1.)
# for _, state in state_dict.items():
# if state != start and state != goal:
# ax.plot(state.x, state.y, 'go')
# ax.text(state.x, state.y,'{0:.2f}'.format(state.cost))
state = goal
rows = 0
while state.parent is not None:
traj = traj_dict[(state.parent, state)]
ax.plot(traj[:,2], traj[:,3], color='magenta', linewidth=3.)
rows += traj.shape[0]
ax.plot(state.x, state.y, 'go')
ax.plot(state.parent.x, state.parent.y, 'go')
state = state.parent
# ax.plot(traj[:,2], traj[:,3], traj[:,0], color='teal', linewidth=1.)
# ax.plot(traj[:,0], traj[:,7], color='black', linewidth=0.5)
# print(rows)
final_traj=np.zeros((rows,9))
state = goal
# row = 0
while state.parent is not None:
traj = traj_dict[(state.parent, state)]
final_traj[(rows-traj.shape[0]):rows,:] = traj
rows -= traj.shape[0]
# row += traj.shape[0]
state = state.parent
with open('scenario_4/final_traj.pickle','wb') as f3:
pickle.dump(final_traj, f3)
#
#################
s1 = State(time=0.,length=0.,road=road,r_s=25.,r_l=road.lane_width, v=15.)
g1 = State(road=road,r_s=95.,r_l=road.lane_width,v=15.)
traj1 = trajectory_forward(s1,g1,cursor)
# print(traj1[-1,:])
# ax.plot(traj1[:,2], traj1[:,3], color='navy', linewidth=2.)
s2 = State(time=0.,length=0.,road=road,r_s=20.,r_l=0.,v=6.)
g2 = State(road=road,r_s=95.,r_l=0.,v=6.)
traj2 = trajectory_forward(s2,g2,cursor)
# print(traj2[-1,:])
# ax.plot(traj2[:,2], traj2[:,3], color='navy', linewidth=2.)
cfg3 = road.sl2xy(30.,-road.lane_width)
obst_s = Vehicle(trajectory=np.array([[-1.,-1.,cfg3[0], cfg3[1], cfg3[2], cfg3[3], 0.,0.,0.]]))
codes6 = [Path.MOVETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.CLOSEPOLY,
]
verts_s = [tuple(obst_s.vertex[i]) for i in range(6)]
verts_s.append(verts_s[0])
ax.add_patch(patches.PathPatch(Path(verts_s, codes6), facecolor='cyan'))
for i in range(31):
state1 = trajectory_interp(traj1, i*goal.time/30)
state2 = trajectory_interp(traj2, i*goal.time/30)
state3 = trajectory_interp(final_traj, i*goal.time/30)
if state1 is not None:
obst_d1 = Vehicle(trajectory=np.array([[-1.,-1.,state1[2], state1[3], state1[4], 0., 0.,0.,0.]]))
verts_d1 = [tuple(obst_d1.vertex[i]) for i in range(6)]
verts_d1.append(verts_d1[0])
ax.add_patch(patches.PathPatch(Path(verts_d1, codes6), facecolor='cyan', alpha=0.1+0.03*i))
if state2 is not None:
obst_d2 = Vehicle(trajectory=np.array([[-1.,-1.,state2[2], state2[3], state2[4], 0., 0.,0.,0.]]))
verts_d2 = [tuple(obst_d2.vertex[i]) for i in range(6)]
verts_d2.append(verts_d2[0])
ax.add_patch(patches.PathPatch(Path(verts_d2, codes6), facecolor='cyan', alpha=0.1+0.03*i))
if state3 is not None:
obst_d3 = Vehicle(trajectory=np.array([[-1.,-1.,state3[2], state3[3], state3[4], 0., 0.,0.,0.]]))
verts_d3 = [tuple(obst_d3.vertex[i]) for i in range(6)]
verts_d3.append(verts_d3[0])
ax.add_patch(patches.PathPatch(Path(verts_d3, codes6), facecolor='blue', alpha=0.1+0.03*i))
plt.axis('equal')
# plt.axis('off')
# plt.savefig('scenario_4/obstacles2.png', dpi=600)
plt.show()
cursor.close()
conn.close()
def navigation():
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
motion_primitives = {}
with open('motion_primitives3.pickle', 'rb') as f:
motion_primitives.update(pickle.load(f))
# {v:{(i,j,k,l):(traj, u)}}
cost_map = np.zeros((500,500))
with open('scenario_3/cost_map.pickle', 'rb') as f1:
cost_map = pickle.load(f1)
heuristic_map = np.zeros((500,500))
with open('scenario_3/heuristic_map.pickle', 'rb') as f2:
heuristic_map = pickle.load(f2)
fig = plt.figure()
ax = fig.add_subplot(111)
boundary_t = [(0.,0.), (100.,0.), (100.,100.), (0.,100.)]
boundary_t.append(boundary_t[0])
obst1_t = [(50.,15.), (75.,15.), (75.,40.), (50.,40.)]
obst1_t.append(obst1_t[0])
obst2_t = [(60.,60.), (85.,60.), (85.,70.), (70.,70.), (70.,90.), (60.,90.)]
obst2_t.append(obst2_t[0])
obst3_t = [(20.,20.), (40.,20.), (40.,40.), (20.,40.)]
obst3_t.append(obst3_t[0])
obst4_t = [(15.,60.), (50.,60.), (50.,80.), (15.,80.)]
obst4_t.append(obst4_t[0])
codes4 = [Path.MOVETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.CLOSEPOLY,
]
codes6 = [Path.MOVETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.CLOSEPOLY,
]
path_b = Path(boundary_t, codes4)
path1 = Path(obst1_t, codes4)
path2 = Path(obst2_t, codes6)
path3 = Path(obst3_t, codes4)
path4 = Path(obst4_t, codes4)
patch_b = patches.PathPatch(path_b, facecolor='green', alpha=0.3)
patch1 = patches.PathPatch(path1, facecolor='cyan')
patch2 = patches.PathPatch(path2, facecolor='cyan')
patch3 = patches.PathPatch(path3, facecolor='cyan')
patch4 = patches.PathPatch(path4, facecolor='cyan')
ax.add_patch(patch_b)
ax.add_patch(patch1)
ax.add_patch(patch2)
ax.add_patch(patch3)
ax.add_patch(patch4)
s_t = (20.,40.,np.pi/4.)
veh = Env.Vehicle(trajectory=np.array([[-1.,-1.,s_t[0], s_t[1], s_t[2], 0., 0.,0.,0.]]))
veh_verts = [tuple(veh.vertex[i]) for i in range(6)]
veh_verts.append(veh_verts[0])
veh_path = Path(veh_verts, codes6)
veh_patch = patches.PathPatch(veh_path, facecolor='red')
# ax.add_patch(veh_patch)
goal = State(state=(80.,90.,-np.pi/3.,0.))
start = State(state=(10.,10.,0.,0.), cost=0., heuristic_map=heuristic_map)
veh1 = Env.Vehicle(trajectory=np.array([[-1.,-1.,start.state[0], start.state[1], start.state[2], 0., 0.,0.,0.]]))
veh2 = Env.Vehicle(trajectory=np.array([[-1.,-1.,goal.state[0], goal.state[1], goal.state[2], 0., 0.,0.,0.]]))
veh1_verts = [tuple(veh1.vertex[i]) for i in range(6)]
veh2_verts = [tuple(veh2.vertex[i]) for i in range(6)]
veh1_verts.append(veh1_verts[0])
veh2_verts.append(veh2_verts[0])
veh1_path = Path(veh1_verts, codes6)
veh2_path = Path(veh2_verts, codes6)
# veh1_patch = patches.PathPatch(veh1_path, facecolor='red')
# veh2_patch = patches.PathPatch(veh2_path, facecolor='purple')
# ax.add_patch(veh1_patch)
# ax.add_patch(veh2_patch)
starttime = datetime.datetime.now()
res, state_dict, traj_dict = Astar(start, goal, cost_map=cost_map, heuristic_map=heuristic_map, vehicle=None, cursor=cursor, motion_primitives=motion_primitives)
endtime = datetime.datetime.now()
print((endtime - starttime).total_seconds()*1000) # 3.4s
print("Size of Graph: {0}".format(len(traj_dict))) # 846
# with open('scenario_3/state_dict.pickle','wb') as f1:
# pickle.dump(state_dict, f1)
# with open('scenario_3/traj_dict.pickle','wb') as f2:
# pickle.dump(traj_dict, f2)
if res:
print("Planning Successes.")
else:
print("Planning Fails.")
# for (s1, s2), traj1 in traj_dict.items():
# plt.plot(traj1[:,2], traj1[:,3], 'navy')
state = goal
rows = 0
while state.parent is not None:
ax.plot(state.state[0], state.state[1], 'ro')
ax.plot(state.parent.state[0], state.parent.state[1], 'ro')
traj = traj_dict[(state.parent, state)]
rows += traj.shape[0]
ax.plot(traj[:,2], traj[:,3], color='magenta', linewidth=2.)
state = state.parent
final_traj=np.zeros((rows,9))
state = goal
while state.parent is not None:
traj = traj_dict[(state.parent, state)]
final_traj[(rows-traj.shape[0]):rows,:] = traj
rows -= traj.shape[0]
state = state.parent
# with open('scenario_3/final_traj.pickle','wb') as f3:
# pickle.dump(final_traj, f3)
for i in range(61):
state1 = trajectory_interp(final_traj, i*goal.time/60)
if state1 is not None:
obst_d1 = Env.Vehicle(trajectory=np.array([[-1.,-1.,state1[2], state1[3], state1[4], 0., 0.,0.,0.]]))
verts_d1 = [tuple(obst_d1.vertex[i]) for i in range(6)]
verts_d1.append(verts_d1[0])
ax.add_patch(patches.PathPatch(Path(verts_d1, codes6), facecolor='blue', alpha=0.1+0.015*i))
plt.axis('equal')
plt.show()
cursor.close()
conn.close()
