for obs in obstacles:
num_x = int(
round((obs.x_max - obs.x_min) / (2 * params_globalmap.xyreso))) + 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
default="output.txt")
# Optional arguments, only for PRM/RRT
parser.add_argument("-n",
help="number of samples for PRM/RRT (default: 1000)",
default=1000)
parser.add_argument(
"-k",
help="number of nearest neighbors for PRM (default: 5)",
default=5)
parser.add_argument("-plot",
help="plot final output? [y/n] (default: y)",
default='y')
args = vars(parser.parse_args())
cspace = configuration_space(args['in'])
if args['algo'] == 'prm':
planner = PRM(cspace, args['n'])
planner.perform_sampling(False)
planner.get_knn(args['k'])
path, path_idx = planner.search(args['plot'] == 'y')
elif args['algo'] == 'rrt':
planner = RRT(cspace, args['n'])
planner.perform_sampling(False)
path, path_idx = planner.search(args['plot'] == 'y')
write_to_file(planner, path_idx, args['out'])