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)