def _compute_state_rays(self, state):
"""
Uses the precomputed rays to compute the rays at the current state
:param state array(3)[float]: state of the robot
:return List[array(N,2)[int]]: precomputed rays shifted by state
"""
shifted_angle_range = self.angle_range + np.round(
state[2] * 180 / np.pi)
if shifted_angle_range[0] == 180:
shifted_angle_range[0] *= -1
if shifted_angle_range[1] == -180:
shifted_angle_range[1] *= -1
desired_angles = wrap_angles(np.arange(
shifted_angle_range[0],
shifted_angle_range[1] + self.angle_increment,
self.angle_increment),
is_radians=False)
state_rays = [(circle_ray + state[:2]).astype(np.int)
for i, circle_ray in enumerate(self.circle_rays)
if self.ray_angles[i] in desired_angles]
return state_rays
def __init__(self,
sensor_range,
map_resolution,
angle_range=(-45, 45),
angle_increment=1):
"""
Python implementation of a lidar
:param sensor_range float: range of sensor in meters
:param map_resolution float: resolution of the map
:param angle_range Tuple[int]: (min angle, max angle) around 0, in degrees ie (-45, 45)
:param angle_increment float: angle increment of which to precompute rays in degrees
"""
Sensor.__init__(self, sensor_range)
assert angle_increment >= 1
self.angle_range = wrap_angles(np.array(angle_range))
self.map_resolution = map_resolution
self.range_px = np.rint(sensor_range / map_resolution).astype(np.int)
self.angle_increment = angle_increment
self.ray_angles, self.circle_rays = self._generate_circle_rays()
def step(self, desired_state):
"""
Execute the given action with the robot in the environment, return the next robot position,
and the output of sensor.measure() (sensor data)
:param desired_state array(3)[float]: desired next state of the robot
:return array(3)[float]: new_state (new robot position), sensor_data (output from sensor)
"""
new_state = np.array(desired_state, dtype=np.float)
new_state[2] = wrap_angles(desired_state[2])
# # todo maybe keep angle of desired state
if not self._is_state_valid(new_state + self.start_state,
use_inflation=False):
new_state = self.state.copy()
# compute sensor_data
measure_state = new_state.copy()
measure_state[:2] += self.start_state[:2]
sensor_data = self.sensor.measure(measure_state)
# set state
self.state = new_state
return new_state.copy(), sensor_data
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 _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)