#setting next trjaectory--> start finding best trajectories if show_animation: axes[1,0].cla() axes[1,0].set_title('global & Local motion primitives') planner.plot_regions(axes[1,0]) # sample_goals = random_sampling(params,8) # generate goal points from waypoints vcd # sample_goals = random_sampling(params,5) sample_goals = goal_sampling_VCD(waypoint_vcd, states[0][0],states[0][1], params_globalmap) sample_goals2 = goal_sampling_VCD(waypoint_vcd, states[1][0],states[1][1], params_globalmap) # sample_goals_total=[sample_goals,sample_goals2] # generate global trjs to each sample goal #agent1 gtrjs= generating_globaltrjs(states[0], cspace,planner, obstacles,sample_goals2,params_globalmap) sp_gtrjs = trjs_to_sample(gtrjs) local_trjs = lane_state_sampling_test1(states[0],obstacles, params_globalmap) trjs_candidate =[] for gtrj in sp_gtrjs: trjs_candidate.append(gtrj) for ltrj in local_trjs: trjs_candidate.append(ltrj) #agent2 sample_goals2 = goal_sampling_VCD(waypoint_vcd2, states[1][0],states[1][1], params_globalmap) # sample_goals_total=[sample_goals,sample_goals2] gtrjs2= generating_globaltrjs(states[1], cspace2,planner2, obstacles,sample_goals2,params_globalmap) sp_gtrjs2 = trjs_to_sample(gtrjs2) local_trjs2 = lane_state_sampling_test1(states[1],obstacles, params_globalmap) trjs_candidate2 =[] for gtrj in sp_gtrjs2: trjs_candidate2.append(gtrj)
cm =plt.get_cmap('gist_rainbow') for i, sp in enumerate(gtrjs): col = cm(1.*i/num_colors) ds = 0.2 # [m] distance of each intepolated points s = np.arange(0, sp.s[-1], ds) rx, ry, ryaw, rk = [], [], [], [] s_iter=0 for i_s in s: ix, iy = sp.calc_position(i_s) rx.append(ix) ry.append(iy) axes[1,0].plot(rx[1:100], ry[1:100], color='g', label="spline") planner.plot_regions(axes[1,0]) #local trajectories local_trjs = lane_state_sampling_test1(state,axes[1,0]) calc_IG_trjs(local_trjs, entropymap , params_localmap, params_globalmap) # calc_IG_trjs # uniform_terminal_state_sampling_test1(state,axes[1,0]) # lane_state_sampling_test1(state,axes[1,0]) # uniform_terminal_state_sampling_test1(state,axes[1,0]) # axes[1,0].set_xlim([-1.2*params.area_size, 1.2*params.area_size]) # limit the plot space # axes[1,0].set_ylim([-1.2*params.area_size, 1.2*params.area_size]) # limit the plot space # planner.plot_regions(axes[1,0]) plt.show() # plt.show(aspect='auto')