obs = Obstacle(-Region_Boundary, Region_Boundary, -Region_Boundary, -Region_Boundary,True)
walls.append(obs) # attach obstacle to obstacle list
obs = Obstacle(-Region_Boundary, Region_Boundary, Region_Boundary, Region_Boundary,True)
walls.append(obs) # attach obstacle to obstacle list
obs = Obstacle(Region_Boundary, Region_Boundary, -Region_Boundary, Region_Boundary,True)
walls.append(obs) # attach obstacle to obstacle list
#create cspace
# init_pos=[pos_x[0],pos_y[0]]
init_pos=[-1.5, 8]
goal_pos=[-5, 9]
# cspace = configuration_space(args['in'])
cspace=configuration_space()
cspace.reset_environment(params_globalmap.boundaries,init_pos,goal_pos, obstacles)
# cspace.plot_config_space()
planner = VerticalCellDecomposition(cspace)
planner.construct_graph()
# path, path_idx = planner.search(True)
waypoint_vcd = planner.generate_waypoint(params_localmap)
planner.plot_regions(axes[1,0])
#waypoint from vcd
way_x=[]
way_y=[]
for point in waypoint_vcd:
# print("x: ", point[0])
# print("y: ", point[1])
way_x.append(point[0])
way_y.append(point[1])
num_y = int(
round((obs.y_max - obs.y_min) / (2 * params_globalmap.xyreso))) + 1
for i in range(num_x):
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))
help="optimization? [y/n] (default: y)",
default="y")
args = vars(parser.parse_args())
#Define two windows:
# axes[0] : robot, obstacle, waypoints, trajectory
# axes[1] : sensor_map,occ_grid
fig, axes = plt.subplots(nrows=3, ncols=1, figsize=(10, 30))
params = Params()
params_globalmap = map_params()
params_localmap = map_params()
cspace = configuration_space(args['in'])
init_pos = cspace.start_state
goal_pos = cspace.goal_state
planner = VerticalCellDecomposition(cspace)
planner.construct_graph()
waypoint_vcd = planner.generate_waypoint(params_localmap)
if args['load'] == 'y':
timeindex = "04171450"
# timeindex = "04021858"
# Open the desired file for reading
dir_path = os.path.dirname(os.path.realpath(__file__))
dir_path = dir_path[:-4]
file_name = dir_path + "/results/data/robot_" + timeindex + "_.csv"
wayfile_name = dir_path + "/results/data/waypoints_" + timeindex + "_.csv"
obsfile_name = dir_path + "/results/data/obstacles2_" + timeindex + "_.csv"
# Open the desired file for reading