# 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()
