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