# Get Scene SCENE_FILE = join(dirname(abspath(__file__)), 'scenes/scene_cpp.ttt') # Start Simulation pr = PyRep() pr.launch(SCENE_FILE, headless=False) pr.start() robot = Shape('start_pose') vision_sensor = VisionSensor('vision_sensor') # Generate a random map randomMapGenerator.generate_random_map(no_of_objects, False) # Setup occ_grid occ_grid = OccupancyGrid() setupOccGrid(occ_grid, vision_sensor) # Compute block size from shape in simulation bounding_box = robot.get_bounding_box() block_size_x = int( round(bounding_box[1] - bounding_box[0], 3) / occ_grid.resolution) block_size_y = int( round(bounding_box[3] - bounding_box[2], 3) / occ_grid.resolution) submapper = GridSubmapper(occ_grid) submapper.process(block_size_x, block_size_y) planner = SubmapPlanner(occ_grid, block_size_x, block_size_y) cur_pos = planner.getPath(submapper.submaps, only_start=True)
# Since our "robot" is actually a shape we need to define our own 2d pose func # current_pose = get_2d_pose(robot) target_pose = get_2d_pose(goal) current_pose = robot.get_position(vision_sensor)[:2] + robot.get_orientation()[-1:] current_pose = (-current_pose[0], current_pose[1], current_pose[2]) print(current_pose) print(get_2d_pose(robot)) pr.step() origin_x, _, origin_y, _, _, _ = vision_sensor.get_bounding_box() # Setup occ_grid occ_grid = OccupancyGrid(origin_x, origin_y) depth = vision_sensor.capture_depth() occ_grid.fromDepth(depth) ''' dij_solver = Dijkstra(occ_grid, current_pose, target_pose) moves = dij_solver.solve() for i in moves: pr.step() set_2d_pose(robot, (i[0], i[1], current_pose[2])) time.sleep(0.05) ''' ''' # RRT Solver
# Set numpy printing options np.set_printoptions(threshold=100 * 100, formatter={'all': lambda x: str(x) + ','}) # Get Scene SCENE_FILE = join(dirname(abspath(__file__)), 'scenes/scene_nonrectilinear.ttt') # Start Simulation pr = PyRep() pr.launch(SCENE_FILE, headless=True) pr.start() # Setup occ_grid vision_sensor = VisionSensor('vision_sensor') occ_grid = OccupancyGrid() setupOccGrid(occ_grid, vision_sensor) pr.step() # End Simulation pr.stop() pr.shutdown() # Process img_grey = np.uint8(occ_grid.grid * 255) contours, hierarchy = cv2.findContours(img_grey, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) img_color = cv2.cvtColor(img_grey, cv2.COLOR_GRAY2BGR) intermediate_step = img_color.copy()