def convert_brain_map_to_map(filename, debug=False):
"""
Given a filename of a brain colored png image of a costmap, convert it to a readable .png map for exploration
by removing the path, and recoloring the image.
:param filename str: location of the image to convert
:param debug bool: show result instead of saving
"""
image = cv2.imread(filename)
if image is None:
assert False and "File not found"
gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
# path is 150, blue_circle is 29, outside is 70, occupied is 226, free is 0
gray_image[gray_image == 150] = 255
gray_image[gray_image == 0] = 255
gray_image[gray_image == 29] = 255
gray_image[gray_image == 70] = 0
gray_image[gray_image == 226] = 0
if debug:
plt.imshow(gray_image, cmap='gray', interpolation='nearest')
plt.show()
else:
save_path = get_exploration_dir() + "/maps/" + os.path.basename(
filename[:-4]) + ".png"
cv2.imwrite(save_path, gray_image)
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_plan(debug=False):
occupancy_map_data = load_occupancy_map_data('test', 'frontier_plan_map.png')
occupancy_map = Costmap(data=occupancy_map_data, resolution=0.03, origin=np.array([-6.305, -6.305]))
pose = np.array([.8, 0, -0.51759265])
frontier_agent = create_frontier_agent_from_params(os.path.join(get_exploration_dir(), "params/params.yaml"))
frontier_agent.is_first_plan = False
plan = frontier_agent.plan(pose, occupancy_map)
if debug:
visualize(occupancy_map, pose, np.array([]), np.array([]), frontier_agent.get_footprint(), [], (1000, 1000), pose,
frontiers=frontier_agent.get_frontiers(compute=True, occupancy_map=occupancy_map))
assert plan.shape[0] > 2
def debug_frontier_agent():
data = np.load('debug3.npy')
state = data[0]
occupancy_map = data[1]
frontier_agent = create_frontier_agent_from_params(get_exploration_dir() + '/params/params.yaml')
path = frontier_agent.plan(state, occupancy_map)
visualization_map = occupancy_map.copy()
visualization_map.data = np.dstack((occupancy_map.data, occupancy_map.data, occupancy_map.data))
draw_footprint_path(frontier_agent.get_footprint(), path, visualization_map, [0, 255, 0])
plt.imshow(visualization_map.data, interpolation='nearest')
plt.show()
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 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
"""
np.random.seed(3)
_, percent_explored, iterations_taken, _ = \
run_frontier_exploration(map_filename=create_gym_environment_from_parameters("RandomAisleTurnEnv"),
params_filename=os.path.join(get_exploration_dir(), "params/params.yaml"),
map_resolution=0.03,
start_state=None,
sensor_range=10.0,
completion_percentage=10,
max_exploration_iterations=None,
render_mode='gym',
render_size_scale=50.0,
render_interval=5)
print("Map", "{:.2f}".format(percent_explored * 100), "\b% explored!",
"This is " + str(iterations_taken) + " iterations!")
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 save(self, filename):
"""
saves the map to file relative to the exploration folder
:param filename str: relative filepath of which to save
"""
cv2.imwrite(get_exploration_dir() + "/" + filename, self.data)