def children(self, node, crossbar=True, checkLOS=True):
# The comments are to disable looking at 3D path. The performances will be improved greatly
if crossbar:
directions = np.array([[0,1,1], [0,1,-1], [0,-1,-1], [0,-1,1],
[1,1,0], [-1,1,0], [-1,-1,0], [1,-1,0],
[1,0,1], [-1,0,1], [-1,0,-1], [1,0,-1]])
# directions = np.array([[1,1,0], [-1,1,0], [-1,-1,0], [1,-1,0]])
else:
directions = np.array([[0, 0, 1], [1, 0, 1], [1, 1, 1], [0, 1, 1], [-1, 1, 1], [-1, 0, 1], [-1, -1, 1], [0, -1, 1], [1, -1, 1],
[1, 0, 0], [1, 1, 0], [0, 1, 0], [-1, 1, 0], [-1, 0, 0], [-1, -1, 0], [0, -1, 0], [1, -1, 0],
[0, 0,-1], [1, 0,-1], [1, 1,-1], [0, 1,-1], [-1, 1,-1], [-1, 0,-1], [-1, -1,-1], [0, -1,-1], [1, -1,-1]])
# directions = np.array([[1, 0, 0], [1, 1, 0], [0, 1, 0], [-1, 1, 0], [-1, 0, 0], [-1, -1, 0], [0, -1, 0], [1, -1, 0]])
potential_children = node.pos + directions * INCREMENT_DISTANCE
children = []
for c in potential_children:
if self.world_dim[0] <= c[0] <= self.world_dim[1] and self.world_dim[2] <= c[1] <= self.world_dim[3] and self.world_dim[4] <= c[2] <= self.world_dim[5]:
cell = pointToCell(c, self.world_dim, self.grid.shape, INCREMENT_DISTANCE)
child = self.grid[cell[0], cell[1], cell[2]]
if child is None:
child = Node(c)
self.grid[cell[0], cell[1], cell[2]] = child
if not checkLOS or not self.ros_node.cast_ray(node.pos, child.pos, radius=UAV_THICKNESS)[0]:
children.append(child)
return children
def children(node, ros_node, world_dim, grid, crossbar=True, checkLOS=True):
if crossbar:
directions = np.array([[0, 1, 1], [0, 1, -1], [0, -1, -1], [0, -1, 1],
[1, 1, 0], [-1, 1, 0], [-1, -1, 0], [1, -1, 0],
[1, 0, 1], [-1, 0, 1], [-1, 0, -1], [1, 0, -1]])
else:
directions = np.array([[0, 0, 1], [1, 0, 1], [1, 1, 1], [0, 1, 1],
[-1, 1, 1], [-1, 0, 1], [-1, -1, 1], [0, -1, 1],
[1, -1, 1], [1, 0, 0], [1, 1, 0], [0, 1, 0],
[-1, 1, 0], [-1, 0, 0], [-1, -1, 0], [0, -1, 0],
[1, -1, 0], [0, 0, -1], [1, 0, -1], [1, 1, -1],
[0, 1, -1], [-1, 1, -1], [-1, 0, -1],
[-1, -1, -1], [0, -1, -1], [1, -1, -1]])
potential_children = node.pos + directions * INCREMENT_DISTANCE
children = []
for c in potential_children:
if world_dim[0] <= c[0] <= world_dim[1] and world_dim[2] <= c[
1] <= world_dim[3] and world_dim[4] <= c[2] <= world_dim[5]:
cell = pointToCell(c, world_dim, grid.shape, INCREMENT_DISTANCE)
child = grid[cell[0], cell[1], cell[2]]
if child is None:
child = Node(c)
grid[cell[0], cell[1], cell[2]] = child
if not checkLOS or not ros_node.cast_ray(
node.pos, child.pos, radius=UAV_THICKNESS)[0]:
children.append(child)
return children
def clean_graph(self, bbmin, bbmax):
a, b = np.minimum(bbmin, bbmax), np.maximum(bbmin, bbmax)
cmin = pointToCell(a, self.world_dim, self.grid.shape, INCREMENT_DISTANCE)
cmin = cmin[0]-1-int(UAV_THICKNESS/INCREMENT_DISTANCE), cmin[1]-1-int(UAV_THICKNESS/INCREMENT_DISTANCE), cmin[2]-1-int(UAV_THICKNESS/INCREMENT_DISTANCE)
cmin = max(0, cmin[0]), max(0, cmin[1]), max(0, cmin[2])
cmin = min(cmin[0], self.grid.shape[0]-1), min(cmin[1], self.grid.shape[1]-1), min(cmin[2], self.grid.shape[2]-1)
cmax = pointToCell(b, self.world_dim, self.grid.shape, INCREMENT_DISTANCE)
cmax = cmax[0]+1+int(UAV_THICKNESS/INCREMENT_DISTANCE), cmax[1]+1+int(UAV_THICKNESS/INCREMENT_DISTANCE), cmax[2]+1+int(UAV_THICKNESS/INCREMENT_DISTANCE)
cmax = max(0, cmax[0]), max(0, cmax[1]), max(0, cmax[2])
cmax = min(cmax[0], self.grid.shape[0]-1), min(cmax[1], self.grid.shape[1]-1), min(cmax[2], self.grid.shape[2]-1)
for i in range(min(cmin[0], cmax[0]), max(cmin[0], cmax[0])+1):
for j in range(min(cmin[1], cmax[1]), max(cmin[1], cmax[1])+1):
for k in range(min(cmin[2], cmax[2]), max(cmin[2], cmax[2])+1):
if (self.grid[i,j,k] in self.openset or self.grid[i,j,k] in self.closedset) and self.grid[i,j,k] != self.start:
self.clearSubtree(self.grid[i,j,k])
def init_graph(self):
grid_shape = (int((self.world_dim[1] - self.world_dim[0]) // INCREMENT_DISTANCE),
int((self.world_dim[3] - self.world_dim[2]) // INCREMENT_DISTANCE),
int((self.world_dim[5] - self.world_dim[4]) // INCREMENT_DISTANCE))
self.grid = np.full(grid_shape, None)
cells = pointToCell(self.start, self.world_dim, self.grid.shape, INCREMENT_DISTANCE)
cellg = pointToCell(self.goal, self.world_dim, self.grid.shape, INCREMENT_DISTANCE)
self.grid[cells[0],cells[1],cells[2]] = Node(self.start)
self.grid[cellg[0],cellg[1],cellg[2]] = Node(self.goal)
self.start = self.grid[cells[0],cells[1],cells[2]]
self.goal = self.grid[cellg[0],cellg[1],cellg[2]]
self.goal.H, self.start.G, self.start.H = 0, 0, dist(self.start, self.goal)
self.openset = set()
self.closedset = set()
def phi_star(ros_node, start, goal, world_dim, display=True):
grid_shape = (int((world_dim[1] - world_dim[0]) // INCREMENT_DISTANCE),
int((world_dim[3] - world_dim[2]) // INCREMENT_DISTANCE),
int((world_dim[5] - world_dim[4]) // INCREMENT_DISTANCE))
global grid
grid = np.full(grid_shape, None)
cells = pointToCell(start, world_dim, grid.shape, INCREMENT_DISTANCE)
cellg = pointToCell(goal, world_dim, grid.shape, INCREMENT_DISTANCE)
grid[cells[0], cells[1],
cells[2]], grid[cellg[0], cellg[1],
cellg[2]] = Node(start), Node(goal)
start, goal = grid[cells[0], cells[1], cells[2]], grid[cellg[0], cellg[1],
cellg[2]]
goal.H, start.G, start.H = 0, 0, dist(start, goal)
openset = set()
closedset = set()
start_time = time.time()
i = 0
while True:
i += 1
path = find_path(ros_node, start, goal, grid, world_dim, openset,
closedset, display)
duration = abs(time.time() - start_time)
break
"""
if DISPLAY_END and WAIT_INPUT:
blockedCells = plot.waitForInput(grid_obs, lambda: plot.display(start, goal, grid, grid_obs))
else:
try:
blockedCells = next(newBlockedCells)
updateGridBlockedCells(blockedCells, grid_obs)
except StopIteration:
break
t1 = time.time()
for pt in corners(blockedCells):
if (grid[pt] in openset or grid[pt] in closedset) and grid[pt] != start:
clearSubtree(grid[pt], grid, grid_obs, openset, closedset)
"""
return path, duration
def update_goal(self, pos):
assert self.world_dim[0] <= pos[0] and pos[0] <= self.world_dim[1], 'Goal not contained on world dimensions x axis'
assert self.world_dim[2] <= pos[1] and pos[1] <= self.world_dim[3], 'Goal not contained on world dimensions y axis'
assert self.world_dim[4] <= pos[2] and pos[2] <= self.world_dim[5], 'Goal not contained on world dimensions z axis'
if self.ros_node.get_point_edt(pos, UAV_THICKNESS) <= GOAL_SAFETY_MARGIN:
pos = self.make_valid_point(pos)
cellg = pointToCell(pos, self.world_dim, self.grid.shape, INCREMENT_DISTANCE)
self.goal = self.grid[cellg[0],cellg[1],cellg[2]]
if self.goal is None:
self.goal = Node(pos)
self.grid[cellg[0],cellg[1],cellg[2]] = self.goal
self.goal.H = 0
for node in self.openset:
node.H = dist(node, self.goal, w_z=W_Z)