def set_goal(self, pose):
r = self.map.get_distances(np.array([pose]))[0]
r = np.clip(r, 0.0, self.max_circle_radius)
self.next_goal = Circle(center=pose[:2],
radius=r,
angle=pose[2],
deflection=0)
def neighbors(self, state):
max_angle = utils.max_angle(self.min_turn_radius, state.radius)
self.thetas = np.linspace(-max_angle,
max_angle,
num=self.branch_factor)
self.euclidean_neighbors[:, 0] = state.radius * np.cos(
self.thetas + state.angle) + state.center[0]
self.euclidean_neighbors[:, 1] = state.radius * np.sin(
self.thetas + state.angle) + state.center[1]
# we use this several times, so compute this icky math here to avoid unnecessary computation
euc = self.euclidean_neighbors.copy()
utils.world_to_map(euc, self.map.map_info)
euc = np.round(euc).astype(int)
radii = self.map.get_distances(euc,
coord_convert=False,
check_bounds=True)
mask = np.logical_and(
np.invert(self.map.get_explored(euc, coord_convert=False)),
self.map.get_permissible(euc, coord_convert=False))
neighbors = map(
lambda c, r, t: Circle(
center=c.copy(
), radius=r, angle=state.angle + t, deflection=t),
self.euclidean_neighbors[mask, :], radii[mask], self.thetas[mask])
return neighbors
def reset(self, pose):
start_state = Circle(center=pose[:2],
radius=self.map.get_distances(np.array([pose
]))[0],
angle=pose[2],
deflection=0)
self.map.clear_exploration_buffer()
super(PathPlanner, self).reset(start_state)
def reset(self, pose):
# Frontier is a priority queue.
start_state = Circle(center=pose[:2],
radius=self.map.get_distances(np.array([pose
]))[0],
angle=pose[2],
deflection=0)
self.map.clear_exploration_buffer()
super(SpaceExploration, self).reset(start_state)
def neighbors(self, state):
''' This function determines the set of points along the perimeter of the given
circle state that are accessible according to ackermann geometry and steering
angle limitations. Then it samples the permissible theta space to generate neighbors.
TODO: computation of neighboring thetas is not accurate - it should actually be
the direction tangent to the path arc at the intersection with the circle radius.
For now this works, and is a strict underestimate of the reachable thetas
'''
max_angle = utils.max_angle(self.min_turn_radius, state.radius)
percentage = np.power(2.0 * self.branch_factor / np.pi, 0.9)
actual_branch_factor = 2 * int(max_angle * percentage / 2.0) + 1
thetas = np.linspace(-max_angle, max_angle, num=actual_branch_factor)
euclidean_neighbors = np.zeros((actual_branch_factor, 2))
euclidean_neighbors[:, 0] = state.radius * np.cos(
thetas + state.angle) + state.center[0]
euclidean_neighbors[:, 1] = state.radius * np.sin(
thetas + state.angle) + state.center[1]
# perform coordinate space conversion here and then index into the exploration and permissible
# buffers to prune states which are not permissible or already explored
euc = euclidean_neighbors.copy()
utils.world_to_map(euc, self.map.map_info)
euc = np.round(euc).astype(int)
radii = self.map.get_distances(
euc, coord_convert=False, check_bounds=True) - 0.05
radii = np.clip(radii, 0.0, self.max_circle_radius)
mask = np.logical_and(
np.invert(self.map.get_explored(euc, coord_convert=False)),
self.map.get_permissible(euc, coord_convert=False))
# generate a neighbor state for each of the permissible
neighbors = map(
lambda c, r, t: Circle(
center=c.copy(), radius=r, angle=state.angle + t, deflection=t
), euclidean_neighbors[mask, :], radii[mask], thetas[mask])
return neighbors
def set_goal(self, pose):
r = self.map.get_distances(np.array([pose]))[0]
self.next_goal = Circle(center=pose[:2],
radius=r,
angle=pose[2],
deflection=0)