def __init__(self,
services: Services,
testing: BasicTesting = None) -> None:
super().__init__(services, testing)
start_vertex = Vertex(self._get_grid().agent.position)
goal_vertex = Vertex(self._get_grid().goal.position)
self._graph = Forest(start_vertex, goal_vertex, [])
def _get_new_vertex(self, q_near: Vertex, q_sample: Point, max_dist) -> Vertex:
dir = q_sample.to_tensor() - q_near.position.to_tensor()
if torch.norm(dir) <= max_dist:
return Vertex(q_sample)
dir_normalized = dir / torch.norm(dir)
q_new = Point.from_tensor(q_near.position.to_tensor() + max_dist * dir_normalized)
return Vertex(q_new)
def __init__(self,
services: Services,
testing: BasicTesting = None) -> None:
super().__init__(services, testing)
self._graph = gen_forest(self._services,
Vertex(self._get_grid().agent.position),
Vertex(self._get_grid().goal.position), [])
self._graph.edges_removable = False
self._init_displays()
def __init__(self,
services: Services,
testing: BasicTesting = None) -> None:
super().__init__(services, testing)
start_vertex = Vertex(self._get_grid().agent.position)
start_vertex.cost = 0
goal_vertex = Vertex(self._get_grid().goal.position)
self._graph = gen_forest(self._services, start_vertex, goal_vertex, [])
self._init_displays()
def __init__(self,
services: Services,
testing: BasicTesting = None) -> None:
super().__init__(services, testing)
self._V_size = 200
self._max_radius = 15
V: List[Vertex] = list()
for i in range(self._V_size):
q_rand: Point = self._get_random_sample()
V.append(Vertex(q_rand, store_connectivity=True))
self._graph = CyclicGraph(
Vertex(self._get_grid().agent.position, store_connectivity=True),
Vertex(self._get_grid().goal.position, store_connectivity=True), V)
def _extract_path(self, q_new):
goal_v: Vertex = Vertex(self._get_grid().goal.position)
self._graph.add_edge(q_new, goal_v)
# trace back
path: List[Vertex] = [goal_v]
while len(path[-1].parents) != 0:
for parent in path[-1].parents:
path.append(parent)
break
del path[-1]
path.reverse()
for p in path:
self.move_agent(p.position)
self.key_frame(ignore_key_frame_skip=True)
def _extract_path(self, q_new):
goal_v: Vertex = Vertex(self._get_grid().goal.position)
self._graph.add_edge(q_new, goal_v) #connect the last sampled point that's close to goal vertex and connet point to goal vertex with edge
path: List[Vertex] = [goal_v]
while len(path[-1].parents) != 0:
for parent in path[-1].parents:
path.append(parent)
break
del path[-1]
path.reverse()
#get animation of path tracing from start to goal
for p in path:
self.move_agent(p.position)
self.key_frame(ignore_key_frame_skip=True)
def _extract_path(self, q_new):
goal_v: Vertex = Vertex(self._get_grid().goal.position)
child_parent_dist = torch.norm(q_new.position.to_tensor() -
goal_v.position.to_tensor())
goal_v.cost = q_new.cost + child_parent_dist
self._graph.add_edge(q_new, goal_v)
path: List[Vertex] = [goal_v]
while len(path[-1].parents) != 0:
for parent in path[-1].parents:
path.append(parent)
break
del path[-1]
path.reverse()
for p in path:
self.move_agent(p.position)
self.key_frame(ignore_key_frame_skip=True)
def _extract_path(self, q_new):
goal_v: Vertex = Vertex(self._get_grid().goal.position)
child_parent_dist = torch.norm(q_new.position.to_tensor() -
goal_v.position.to_tensor())
goal_v.cost = q_new.cost + child_parent_dist
self._graph.add_edge(q_new, goal_v)
path: List[Vertex] = [goal_v]
while len(path[-1].parents) != 0:
for parent in path[-1].parents:
path.append(parent)
break
del path[-1]
path.reverse()
for p in path:
self.move_agent(p.position)
#sends waypoint for ros extension
grid: Map = self._get_grid()
if isinstance(grid, RosMap):
grid.publish_wp(grid.agent.position)
self.key_frame(ignore_key_frame_skip=True)
def remove_edge(self, parent: Vertex, child: Vertex):
parent.remove_child(child)
child.remove_parent(parent)
self.size -= 1
def add_edge(self, parent: Vertex, child: Vertex):
if child is not parent:
parent.add_child(child)
child.add_parent(parent)
self.size += 1
def __init__(self, services: Services, testing: BasicTesting = None) -> None:
super().__init__(services, testing)
self._graph = Forest(Vertex(self._get_grid().agent.position), Vertex(self._get_grid().goal.position), [])
self._max_dist = 10
self._iterations = 10000