#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')