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 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_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_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 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 run_frontier_exploration(map_filename,
params_filename,
start_state,
sensor_range,
map_resolution,
completion_percentage,
render=True,
render_mode='exp',
render_interval=1,
render_size_scale=1.7,
completion_check_interval=1,
render_wait_for_key=True,
max_exploration_iterations=None):
"""
Interface for running frontier exploration on the grid world environment that is initialized via map_filename.. etc
:param map_filename str: path of the map to load into the grid world environment, needs to be a uint8 png with
values 127 for unexplored, 255 for free, 0 for occupied.
:param params_filename str: path of the params file for setting up the frontier agent etc.
See exploration/params/ for examples
:param start_state array(3)[float]: starting state of the robot in the map (in meters) [x, y, theta], if None the starting
state is random
:param sensor_range float: range of the sensor (lidar) in meters
:param map_resolution float: resolution of the map desired
:param completion_percentage float: decimal of the completion percentage desired i.e (.97), how much of the ground
truth environment to explore, note that 1.0 is not always reachable because of
footprint.
:param render bool: whether or not to visualize
:param render_mode str: specify rendering function (bc_exploration or bc_gym_planning_env)
:param render_interval int: visualize every render_interval iterations
:param render_size_scale Tuple(int): (h, w), the size of the render window in pixels
:param completion_check_interval int: check for exploration completion every completion_check_interval iterations
:param render_wait_for_key bool: if render is enabled, if render_wait_for_key is True then the exploration algorithm
will wait for key press to start exploration. Timing is not affected.
:param max_exploration_iterations int: number of exploration cycles to run before exiting
:return Costmap: occupancy_map, final map from exploration), percentage explored, time taken to explore
"""
# some parameters
frontier_agent = create_frontier_agent_from_params(params_filename)
footprint = frontier_agent.get_footprint()
# pick a sensor
sensor = Lidar(sensor_range=sensor_range,
angular_range=250 * np.pi / 180,
angular_resolution=1.0 * np.pi / 180,
map_resolution=map_resolution)
# setup grid world environment
env = GridWorld(map_filename=map_filename,
map_resolution=map_resolution,
sensor=sensor,
footprint=footprint,
start_state=start_state)
# setup corresponding gym environment
env_instance = RandomAisleTurnEnv()
render_size = (np.array(env.get_map_shape()[::-1]) *
render_size_scale).astype(np.int)
# setup log-odds mapper, we assume the measurements are very accurate,
# thus one scan should be enough to fill the map
padding = 1.
map_shape = np.array(env.get_map_shape()) + int(
2. * padding // map_resolution)
initial_map = Costmap(
data=Costmap.UNEXPLORED * np.ones(map_shape, dtype=np.uint8),
resolution=env.get_map_resolution(),
origin=[-padding - env.start_state[0], -padding - env.start_state[1]])
clearing_footprint_points = footprint.get_clearing_points(map_resolution)
clearing_footprint_coords = xy_to_rc(clearing_footprint_points,
initial_map).astype(np.int)
initial_map.data[clearing_footprint_coords[:, 0],
clearing_footprint_coords[:, 1]] = Costmap.FREE
mapper = LogOddsMapper(initial_map=initial_map,
sensor_range=sensor.get_sensor_range(),
measurement_certainty=0.8,
max_log_odd=8,
min_log_odd=-8,
threshold_occupied=.5,
threshold_free=.5)
# reset the environment to the start state, map the first observations
pose, [scan_angles, scan_ranges] = env.reset()
occupancy_map = mapper.update(state=pose,
scan_angles=scan_angles,
scan_ranges=scan_ranges)
if render:
if render_mode == 'exp':
visualize(occupancy_map=occupancy_map,
state=pose,
footprint=footprint,
start_state=start_state,
scan_angles=scan_angles,
scan_ranges=scan_ranges,
path=[],
render_size=render_size,
frontiers=[],
wait_key=0 if render_wait_for_key else 1)
elif render_mode == 'gym':
visualize_in_gym(gym_env=env_instance,
exp_map=occupancy_map,
path=[],
pose=pose)
else:
raise NameError(
"Invalid rendering method.\nPlease choose one of \"exp\" or \"gym\" methods."
)
iteration = 0
is_last_plan = False
was_successful = True
percentage_explored = 0.0
while True:
if iteration % completion_check_interval == 0:
percentage_explored = env.compare_maps(occupancy_map)
if percentage_explored >= completion_percentage:
is_last_plan = True
if max_exploration_iterations is not None and iteration > max_exploration_iterations:
was_successful = False
is_last_plan = True
# using the current map, make an action plan
path = frontier_agent.plan(state=pose,
occupancy_map=occupancy_map,
is_last_plan=is_last_plan)
# if we get empty lists for policy/path, that means that the agent was
# not able to find a path/frontier to plan to.
if not path.shape[0]:
print(
"No more frontiers! Either the map is 100% explored, or bad start state, or there is a bug!"
)
break
# if we have a policy, follow it until the end. update the map sparsely (speeds it up)
for j, desired_state in enumerate(path):
if footprint.check_for_collision(desired_state,
occupancy_map,
unexplored_is_occupied=True):
footprint_coords = footprint.get_ego_points(
desired_state[2], map_resolution) + desired_state[:2]
footprint_coords = xy_to_rc(footprint_coords,
occupancy_map).astype(np.int)
footprint_coords = footprint_coords[which_coords_in_bounds(
footprint_coords, occupancy_map.get_shape())]
occupancy_map.data[footprint_coords[:, 0],
footprint_coords[:, 1]] = Costmap.FREE
pose, [scan_angles, scan_ranges] = env.step(desired_state)
occupancy_map = mapper.update(state=pose,
scan_angles=scan_angles,
scan_ranges=scan_ranges)
# put the current laserscan on the map before planning
occupied_coords = scan_to_points(scan_angles + pose[2],
scan_ranges) + pose[:2]
occupied_coords = xy_to_rc(occupied_coords,
occupancy_map).astype(np.int)
occupied_coords = occupied_coords[which_coords_in_bounds(
occupied_coords, occupancy_map.get_shape())]
occupancy_map.data[occupied_coords[:, 0],
occupied_coords[:, 1]] = Costmap.OCCUPIED
# shows a live visualization of the exploration process if render is set to true
if render and j % render_interval == 0:
if render_mode == 'exp':
visualize(occupancy_map=occupancy_map,
state=pose,
footprint=footprint,
start_state=start_state,
scan_angles=scan_angles,
scan_ranges=scan_ranges,
path=path,
render_size=render_size,
frontiers=frontier_agent.get_frontiers(
compute=True, occupancy_map=occupancy_map),
wait_key=1,
path_idx=j)
elif render_mode == 'gym':
visualize_in_gym(gym_env=env_instance,
exp_map=occupancy_map,
path=path,
pose=pose)
else:
raise NameError(
"Invalid rendering method.\nPlease choose one of \"exp\" or \"gym\" methods."
)
if is_last_plan:
break
iteration += 1
if render:
cv2.waitKey(0)
return occupancy_map, percentage_explored, iteration, was_successful
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])