for j in range(num_y):
obspoint_x = obs.x_min + i * 2 * params_globalmap.xyreso
obspoint_y = obs.y_min + j * 2 * params_globalmap.xyreso
obpoints.append([obspoint_x, obspoint_y])
# print("obspoints:", obpoints)
#create cspace
print("init_pos", init_pos)
goal_pos = [8.5, -2.0] #temporary goal pose
cspace = configuration_space()
cspace.reset_environment(params_globalmap.boundaries, init_pos, goal_pos,
obstacles)
planner = VerticalCellDecomposition(cspace)
planner.reset_cspace(cspace)
planner.vertical_lines()
planner.region_disection(goal_pos)
# planner.generate_waypoint(params_localmap)
# planner.plot_regions()
# cspace.plot_config_space()
# planner.construct_graph()
# path, path_idx = planner.search(True, goal_pos)
#Define four windows:
# axes[0,0] : robot, obstacle, goal_points,
# axes[1,0] : trajectories, region
# axes[0,1] : local sensor_map
# axes[1,1] : entropy_map
# fig,axes=plt.subplots(nrows=2,ncols=2,figsize=(40,40))
waypoint_vcd = planner.generate_waypoint(params_localmap)
#waypoint from vcd
for j in range(num_y):
obspoint_x = obs.x_min+i*2*params_globalmap.xyreso
obspoint_y = obs.y_min+j*2*params_globalmap.xyreso
obpoints.append([obspoint_x, obspoint_y])
# print("obspoints:", obpoints)
#create cspace
print("init_poses", init_poses)
goal_poses=[[2.5, -5.0],[5.5, 8.0]] #temporary goal pose
cspace=configuration_space()
cspace.reset_environment(params_globalmap.boundaries,init_poses[0],goal_poses[0], obstacles)
planner = VerticalCellDecomposition(cspace)
planner.reset_cspace(cspace)
planner.vertical_lines()
planner.region_disection(goal_poses[0])
cspace2=configuration_space()
cspace2.reset_environment(params_globalmap.boundaries,init_poses[1],goal_poses[1], obstacles)
planner2 = VerticalCellDecomposition(cspace2)
planner2.reset_cspace(cspace2)
planner2.vertical_lines()
planner2.region_disection(goal_poses[1])
# planner.generate_waypoint(params_localmap)
# planner.plot_regions()
# cspace.plot_config_space()
# planner.construct_graph()