Exemplo n.º 1
0
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()
Exemplo n.º 2
0
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()
Exemplo n.º 3
0
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()
Exemplo n.º 4
0
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()