def measure(self, state, debug=False):
"""
given the current robot pose, return the lidar data
:param state array(3)[float]: [x, y, theta (radians)] pose of the robot
:param debug bool: show debug plot?
:return Tuple[array(N)[float], array(N)[float]]: [lidar angles radians, lidar ranges meters] if ray did not hit obstacle, value is np.nan
"""
assert self._map is not None \
and "Please set the map using set_map() before calling measure, this will initialize lidar as well."
pose = state.copy()
pose_px = xy_to_rc(pose, self._map)
ranges = np.zeros_like(self._ray_angles)
for i, ego_ray in enumerate(self._ego_rays):
rotation_matrix = get_rotation_matrix_2d(-state[2])
rotated_ray = np.rint(ego_ray.dot(rotation_matrix))
ray = (pose_px[:2] + rotated_ray).astype(np.int)
ray = ray[which_coords_in_bounds(ray, self._map.get_shape())]
occupied_ind = np.argwhere(
self._map.data[ray[:, 0], ray[:, 1]] == Costmap.OCCUPIED)
if occupied_ind.shape[0]:
ranges[i] = np.linalg.norm(pose_px[:2] - ray[int(
occupied_ind[0])]) * self._map.resolution
else:
ranges[i] = np.nan
if debug:
points = scan_to_points(self._ray_angles + state[2],
ranges) + pose_px[:2]
plt.plot(points[:, 0], points[:, 1])
plt.show()
return self._ray_angles, ranges
def draw_frontiers(visualization_map, frontiers, color):
"""
Draw the frontiers onto the map.
:param visualization_map Costmap: costmap to draw on
:param frontiers List[np.ndarray]: of frontiers, each frontier is a set of coordinates
:param color Union[int, array(3)[uint8]]: depending on the shape of visualization_map.data,
a valid value for the color to draw the path
"""
for frontier in frontiers:
frontier_px = xy_to_rc(frontier, visualization_map).astype(np.int)
frontier_px = frontier_px[which_coords_in_bounds(frontier_px, visualization_map.get_shape())]
cv2.drawContours(visualization_map.data, [frontier_px[:, ::-1]], 0, color, 2)
def get_downscaled(self, desired_resolution):
"""
Return a downscaled version of the costmap, makes sure to preserve obstacles and free space that is marked
:param desired_resolution float: resolution in meters of the downscaled costmap
:return Costmap: object
"""
assert self.resolution <= desired_resolution
scale_factor = self.resolution / desired_resolution
scaled_shape = np.rint(np.array(self.data.shape) *
scale_factor).astype(np.int)
scaled_data = np.zeros(scaled_shape)
scaled_occupied_coords = np.rint(
np.argwhere(self.data == Costmap.OCCUPIED) * scale_factor).astype(
np.int)
scaled_unexplored_coords = np.rint(
np.argwhere(self.data == Costmap.UNEXPLORED) *
scale_factor).astype(np.int)
scaled_free_coords = np.rint(
np.argwhere(self.data == Costmap.FREE) * scale_factor).astype(
np.int)
scaled_occupied_coords = scaled_occupied_coords[which_coords_in_bounds(
scaled_occupied_coords, scaled_shape)]
scaled_unexplored_coords = scaled_unexplored_coords[
which_coords_in_bounds(scaled_unexplored_coords, scaled_shape)]
scaled_free_coords = scaled_free_coords[which_coords_in_bounds(
scaled_free_coords, scaled_shape)]
# order is important here, we want to make sure to keep the obstacles
scaled_data[scaled_free_coords[:, 0],
scaled_free_coords[:, 1]] = Costmap.FREE
scaled_data[scaled_unexplored_coords[:, 0],
scaled_unexplored_coords[:, 1]] = Costmap.UNEXPLORED
scaled_data[scaled_occupied_coords[:, 0],
scaled_occupied_coords[:, 1]] = Costmap.OCCUPIED
return Costmap(data=scaled_data.astype(np.uint8),
resolution=desired_resolution,
origin=self.origin)
def draw_scan_ranges(visualization_map, state, scan_angles, scan_ranges, color):
"""
Draw the occupied points of a lidar scan onto the map
:param visualization_map Costmap: costmap to draw on
:param state array(3)[float]: pose of the robot [x, y, theta]
:param scan_angles array(N)[float]: angles of the lidar scan
:param scan_ranges array(N)[float]: ranges of the lidar scan
:param color Union[int, array(3)[uint8]]: depending on the shape of visualization_map.data,
a valid value for the color to draw the path
"""
occupied_coords = scan_to_points(scan_angles + state[2], scan_ranges) + state[:2]
occupied_coords = xy_to_rc(occupied_coords, visualization_map).astype(np.int)
occupied_coords = occupied_coords[which_coords_in_bounds(occupied_coords, visualization_map.get_shape())]
visualization_map.data[occupied_coords[:, 0], occupied_coords[:, 1]] = color
def measure(self, state, debug=False):
"""
Returns the occupied free ego coordinates at the current state
:param state array(3)[float]: state of the robot
:param debug bool: show plots?
:return Union[array(N,2)[int], array(N,2)[int]]: ego occupied, ego free coordinates
"""
if self._map is None:
assert False and "Sensor's map is not currently set, please initialize with set_map()"
state_rays = self._compute_state_rays(state)
# remove ray points that are out of bounds
rays = []
for state_ray in state_rays:
in_bound_point_inds = which_coords_in_bounds(
state_ray, map_shape=self._map.data.shape)
rays.append(state_ray[in_bound_point_inds])
if debug:
vis_map = self._map.copy()
for ray in rays:
vis_map[ray[:, 0], ray[:, 1]] = 75
plt.imshow(vis_map)
plt.show()
occupied = []
free = []
for ray in rays:
ind = -1
for ind, point in enumerate(ray):
if self._map[int(point[0]), int(point[1])] == 0:
occupied.append(point)
break
if ind != -1:
free.extend(ray[:ind, :])
else:
free.extend(ray)
return [
np.array(occupied) -
state[:2].astype(np.int) if len(occupied) else np.empty((0, )),
np.array(free) -
state[:2].astype(np.int) if len(free) else np.empty((0, ))
]
def test_extract_frontiers(debug=False):
occupancy_map_data = load_occupancy_map_data('test', 'vw_partly_explored_test.png')
occupancy_map = Costmap(data=occupancy_map_data, resolution=0.03, origin=np.array([0, 0]))
frontiers = extract_frontiers(occupancy_map=occupancy_map, approx=False,
kernel=cv2.getStructuringElement(cv2.MORPH_CROSS, (3, 3)))
frontier_sizes = np.array([frontier.shape[0] if len(frontier.shape) > 1 else 1 for frontier in frontiers])
significant_frontiers = [frontier for i, frontier in enumerate(frontiers) if frontier_sizes[i] > 70]
assert len(significant_frontiers) == 4
if debug:
frontier_map = np.repeat([occupancy_map.data], repeats=3, axis=0).transpose((1, 2, 0))
for frontier in significant_frontiers:
frontier_px = xy_to_rc(frontier, occupancy_map).astype(np.int)
frontier_px = frontier_px[which_coords_in_bounds(frontier_px, occupancy_map.get_shape())]
frontier_map[frontier_px[:, 0], frontier_px[:, 1]] = [255, 0, 0]
plt.imshow(frontier_map, cmap='gray', interpolation='nearest')
plt.show()
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 plan(self, state, occupancy_map, debug=False, is_last_plan=False):
"""
Given the robot position (state) and the occupancy_map, calculate the path/policy to the "best" frontier to
explore.
:param state array(3)[float]: robot pose in coordinate space [x, y, theta (radians)]
:param occupancy_map Costmap: object corresponding to the current map
:param debug bool: show debug windows?
:param is_last_plan bool: whether we are done exploring and would like to replan to the starting state
:return array(N, 3)[float]: array of coordinates of the path
"""
self._plan_iteration += 1
# precompute items for collision checking
if self._footprint_masks is None or self._footprint_mask_radius is None or self._footprint_outline_coords is None:
self._footprint_masks = self._footprint.get_footprint_masks(
occupancy_map.resolution, angles=self._planning_angles)
self._footprint_outline_coords = self._footprint.get_outline_coords(
occupancy_map.resolution, angles=self._planning_angles)
self._footprint_mask_radius = self._footprint.get_mask_radius(
occupancy_map.resolution)
if self._planning_resolution is not None:
assert occupancy_map.resolution <= self._planning_resolution
planning_scale = int(
np.round(self._planning_resolution / occupancy_map.resolution))
else:
planning_scale = 1
# this kernel works better than a simple 3,3 ones
kernel = cv2.getStructuringElement(cv2.MORPH_CROSS, (3, 3))
# get the frontiers on the current map
exploration_map = occupancy_map.copy()
frontiers = extract_frontiers(occupancy_map=exploration_map,
kernel=kernel)
frontiers = self._filter_frontiers(frontiers, exploration_map)
self._frontiers = frontiers
# rank the frontiers based off the mode
frontier_ranks = self._compute_frontier_ranks(state, frontiers)
# todo clean frontier blacklist every N iterations.
if not len(frontiers):
self._frontier_blacklist = [[]]
# if theres no frontiers, there's nothing to plan to, maybe this means we are done
if not len(frontiers):
return np.empty((0, 3))
# clean the occupancy map, eat up small unexplored pixels we dont want to consider a collision
exploration_map = cleanup_map_for_planning(
occupancy_map=exploration_map, kernel=kernel)
# define our planning function, we use a partial here because most of the inputs are the same
oriented_astar_partial = partial(
oriented_astar,
start=state,
occupancy_map=exploration_map,
footprint_masks=self._footprint_masks,
outline_coords=self._footprint_outline_coords,
obstacle_values=[Costmap.OCCUPIED, Costmap.UNEXPLORED],
planning_scale=planning_scale,
epsilon=self._planning_epsilon)
num_replans = 0
frontier_idx = 0
plan_successful = False
failsafe_path = None
tried_blacklist_frontiers = False
planning_delta = self._footprint_mask_radius
while not plan_successful and frontier_ranks.shape[0]:
if frontier_idx > frontier_ranks.shape[0] - 1:
frontier_idx = 0
num_replans += 1
planning_delta *= self._planning_delta_scale
if num_replans < self._max_replans - 1:
print(
"Not able to plan to frontiers. Increasing planning delta and trying again!"
)
else:
if failsafe_path is None and not tried_blacklist_frontiers:
print(
"Not able to plan to any frontiers, resetting blacklist"
)
tried_blacklist_frontiers = True
self._frontier_blacklist = [[]]
continue
else:
print(
"Not able to plan to any frontiers, choosing failsafe path"
)
break
best_frontier = frontiers[frontier_ranks[frontier_idx]]
assert len(best_frontier.shape) == 2
if best_frontier.shape[0] == 0:
continue
best_frontier_rc_list = xy_to_rc(
best_frontier, exploration_map).astype(np.int).tolist()
if best_frontier_rc_list in self._frontier_blacklist:
frontier_ranks = np.delete(frontier_ranks, frontier_idx)
continue
# based off the mode, we will compute a path to the closest point or the middle of the frontier
if self._mode.split('-')[1] == 'closest':
# navigate to the closest point on the best frontier
desired_coord_ranks = np.argsort(
np.sum((state[:2] - best_frontier)**2, axis=1))
elif self._mode.split('-')[1] == 'middle':
# navigate to the middle of the best frontier
frontier_mean = np.mean(best_frontier, axis=0)
desired_coord_ranks = np.argsort(
np.sqrt(np.sum((frontier_mean - best_frontier)**2,
axis=1)))
else:
desired_coord_ranks = None
assert False and "Mode not supported"
goal = None
# todo try more angles
start_frontier_vector = best_frontier[
desired_coord_ranks[0]] - state[:2]
angle_to_frontier = wrap_angles(
np.arctan2(start_frontier_vector[0], start_frontier_vector[1]))
# find a point near the desired coord where our footprint fits
for _, ind in enumerate(desired_coord_ranks):
candidate_state = np.concatenate(
(np.array(best_frontier[ind]).squeeze(),
[angle_to_frontier]))
if not self._footprint.check_for_collision(
state=candidate_state, occupancy_map=exploration_map):
goal = candidate_state
break
# todo need to integrate goal region at this step. maybe raytrace some poses and try those?
# if we can't find a pose on the frontier we can plan to, add this frontier to blacklist
if goal is None:
self._frontier_blacklist.append(best_frontier_rc_list)
frontier_ranks = np.delete(frontier_ranks, frontier_idx)
continue
if debug:
frontier_map = np.repeat([exploration_map.data],
repeats=3,
axis=0).transpose((1, 2, 0)).copy()
best_frontier = frontiers[frontier_ranks[frontier_idx]]
best_frontier_vis = xy_to_rc(best_frontier,
exploration_map).astype(np.int)
best_frontier_vis = best_frontier_vis[which_coords_in_bounds(
best_frontier_vis, exploration_map.get_shape())]
frontier_map[best_frontier_vis[:, 0],
best_frontier_vis[:, 1]] = [255, 0, 0]
for _, ind in enumerate(desired_coord_ranks):
candidate_state = np.concatenate(
(np.array(best_frontier[ind]).squeeze(),
[angle_to_frontier]))
if not self._footprint.check_for_collision(
state=candidate_state,
occupancy_map=exploration_map):
goal = candidate_state
break
goal_vis = xy_to_rc(goal, exploration_map)
frontier_map[int(goal_vis[0]), int(goal_vis[1])] = [0, 255, 0]
cv2.circle(frontier_map,
tuple(
xy_to_rc(state[:2],
exploration_map)[::-1].astype(int)),
self._footprint.get_radius_in_pixels(
exploration_map.resolution), (255, 0, 255),
thickness=-1)
plt.imshow(frontier_map, interpolation='nearest')
plt.show()
plan_successful, path = oriented_astar_partial(
goal=goal, delta=planning_delta)
if debug:
oriented_astar_partial = partial(
oriented_astar,
start=state,
occupancy_map=exploration_map,
footprint_masks=self._footprint_masks,
outline_coords=self._footprint_outline_coords,
obstacle_values=[Costmap.OCCUPIED, Costmap.UNEXPLORED],
planning_scale=planning_scale,
epsilon=self._planning_epsilon)
plan_successful, path = oriented_astar_partial(
goal=goal, delta=planning_delta)
path_map = np.array(exploration_map.data)
path_vis = xy_to_rc(path,
exploration_map)[:, :2].astype(np.int)
best_frontier = frontiers[frontier_ranks[frontier_idx]]
best_frontier_vis = xy_to_rc(best_frontier,
exploration_map).astype(np.int)
best_frontier_vis = best_frontier_vis[which_coords_in_bounds(
best_frontier_vis, exploration_map.get_shape())]
path_map[best_frontier_vis[:, 0], best_frontier_vis[:, 1]] = 75
path_map[path_vis[:, 0], path_vis[:, 1]] = 175
plt.imshow(path_map, cmap='gray', interpolation='nearest')
plt.show()
if plan_successful and path.shape[0] <= 1:
plan_successful = False
self._frontier_blacklist.append(best_frontier_rc_list)
frontier_ranks = np.delete(frontier_ranks, frontier_idx)
continue
for i, pose in enumerate(path):
if self._footprint.check_for_collision(pose, exploration_map):
path = path[:i, :]
break
if failsafe_path is None:
failsafe_path = path.copy()
if plan_successful:
return path
frontier_idx += 1
return failsafe_path if failsafe_path is not None else np.array(
[state])
def extract_frontiers(occupancy_map,
approx=True,
approx_iters=2,
kernel=cv2.getStructuringElement(cv2.MORPH_CROSS,
(3, 3)),
debug=False):
"""
Given a map of free/occupied/unexplored pixels, identify the frontiers.
This method is a quicker method than the brute force approach,
and scales pretty well with large maps. Using cv2.dilate for 1 pixel on the unexplored space
and comparing with the free space, we can isolate the frontiers.
:param occupancy_map Costmap: object corresponding to the map to extract frontiers from
:param approx bool: does an approximation of the map before extracting frontiers. (dilate erode, get rid of single
pixel unexplored areas creating a large number fo frontiers)
:param approx_iters int: number of iterations for the dilate erode
:param kernel array(N, N)[float]: the kernel of which to use to extract frontiers / approx map
:param debug bool: show debug windows?
:return List[array(N, 2][float]: list of frontiers, each frontier is a set of coordinates
"""
# todo regional frontiers
# extract coordinates of occupied, unexplored, and free coordinates
occupied_coords = np.argwhere(
occupancy_map.data.astype(np.uint8) == Costmap.OCCUPIED)
unexplored_coords = np.argwhere(
occupancy_map.data.astype(np.uint8) == Costmap.UNEXPLORED)
free_coords = np.argwhere(
occupancy_map.data.astype(np.uint8) == Costmap.FREE)
if free_coords.shape[0] == 0 or unexplored_coords.shape[0] == 0:
return []
# create a binary mask of unexplored pixels, letting unexplored pixels = 1
unexplored_mask = np.zeros_like(occupancy_map.data)
unexplored_mask[unexplored_coords[:, 0], unexplored_coords[:, 1]] = 1
# dilate using a 3x3 kernel, effectively increasing
# the size of the unexplored space by one pixel in all directions
dilated_unexplored_mask = cv2.dilate(unexplored_mask, kernel=kernel)
dilated_unexplored_mask[occupied_coords[:, 0], occupied_coords[:, 1]] = 1
# create a binary mask of the free pixels
free_mask = np.zeros_like(occupancy_map.data)
free_mask[free_coords[:, 0], free_coords[:, 1]] = 1
# can isolate the frontiers using the difference between the masks,
# and looking for contours
frontier_mask = ((1 - dilated_unexplored_mask) - free_mask)
if approx:
frontier_mask = cv2.dilate(frontier_mask,
kernel=kernel,
iterations=approx_iters)
frontier_mask = cv2.erode(frontier_mask,
kernel=kernel,
iterations=approx_iters)
# this indexing will work with opencv 2.x 3.x and 4.x
frontiers_xy_px = cv2.findContours(frontier_mask, cv2.RETR_LIST,
cv2.CHAIN_APPROX_NONE)[-2:][0]
frontiers = [
rc_to_xy(np.array(frontier).squeeze(1)[:, ::-1], occupancy_map)
for frontier in frontiers_xy_px
]
if debug:
frontier_map = np.repeat([occupancy_map.data], repeats=3,
axis=0).transpose((1, 2, 0))
# frontiers = [frontiers[rank] for i, rank in enumerate(frontier_ranks)
# if i < self.num_frontiers_considered]
for frontier in frontiers:
# if frontier.astype(np.int).tolist() in self.frontier_blacklist:
# continue
frontier_px = xy_to_rc(frontier, occupancy_map).astype(np.int)
frontier_px = frontier_px[which_coords_in_bounds(
frontier_px, occupancy_map.get_shape())]
frontier_map[frontier_px[:, 0], frontier_px[:, 1]] = [255, 0, 0]
plt.imshow(frontier_map, cmap='gray', interpolation='nearest')
plt.show()
return frontiers
def check_for_collision(self,
state,
occupancy_map,
unexplored_is_occupied=False,
use_python=False,
debug=False):
"""
using the state and the map, check for a collision on the map.
:param state array(3)[float]: state of the robot [x, y, theta]
:param occupancy_map Costmap: object to check the footprint against
:param unexplored_is_occupied bool: whether to treat unexplored on the map as occupied
:param use_python bool: whether to use python collision checking or c++
:param debug bool: show debug plots?
:return bool: True for collision
"""
# check if we have computed a mask for this resolution, if not compute it
if occupancy_map.resolution not in list(
self._rotated_masks_set.keys()):
self._add_new_masks(occupancy_map.resolution)
# convert state to pixel coordinates
state_px = xy_to_rc(pose=state, occupancy_map=occupancy_map)
position = np.array(state_px[:2]).astype(np.int)
if debug:
map_vis = occupancy_map.copy()
map_vis.data = np.repeat([occupancy_map.data], repeats=3,
axis=0).transpose((1, 2, 0)).copy()
self.draw(rc_to_xy(state_px, occupancy_map), map_vis,
[255, 10, 10])
occupied_inds = np.argwhere(occupancy_map.data == Costmap.OCCUPIED)
map_vis.data[occupied_inds[:, 0], occupied_inds[:, 1]] = [0, 0, 0]
plt.imshow(map_vis.data, interpolation='nearest')
plt.show()
# get the mask radius that was saved for this resolution
mask_radius = self._mask_radius_set[occupancy_map.resolution]
# compute the closest angle to the current angle in the state, and get that rotated footprint
closest_angle_ind = np.argmin(np.abs(state_px[2] - self._mask_angles))
footprint_mask = self._rotated_masks_set[
occupancy_map.resolution][closest_angle_ind]
outline_coords = self._rotated_outline_coords_set[
occupancy_map.resolution][closest_angle_ind]
if not use_python:
obstacle_values = [
Costmap.OCCUPIED, Costmap.UNEXPLORED
] if unexplored_is_occupied else [Costmap.OCCUPIED]
is_colliding = check_for_collision(state,
occupancy_map,
footprint_mask=footprint_mask,
outline_coords=outline_coords,
obstacle_values=obstacle_values)
else:
# part of robot off the edge of map, it is a collision, because we assume map is bounded
if not np.all(
which_coords_in_bounds(
coords=(outline_coords + state_px[:2]).astype(np.int),
map_shape=occupancy_map.get_shape())):
return True
# get a small subsection around the state in the map (as big as our mask),
# and do a masking check with the footprint to see if there is a collision.
# if part of our subsection is off the map, we need to clip the size
# to reflect this, in both the subsection and the mask
min_range = [position[0] - mask_radius, position[1] - mask_radius]
max_range = [
position[0] + mask_radius + 1, position[1] + mask_radius + 1
]
clipped_min_range, clipped_max_range = clip_range(
min_range, max_range, occupancy_map.data.shape)
min_range_delta = clipped_min_range - np.array(min_range)
max_range_delta = clipped_max_range - np.array(max_range)
ego_map = occupancy_map.data[
clipped_min_range[0]:clipped_max_range[0],
clipped_min_range[1]:clipped_max_range[1]].copy()
# todo might be - max_range_delta
footprint_mask = footprint_mask[
min_range_delta[0]:footprint_mask.shape[1] +
max_range_delta[0],
min_range_delta[1]:footprint_mask.shape[1] +
max_range_delta[1]]
# treat unexplored space as obstacle
if unexplored_is_occupied:
ego_map[ego_map == Costmap.UNEXPLORED] = Costmap.OCCUPIED
is_colliding = np.any(footprint_mask == ego_map)
if debug:
plt.imshow(footprint_mask - 0.1 * ego_map)
plt.show()
return is_colliding
def run_frontiers_in_gym(map_filename,
params_filename,
start_state,
sensor_range,
map_resolution,
render=True,
render_interval=10,
max_exploration_iterations=None):
"""
Interface for running frontier exploration in the bc gym 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 bc_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 render bool: whether or not to visualize
:param render_interval int: visualize every render_interval iterations
: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()
# setup costmap
map = exp_CostMap(data=cv2.imread(map_filename),
resolution=map_resolution,
origin=np.array([0., 0.]))
map_data = to_brain_costmap(map).get_data()
costmap = gym_CostMap(data=map_data[:, :, 0],
resolution=map_resolution,
origin=np.array([0., 0.]))
padding = 0.
map_shape = np.array(map_data.shape[:2]) + int(
2. * padding // map_resolution)
exp_initial_map = exp_CostMap(data=exp_CostMap.UNEXPLORED *
np.ones(map_shape, dtype=np.uint8),
resolution=map_resolution,
origin=[-padding - 0., -padding - 0.])
footprint_coords = footprint.get_ego_points(
start_state[2], map_resolution) + start_state[:2]
footprint_coords = xy_to_rc(footprint_coords,
exp_initial_map).astype(np.int)
footprint_coords = footprint_coords[which_coords_in_bounds(
footprint_coords, exp_initial_map.get_shape())]
exp_initial_map.data[footprint_coords[:, 0],
footprint_coords[:, 1]] = exp_CostMap.FREE
gym_initial_map = to_brain_costmap(exp_initial_map)
# pick a sensor
sensor = VirtualLidar(range_max=sensor_range,
range_angular=250 * np.pi / 180,
costmap=costmap,
resolution_angular=1.0 * np.pi / 180)
# define log-odds mapper
mapper = LogOddsMapper(initial_map=exp_initial_map,
sensor_range=sensor.get_range_max(),
measurement_certainty=0.8,
max_log_odd=8,
min_log_odd=-8,
threshold_occupied=.5,
threshold_free=.5)
# define planning environment parameters
env_params = EnvParams(
iteration_timeout=1200,
pose_delay=0,
control_delay=0,
state_delay=0,
goal_spat_dist=1.0,
goal_ang_dist=np.pi / 2,
dt=0.05, # 20 Hz
path_limiter_max_dist=5.0,
sensor=sensor)
env = PlanEnv(costmap=gym_initial_map,
path=start_state[None, :],
params=env_params)
obs = env.reset()
if render:
env.render()
pose = obs.pose
scan_angles, scan_ranges = obs.get_lidar_scan()
occupancy_map = mapper.update(state=pose,
scan_angles=scan_angles,
scan_ranges=scan_ranges)
iteration = 0
is_last_plan = False
was_successful = True
j = 0
while 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)
env = PlanEnv(costmap=to_brain_costmap(occupancy_map),
path=path,
params=env_params)
path = list(path)
done = False
while len(path) != 0 or not done:
desired_pose = path.pop(0)
if footprint.check_for_collision(desired_pose,
occupancy_map,
unexplored_is_occupied=True):
footprint_coords = footprint.get_ego_points(
desired_pose[2], map_resolution) + desired_pose[: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]] = exp_CostMap.FREE
obs, _, done, _ = env.simple_step(desired_pose)
pose = obs.pose
scan_angles, scan_ranges = obs.get_lidar_scan()
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, _ = sensor.get_scan_points(pose)
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]] = exp_CostMap.OCCUPIED
if render and j % render_interval == 0:
state = env.get_state()
state.costmap = to_brain_costmap(occupancy_map)
env.set_state(state)
env.render()
j += 1
if is_last_plan:
break
iteration += 1
if render:
cv2.waitKey(0)
return occupancy_map, iteration, was_successful
def _scan_to_occupied_free_coords(self,
state,
angles,
ranges,
debug=False):
"""
converts laser scan to occupied and free coordinates for mapping
:param state array(3)[float]: current state of the robot
:param angles array(N)[float]: lidar angles of the robot
:param ranges array(N)[float]: lidar ranges corresponding to the angles
:param debug bool: show debug?
:return Tuple[array(N,2)[int], array(N,2)[int]]: occupied coordinates, and free coordinates (in row column)
"""
new_angles = wrap_angles(angles.copy() + state[2])
state_px = xy_to_rc(state, self._map)
position_px = state_px[:2].astype(np.int)
with np.errstate(invalid='ignore'):
free_inds = np.logical_or(np.isnan(ranges),
ranges >= self.sensor_range)
occ_inds = np.logical_not(free_inds)
ranges = ranges.copy()
ranges[free_inds] = self.sensor_range
occupied_coords = scan_to_points(new_angles[occ_inds],
ranges[occ_inds]) + state[:2]
occupied_coords = xy_to_rc(occupied_coords, self._map).astype(np.int)
free_endcoords = scan_to_points(new_angles[free_inds],
ranges[free_inds]) + state[:2]
free_endcoords = xy_to_rc(free_endcoords, self._map).astype(np.int)
if debug:
occupied_coords_vis = occupied_coords[which_coords_in_bounds(
occupied_coords, self._map.get_shape())]
free_coords_vis = free_endcoords[which_coords_in_bounds(
free_endcoords, self._map.get_shape())]
map_vis = np.repeat([self._map.data], repeats=3, axis=0).transpose(
(1, 2, 0)).copy()
map_vis[occupied_coords_vis[:, 0],
occupied_coords_vis[:, 1]] = [255, 0, 0]
map_vis[free_coords_vis[:, 0], free_coords_vis[:, 1]] = [0, 255, 0]
plt.imshow(map_vis)
plt.show()
free_coords = np.array([position_px])
for i in range(occupied_coords.shape[0]):
bresenham_line = bresenham2d_with_intensities(
position_px, occupied_coords[i, :], self._map.data.T)[:-1]
free_coords = np.vstack((free_coords, bresenham_line[:, :2]))
for i in range(free_endcoords.shape[0]):
bresenham_line = bresenham2d_with_intensities(
position_px, free_endcoords[i, :], self._map.data.T)
free_coords = np.vstack((free_coords, bresenham_line[:, :2]))
free_coords = free_coords.astype(np.int)
occupied_coords = occupied_coords[which_coords_in_bounds(
occupied_coords, self._map.get_shape())]
free_coords = free_coords[which_coords_in_bounds(
free_coords, self._map.get_shape())]
if debug:
map_vis = np.repeat([self._map.copy()], repeats=3,
axis=0).transpose((1, 2, 0))
map_vis[occupied_coords[:, 0], occupied_coords[:,
1]] = [10, 10, 127]
map_vis[free_coords[:, 0], free_coords[:, 1]] = [127, 10, 127]
map_vis[int(state[0]), int(state[1])] = [127, 122, 10]
plt.imshow(map_vis, interpolation='nearest')
plt.show()
return occupied_coords.astype(np.int), free_coords.astype(np.int)
