Пример #1
0
    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])
Пример #2
0
        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))
Пример #3
0
                        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