def test_compare_maps_with_floodfill(): ground_truth = cv2.imread( get_maps_dir() + "/test/vw_ground_truth_test.png", cv2.COLOR_BGR2GRAY) current_map = cv2.imread( get_maps_dir() + "/test/vw_partly_explored_test.png", cv2.COLOR_BGR2GRAY) current_map = Costmap(current_map, 1, np.array([0., 0.])) start_state = np.array([200, 230, 0]) num_free = compute_connected_pixels(start_state, ground_truth).shape[0] measured_num_free = np.argwhere(current_map.data == Costmap.FREE).shape[0] x = measured_num_free / float(num_free) assert np.round(x, 4) == 0.2790
def main(): test_creation() test_reset() test_step_movement() test_step_data() test_compare_maps() run_example = False if run_example: test_map_filename = os.path.join(get_maps_dir(), "test/circle.png") footprint = CustomFootprint(np.array([[0., 0.]]), np.pi / 2) env = GridWorld(map_filename=test_map_filename, footprint=footprint, sensor=Neighborhood(sensor_range=1, values=[0, 255]), start_state=[1, 3, 0], map_resolution=1, thicken_obstacles=False) _, _ = env.reset() path = np.array([[0, -1, 0], [0, -2, 0], [1, -2, 0], [2, -2, 0], [2, -1, 0], [2, 0, 0], [1, 0, 0], [0, 0, 0]]) for point in path: _, _ = env.step(point) env.render(wait_key=0)
def test_compare_maps(): """ Tests the compare maps function ---maps/test/test.png 0 0 0 0 0 0 255 255 255 0 0 255 0 255 0 0 255 255 255 0 0 0 0 0 0 ---end file """ test_map_filename = os.path.join(get_maps_dir(), "test/circle.png") footprint = CustomFootprint(np.array([[0., 0.]]), np.pi / 2) env = GridWorld(map_filename=test_map_filename, footprint=footprint, sensor=Neighborhood(sensor_range=1, values=[0, 255]), start_state=[1, 3, 0], map_resolution=1, thicken_obstacles=False) map_data = np.array([[0, 0, 0, 0, 0], [0, 255, 255, 0, 0], [0, 255, 0, 255, 0], [0, 0, 255, 0, 0], [0, 0, 0, 0, 0]]).astype(np.uint8) occupancy_map = Costmap(data=map_data, resolution=1, origin=[0, 0]) percentage_completed = env.compare_maps(occupancy_map) assert percentage_completed == 0.625
def load_occupancy_map_data(group, filename): """ Reads in an occupancy map image from the maps folder, given the group name, and the image name. :param group str: the subfolder within maps to browse :param filename str: the image filename within the subfolder group. :return array(N,M)[uint8]: occupancy map data corresponded to the loaded image file """ return cv2.cvtColor( cv2.imread(os.path.join(get_maps_dir(), group, filename)), cv2.COLOR_BGR2GRAY).astype(np.uint8)
def test_frontier_based_exploration(): np.random.seed(3) _, _, _, _ = \ run_frontier_exploration(map_filename=os.path.join(get_maps_dir(), "test/vw_ground_truth_test.png"), params_filename=os.path.join(get_exploration_dir(), "params/params.yaml"), map_resolution=0.03, start_state=None, sensor_range=10.0, completion_percentage=0.97, max_exploration_iterations=2, render=False)
def test_creation(): test_map_filename = os.path.join(get_maps_dir(), "test/circle.png") footprint = CustomFootprint(np.array([[0., 0.]]), np.pi / 2) env = GridWorld(map_filename=test_map_filename, footprint=footprint, sensor=Neighborhood(sensor_range=1, values=[0, 255]), start_state=[1, 3, 0], map_resolution=1, thicken_obstacles=False) assert np.all(env.state == [0, 0, 0]) assert np.all(env.map.data == [[0, 0, 0, 0, 0], [0, 255, 255, 255, 0], [0, 255, 0, 255, 0], [0, 255, 255, 255, 0], [0, 0, 0, 0, 0]])
def create_gym_environment_from_parameters(env_mode): """ Creates a random gym environment and stores it in the "maps" directory :param env_mode str: indicates the type of environment in which we want to run the exploration algorithm :return map_filename string: map filename """ if env_mode == "MiniEnv": raise NotImplementedError # occupancy_grid = MiniEnv().get_state().costmap.get_data() elif env_mode == "RandomMiniEnv": raise NotImplementedError # occupancy_grid = RandomMiniEnv().get_state().costmap.get_data() elif env_mode == "AisleTurnEnv": raise NotImplementedError # occupancy_grid = AisleTurnEnv().get_state().costmap.get_data() elif env_mode == "RandomAisleTurnEnv": occupancy_grid = RandomAisleTurnEnv().get_state().costmap.get_data() elif env_mode == "ColoredCostmapRandomAisleTurnEnv": occupancy_grid = ColoredCostmapRandomAisleTurnEnv().get_state( ).costmap.get_data() elif env_mode == "ColoredEgoCostmapRandomAisleTurnEnv": occupancy_grid = ColoredEgoCostmapRandomAisleTurnEnv().get_state( ).costmap.get_data() elif env_mode == "ComplexCostmapRandomAisleTurnEnv": complexity = np.random.randint(2, 5) map_bundle = np.zeros([500, 500, complexity]) for m in range(complexity): map_bundle[:, :, m] = \ cv2.resize(ColoredCostmapRandomAisleTurnEnv().get_state().costmap.get_data(), (500, 500)) occupancy_grid = np.sum(map_bundle, axis=2) else: raise NameError( "Invalid gym enviroment type!\nPlease choose one of the following gym environments:" "\nMiniEnv \\ RandomMiniEnv \\ AisleTurnEnv \\ " "RandomAisleTurnEnv \\ ColoredCostmapRandomAisleTurnEnv \\ " "ColoredEgoCostmapRandomAisleTurnEnv \\ ComplexCostmapRandomAisleTurnEnv" ) occupancy_grid[occupancy_grid != 0] = 255 occupancy_grid = -1 * occupancy_grid + 255 path = os.path.join(get_maps_dir(), 'gym') if not os.path.exists(path): os.mkdir(path) map_filename = os.path.join(path, 'random_gym_map.png') cv2.imwrite(map_filename, occupancy_grid) return map_filename
def run_frontier_benchmarks(): """ Runs the frontier benchmarks """ # shared parameters num_instances = 20 sensor_range = 10.0 completion_percentage = 0.95 max_exploration_iterations = 75 params = "params/params.yaml" # individual parameters maps_to_run = ["brain/vw_ground_truth_full_edited.png"] consistency_start_states = [np.array([2.5, 5.5, -np.pi / 4])] results = {} for i, map_name in enumerate(maps_to_run): map_filename = os.path.join(get_maps_dir(), map_name) params_filename = os.path.join(get_exploration_dir(), params) completion_config = dict(map_filename=map_filename, params_filename=params_filename, start_state=None, sensor_range=sensor_range, completion_percentage=completion_percentage, render=False, render_wait_for_key=False, max_exploration_iterations=max_exploration_iterations) consistency_config = completion_config.copy() consistency_config['start_state'] = consistency_start_states[i] consistency_results = run_consistency(consistency_config, num_instances=num_instances) completion_results = run_completion(completion_config, num_instances=num_instances) print(map_name) print('consistency:') _ = [print(consistency_result) for consistency_result in consistency_results] print() print('completion:') _ = [print(completion_result) for completion_result in completion_results] print() results[map_name] = {'consistency': consistency_results, 'completion': completion_results} with open('benchmark_results.pkl', 'w') as f: pickle.dump(results, f)
def test_step_data(): """ Tests the step function on an environment, checking out of bounds, and collision ---maps/test/circle.png 0 0 0 0 0 0 255 255 255 0 0 255 0 255 0 0 255 255 255 0 0 0 0 0 0 ---end file """ test_map_filename = os.path.join(get_maps_dir(), "test/circle.png") footprint = CustomFootprint(np.array([[0., 0.]]), np.pi / 2) env = GridWorld(map_filename=test_map_filename, footprint=footprint, sensor=Neighborhood(sensor_range=1, values=[0, 255]), start_state=[1, 3, 0], map_resolution=1, thicken_obstacles=False) _, data = env.reset() # check occupied coords assert np.all( data[0] == [[-1, -1], [-1, 0], [-1, 1], [0, -1], [1, -1], [1, 1]]) # check free coords assert np.all(data[1] == [[0, 0], [0, 1], [1, 0]]) # todo in reality this sensor_range change test may belong in sensors.py footprint = CustomFootprint(np.array([[0., 0.]]), np.pi / 2) env = GridWorld(map_filename=test_map_filename, footprint=footprint, sensor=Neighborhood(sensor_range=2, values=[0, 255]), start_state=[1, 3, 0], map_resolution=1, thicken_obstacles=False) _, data = env.step([0, 1, 0]) # check occupied coords assert np.all(data[0] == [[-1, -1], [-1, 0], [-1, 1], [-1, 2], [0, -1], [1, -1], [1, 1], [2, -1]]) # check free coords assert np.all(data[1] == [[0, 0], [0, 1], [0, 2], [1, 0], [1, 2], [2, 0], [2, 1], [2, 2]])
def main(): """ Main Function """ np.random.seed(3) occupancy_map, iterations_taken, _ = \ run_frontiers_in_gym(map_filename=os.path.join(get_maps_dir(), "brain/vw_ground_truth_full_edited.png"), params_filename=os.path.join(get_exploration_dir(), "params/params.yaml"), map_resolution=0.03, start_state=np.array([15., 10., 0.]), sensor_range=10.0, max_exploration_iterations=25, render_interval=10) print("This is " + str(iterations_taken) + " iterations!") plt.figure() plt.imshow(occupancy_map.data.astype(np.uint8), cmap='gray') plt.show()
def main(): """ Main Function """ # big target 270 plans crash np.random.seed(3) _, percent_explored, iterations_taken, _ = \ run_frontier_exploration(map_filename=os.path.join(get_maps_dir(), "brain/vw_ground_truth_full_edited.png"), params_filename=os.path.join(get_exploration_dir(), "params/params.yaml"), map_resolution=0.03, start_state=None, sensor_range=10.0, completion_percentage=0.98, max_exploration_iterations=None, render_size_scale=2.0, render_interval=5) print("Map", "{:.2f}".format(percent_explored * 100), "\b% explored!", "This is " + str(iterations_taken) + " iterations!")
def test_reset(): """ Tests the reset function on an environment ---maps/test/circle.png 0 0 0 0 0 0 255 255 255 0 0 255 0 255 0 0 255 255 255 0 0 0 0 0 0 ---end file """ test_map_filename = os.path.join(get_maps_dir(), "test/circle.png") footprint = CustomFootprint(np.array([[0., 0.]]), np.pi / 2) env = GridWorld(map_filename=test_map_filename, footprint=footprint, sensor=Neighborhood(sensor_range=1, values=[0, 255]), start_state=[1, 3, 0], map_resolution=1, thicken_obstacles=False) env.state = np.array([70, 70, 0]) env.reset() assert np.all(env.state == [0, 0, 0])
def test_step_movement(): """ Tests the step function on an environment, checking out of bounds, and collision ---maps/test/circle.png 0 0 0 0 0 0 255 255 255 0 0 255 0 255 0 0 255 255 255 0 0 0 0 0 0 ---end file """ test_map_filename = os.path.join(get_maps_dir(), "test/circle.png") footprint = CustomFootprint(np.array([[0., 0.]]), np.pi / 2) env = GridWorld(map_filename=test_map_filename, footprint=footprint, sensor=Neighborhood(sensor_range=1, values=[0, 255]), start_state=[1, 3, 0], map_resolution=1, thicken_obstacles=False) # todo test is broken (not out of bounds, all obstacles) we arent testing out of bounds # test up out of bounds new_state, _ = env.step([0, 1, 0]) assert np.all(new_state == [0, 0, 0]) # test left out of bounds new_state, _ = env.step([-1, 0, 0]) assert np.all(new_state == [0, 0, 0]) # test down movement new_state, _ = env.step([0, -1, 0]) assert np.all(new_state == [0, -1, 0]) # test right obstacle new_state, _ = env.step([1, -1, 0]) assert np.all(new_state == [0, -1, 0]) new_state, _ = env.step([0, -2, 0]) assert np.all(new_state == [0, -2, 0]) # test down out of bounds new_state, _ = env.step([0, -3, 0]) assert np.all(new_state == [0, -2, 0]) # test right movement new_state, _ = env.step([1, -2, 0]) assert np.all(new_state == [1, -2, 0]) # test up obstacle new_state, _ = env.step([1, -1, 0]) assert np.all(new_state == [1, -2, 0]) new_state, _ = env.step([2, -2, 0]) assert np.all(new_state == [2, -2, 0]) # test right out of bounds new_state, _ = env.step([2, -2, 0]) assert np.all(new_state == [2, -2, 0]) # test up movement new_state, _ = env.step([2, -1, 0]) assert np.all(new_state == [2, -1, 0]) # test left obstacle new_state, _ = env.step([1, -1, 0]) assert np.all(new_state == [2, -1, 0]) new_state, _ = env.step([2, 0, 0]) assert np.all(new_state == [2, 0, 0]) # test left movement new_state, _ = env.step([1, 0, 0]) assert np.all(new_state == [1, 0, 0]) # test down obstacle new_state, _ = env.step([1, -1, 0]) assert np.all(new_state == [1, 0, 0]) new_state, _ = env.step([0, 0, 0]) assert np.all(new_state == [0, 0, 0])