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 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()