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