def find_path(self, environment: GridEnvironment, start: Tuple[int, int],
end: Tuple[int, int]) -> Union[List[Tuple[int, int]], None]:
# Use heapq;the thread safety provided by ProrityQueue is not needed, as we only exec on a single thread
open = []
heappush(open, (0, start))
closed = {start: None}
costs = np.full(environment.grid.shape, np.inf)
costs[start[0], start[1]] = 0
while open:
node = heappop(open)[1]
if node == end:
import matplotlib.pyplot as mpl
mpl.matshow(costs)
mpl.show()
return self._reconstruct_path(end, closed, environment.grid)
current_cost = costs[node[0], node[1]]
for neighbour in environment.get_neighbours(node):
x, y = neighbour[1], neighbour[0]
cost = current_cost + environment.f_cost(node, neighbour)
if costs[y, x] > cost:
costs[y, x] = cost
heappush(
open,
(cost + self.heuristic(neighbour, end), neighbour))
closed[neighbour] = node
return None
def find_path(self, environment: GridEnvironment, start: Tuple[int, int],
end: Tuple[int, int]) -> Union[List[Tuple[int, int]], None]:
if not environment.diagonals:
raise ValueError('JPS relies on a grid environment with diagonals')
self.environment = environment
self._max_y, self._max_x = self.environment.grid.shape[
0] - 1, self.environment.grid.shape[1] - 1
self.goal = end
# Use heapq;the thread safety provided by ProrityQueue is not needed, as we only exec on a single thread
open = []
heappush(open, (0, start))
closed = {start: None}
costs = np.full(environment.grid.shape, np.inf)
costs[start[0], start[1]] = 0
# if __debug__:
# debug_heuristic_cost = np.full(environment.grid.shape, np.inf)
while open:
node = heappop(open)[1]
if node == end:
# if __debug__:
# import matplotlib.pyplot as mpl
# mpl.matshow(costs)
# mpl.matshow(debug_heuristic_cost)
# mpl.show()
return self._reconstruct_path(end, closed, environment.grid)
cy, cx = node[0], node[1]
current_cost = costs[cy, cx]
successors = []
for neighbour in environment.get_neighbours(node):
dx, dy = neighbour[1] - cx, neighbour[0] - cy
jumpPoint = self._jump(cy, cx, dy, dx)
if jumpPoint:
successors.append(jumpPoint)
for successor in successors:
x, y = successor[1], successor[0]
cost = current_cost + environment.f_cost(node, successor)
if costs[y, x] > cost:
costs[y, x] = cost
h = self.heuristic(successor, end)
heappush(open, (cost + h, successor))
closed[successor] = node
# if __debug__:
# debug_heuristic_cost[y, x] = h
return None
def find_path(self, environment: GridEnvironment, start: Node, end: Node) -> Union[
List[Node], None]:
if not environment.diagonals:
raise ValueError('JPS relies on a grid environment with diagonals')
if self.heuristic_env_hash != hash(environment):
raise ValueError("Risk based heuristic and algorithm should have the same environment")
global max_y, max_x
grid = environment.grid
max_y, max_x = grid.shape[0] - 1, grid.shape[1] - 1
self.goal = end
# Check if start and goal are the same
if start == end:
return [start]
# Use heapq;the thread safety provided by ProrityQueue is not needed, as we only exec on a single thread
open = [start]
start.f = start.g = start.h = 0
open_cost = {start: start.f}
closed = set()
for neighbour in environment.get_neighbours(start):
heappush(open, neighbour)
closed[neighbour] = start
neighbour = self.heuristic(start, neighbour)
while open:
node = heappop(open)
if node == end:
return _reconstruct_path(end, grid)
parent = closed[node]
py, px = parent[0], parent[1]
cy, cx = node[0], node[1]
current_cost = costs[cy, cx]
dy, dx = np.clip(cy - py, -1, 1), np.clip(cx - px, -1, 1)
ny, nx = cy + dy, cx + dx
# If the next node is not passable, the current node will be a dead end
if not is_passable(grid, ny, nx):
continue
jump_points = jump(grid, cy, cx, dy, dx, self.goal[0], self.goal[1], grid[ny, nx], self.jump_gap,
self.jump_limit, 0)
for node_vals in jump_points:
if (node_vals == -1).all():
continue
x, y = node_vals[0], node_vals[1]
successor = (y, x)
cost = current_cost + self.heuristic(node, successor)
if costs[y, x] > cost:
costs[y, x] = cost
h = self.heuristic(successor.position, end.position)
heappush(open, (cost + h, successor))
closed[successor] = node
return None
def find_path(self, environment: GridEnvironment, start: Node, end: Node, k=1, smooth=True, **kwargs) -> Union[
List[Node], None]:
grid = environment.grid
# Use heapq;the thread safety provided by PriorityQueue is not needed, as we only exec on a single thread
open = [start]
start.f = start.g = start.h = 0
open_cost = {start: start.f}
closed = set()
while open:
node = heappop(open)
if node in open_cost:
open_cost.pop(node)
if node in closed:
continue
closed.add(node)
if node == end:
return _reconstruct_path(node, grid, smooth=smooth)
current_cost = node.f
for neighbour in environment.get_neighbours(node):
cost = current_cost + grid[neighbour.position]
if cost < neighbour.g:
neighbour.g = cost
h = abs((node.position[0] - end.position[0])) + abs((node.position[1] - end.position[1]))
neighbour.h = h
neighbour.f = cost + (k * h)
neighbour.parent = node
if neighbour not in open_cost or neighbour.f < open_cost[neighbour]:
heappush(open, neighbour)
open_cost[neighbour] = neighbour.f
return None
def make(min_lat, max_lat, min_lon, max_lon, start_lat, start_lon, end_lat,
end_lon, aircraft, failure_prob, output_path, resolution, hour,
altitude, airspeed, wind_direction, wind_speed, algo, algo_args):
"""
Path minimising the fatality risk posed.
Generate a path minimising the fatality risk posed by a specified aircraft in the specified
bounds. The path is output as a GeoJSON file in the form of a LineString of EPSG:4326 coordinates in (lon, lat) order
from start to end.
If an aircraft config json file is not specified, a default aircraft with reasonable parameters is used.
All coordinates should be in decimal degrees and form a non degenerate polygon.
"""
bounds = make_bounds_polygon((min_lon, max_lon), (min_lat, max_lat))
pop_grid = _make_pop_grid(bounds, hour, resolution)
if not aircraft:
aircraft = _setup_default_aircraft()
else:
aircraft = _import_aircraft(aircraft)
strike_grid, v_is = _make_strike_grid(aircraft, airspeed, altitude,
failure_prob, pop_grid, resolution,
wind_direction, wind_speed)
res = _make_fatality_grid(aircraft, strike_grid, v_is)
raster_shape = res.shape
raster_indices = dict(Longitude=np.linspace(min_lon,
max_lon,
num=raster_shape[0]),
Latitude=np.linspace(min_lat,
max_lat,
num=raster_shape[1]))
start_x, start_y = snap_coords_to_grid(raster_indices, start_lon,
start_lat)
end_x, end_y = snap_coords_to_grid(raster_indices, end_lon, end_lat)
env = GridEnvironment(res, diagonals=False)
if algo == 'ra*':
algo = RiskGridAStar()
elif algo == 'ga':
raise NotImplementedError("GA CLI not implemented yet")
algo = GeneticAlgorithm([], **algo_args)
path = algo.find_path(env, Node((start_x, start_y)), Node((end_x, end_y)))
if not path:
print('Path not found')
return 1
snapped_path = []
for node in path:
lat = raster_indices['Latitude'][node.position[1]]
lon = raster_indices['Longitude'][node.position[0]]
snapped_path.append((lon, lat))
snapped_path = sg.LineString(snapped_path)
dataframe = gpd.GeoDataFrame({
'geometry': [snapped_path]
}).set_crs('EPSG:4326')
dataframe.to_file(os.path.join(output_path, f'path.geojson'),
driver='GeoJSON')
def annotate(self,
data: List[gpd.GeoDataFrame],
raster_data: Tuple[Dict[str, np.array], np.array],
resolution=30,
**kwargs) -> Geometry:
raster_grid = np.flipud(raster_data[1])
start_x, start_y = snap_coords_to_grid(raster_data[0],
self.start_coord[1],
self.start_coord[0])
end_x, end_y = snap_coords_to_grid(raster_data[0], self.end_coord[1],
self.end_coord[0])
if raster_grid[start_y, start_x] < 0:
print('Start node in blocked area, path impossible')
return None
elif raster_grid[end_y, end_x] < 0:
print('End node in blocked area, path impossible')
return None
env = GridEnvironment(raster_grid, diagonals=False)
algo = self.algo(
heuristic=self.heuristic(env, risk_to_dist_ratio=self.rdr))
t0 = time()
if isinstance(algo, RiskThetaStar):
self.path = algo.find_path(env,
Node((start_y, start_x)),
Node((end_y, end_x)),
thres=self.thresh)
elif isinstance(algo, RiskGridAStar):
self.path = algo.find_path(env, Node((start_y, start_x)),
Node((end_y, end_x)))
if self.path is None:
print("Path not found")
return None
else:
print('Path generated in ', time() - t0)
# mpl.matshow(raster_data[1], cmap='jet')
# mpl.colorbar()
# mpl.plot([n.x for n in path], [n.y for n in path], color='red')
# mpl.title(
# f'Costmap \n Start (x,y):({snapped_start_lon_idx}, {snapped_start_lat_idx})'
# f'\n End (x,y):({snapped_end_lon_idx}, {snapped_end_lat_idx})')
# mpl.show()
snapped_path = []
for node in self.path:
lat = raster_data[0]['Latitude'][node.position[0]]
lon = raster_data[0]['Longitude'][node.position[1]]
snapped_path.append((lon, lat))
snapped_path = sg.LineString(snapped_path)
self.dataframe = gpd.GeoDataFrame({
'geometry': [snapped_path]
}).set_crs('EPSG:4326')
return gv.Contours(self.dataframe).opts(line_width=4,
line_color='magenta')
class GridEnvironmentNoDiagonalsNoPruningTestCase(unittest.TestCase):
def setUp(self) -> None:
super().setUp()
self.small_environment = GridEnvironment(SMALL_TEST_GRID,
diagonals=False)
self.large_environment = GridEnvironment(LARGE_TEST_GRID,
diagonals=False)
def test_grid_to_graph(self):
self.maxDiff = None
graph = self.small_environment._generate_graph()
self.assertEqual(len(graph),
SMALL_TEST_GRID.shape[0] * SMALL_TEST_GRID.shape[1],
"Graph has wrong length")
def test_large_graph_gen(self):
graph = self.large_environment._generate_graph()
pass
def annotate(self,
data: List[gpd.GeoDataFrame],
raster_data: Tuple[Dict[str, np.array], np.array],
resolution=20,
**kwargs) -> Geometry:
raster_grid = raster_data[1] * resolution**2
start_x, start_y = snap_coords_to_grid(raster_data[0],
self.start_coords[1],
self.start_coords[0])
end_x, end_y = snap_coords_to_grid(raster_data[0], self.end_coords[1],
self.end_coords[0])
if raster_grid[start_y, start_x] < 0:
print('Start node in blocked area, path impossible')
return None
elif raster_grid[end_y, end_x] < 0:
print('End node in blocked area, path impossible')
return None
env = GridEnvironment(raster_grid, diagonals=False)
algo = self.algo(
heuristic=self.heuristic(env, risk_to_dist_ratio=self.rdr))
t0 = time()
path = algo.find_path(env, (start_y, start_x), (end_y, end_x))
if path is None:
print("Path not found")
return None
else:
print('Path generated in ', time() - t0)
# mpl.matshow(raster_data[1], cmap='jet')
# mpl.colorbar()
# mpl.plot([n.x for n in path], [n.y for n in path], color='red')
# mpl.title(
# f'Costmap \n Start (x,y):({snapped_start_lon_idx}, {snapped_start_lat_idx})'
# f'\n End (x,y):({snapped_end_lon_idx}, {snapped_end_lat_idx})')
# mpl.show()
snapped_path = []
for node in path:
lat = raster_data[0]['Latitude'][node[0]]
lon = raster_data[0]['Longitude'][node[1]]
snapped_path.append((lon, lat))
snapped_path = sg.LineString(snapped_path)
self.dataframe = gpd.GeoDataFrame({
'geometry': [snapped_path]
}).set_crs('EPSG:4326')
self.endpoint = self.dataframe.iloc[0].geometry.coords[-1]
return super(PathfindingLayer, self).annotate(data, raster_data,
**kwargs)
def generate_path_data_popup(self, layer):
from seedpod_ground_risk.pathfinding.environment import GridEnvironment
from seedpod_ground_risk.ui_resources.info_popups import DataWindow
from seedpod_ground_risk.layers.fatality_risk_layer import FatalityRiskLayer
for dlayer in self.data_layers:
if isinstance(
dlayer,
FatalityRiskLayer) and layer.aircraft == dlayer.ac_dict:
cur_layer = GridEnvironment(
self._generated_data_layers[dlayer.key][1])
grid = cur_layer.grid
popup = DataWindow(layer, grid)
popup.exec()
break
def find_path(self, environment: GridEnvironment, start: Node, end: Node, k=0.9, smooth=True, **kwargs) -> Union[
List[Node], None]:
grid = environment.grid
min_dist = 2 ** 0.5
goal_val = grid[end.position]
# Use heapq;the thread safety provided by PriorityQueue is not needed, as we only exec on a single thread
open = [start]
start.f = start.g = start.h = 0
open_cost = {start: start.f}
closed = set()
while open:
node = heappop(open)
if node in open_cost:
open_cost.pop(node)
if node in closed:
continue
closed.add(node)
if node == end:
return _reconstruct_path(node, grid, smooth=smooth)
current_cost = node.f
node_val = grid[node.position]
for neighbour in environment.get_neighbours(node):
cost = current_cost \
+ (((grid[neighbour.position] + node_val) / 2)
* (((node.position[1] - neighbour.position[1]) ** 2 + (
node.position[0] - neighbour.position[0]) ** 2) ** 0.5))
if cost < neighbour.g:
neighbour.g = cost
dist = ((node.position[1] - end.position[1]) ** 2 + (
node.position[0] - end.position[0]) ** 2) ** 0.5
line = skline(node.position[1], node.position[0], end.position[1], end.position[0])
min_val = grid[line[0], line[1]].min()
node_val = grid[node.position]
h = k * ((((node_val + goal_val) / 2) * min_dist) + ((dist - min_dist) * min_val))
# h = self.heuristic(neighbour.position, end.position)
neighbour.h = h
neighbour.f = cost + h
neighbour.parent = node
if neighbour not in open_cost or neighbour.f < open_cost[neighbour]:
heappush(open, neighbour)
open_cost[neighbour] = neighbour.f
return None
def find_path(self, environment: GridEnvironment, start: Tuple[int, int],
end: Tuple[int, int]) -> Union[List[Tuple[int, int]], None]:
# Use heapq;the thread safety provided by ProrityQueue is not needed, as we only exec on a single thread
open = [(0, start)]
# heappush(open, (0, start))
closed = {start: None}
costs = np.full(environment.grid.shape, np.inf)
costs[start[0], start[1]] = 0
# if __debug__:
# debug_heuristic_cost = np.full(environment.grid.shape, np.inf)
while open:
node = heappop(open)[1]
if node == end:
# if __debug__:
# import matplotlib.pyplot as mpl
# mpl.matshow(costs)
# mpl.title('R A* g cost (start to node)')
# mpl.colorbar()
# # mpl.matshow(debug_heuristic_cost)
# # mpl.title('R A* h cost (node to goal)')
# # mpl.colorbar()
# mpl.show()
return self._reconstruct_path(end, closed, environment.grid)
current_cost = costs[node[0], node[1]]
for neighbour in environment.get_neighbours(node):
x, y = neighbour[1], neighbour[0]
cost = current_cost + self.heuristic(node, neighbour)
if costs[y, x] > cost:
costs[y, x] = cost
h = self.heuristic(neighbour, end)
heappush(open, (cost + h, neighbour))
closed[neighbour] = node
# if __debug__:
# debug_heuristic_cost[y, x] = h
# import matplotlib.pyplot as mpl
# mpl.matshow(costs)
# mpl.title('R A* g cost (start to node)')
# mpl.colorbar()
# mpl.matshow(debug_heuristic_cost)
# mpl.title('R A* h cost (node to goal)')
# mpl.colorbar()
# mpl.show()
return None
def find_path(self,
environment: GridEnvironment,
start: Node,
end: Node,
smooth=False,
k=1,
thres=3e-8,
method=np.mean,
**kwargs) -> Union[List[Node], None]:
grid = environment.grid
self.risk_threshold = thres
self.cost_method = method
self.max_cost = grid.max()
self.max_dist = np.sqrt((grid.shape[0]**2) + (grid.shape[1]**2))
# Use heapq;the thread safety provided by PriorityQueue is not needed, as we only exec on a single thread
open = [start]
start.f = start.g = start.h = 0
start.parent = start
open_cost = {start: self._euc_dist(start, end)}
closed = set()
while open:
node = heappop(open)
if node in open_cost:
open_cost.pop(node)
else:
continue
closed.add(node)
if node == end:
return _reconstruct_path(node, grid, smooth=smooth)
for neighbour in environment.get_neighbours(node):
if neighbour in closed:
continue
cost, parent = self._calc_cost(neighbour, node, grid)
if neighbour in open_cost and cost < neighbour.g:
del open_cost[neighbour]
neighbour.g = cost
neighbour.parent = parent
neighbour.f = cost + (k * self._euc_dist(neighbour, node))
heappush(open, neighbour)
open_cost[neighbour] = neighbour.f
return None
def find_path(self, environment: GridEnvironment, start: Node, end: Node) -> Union[
List[Node], None]:
if not environment.diagonals:
raise ValueError('JPS relies on a grid environment with diagonals')
self.environment = environment
grid = environment.grid
self._max_y, self._max_x = self.environment.grid.shape[0] - 1, self.environment.grid.shape[1] - 1
self.goal = end
# Use heapq;the thread safety provided by ProrityQueue is not needed, as we only exec on a single thread
open = [start]
start.f = start.g = start.h = 0
open_cost = {start: start.f}
closed = set()
while open:
node = heappop(open)
open_cost.pop(node)
if node in closed:
continue
closed.add(node)
if node == end:
return _reconstruct_path(end, grid)
current_cost = node.f
cy, cx = node.position
successors = []
for neighbour in environment.get_neighbours(node):
dx, dy = neighbour.position[1] - cx, neighbour.position[0] - cy
jump_point = self._jump(cy, cx, dy, dx)
if jump_point:
successors.append(Node(jump_point))
for successor in successors:
cost = current_cost + grid[successor.position]
if cost < successor.g:
successor.g = cost
h = self.heuristic(successor.position, end.position)
successor.h = h
successor.f = h + cost
if successor not in open_cost or successor.f < open_cost[successor]:
heappush(open, successor)
open_cost[successor] = successor.f
return None
def setUp(self) -> None:
super().setUp()
self.env = GridEnvironment(SMALL_TEST_GRID)
self.heuristic = EuclideanRiskHeuristic(self.env, risk_to_dist_ratio=1)
def make_path(cost_grid,
bounds_poly,
start_latlon,
end_latlon,
algo='rt*',
pathwise_cost=False,
**kwargs):
from seedpod_ground_risk.path_analysis.utils import snap_coords_to_grid
from seedpod_ground_risk.pathfinding.environment import GridEnvironment, Node
import shapely.geometry as sg
raster_shape = cost_grid.shape
min_lat, min_lon, max_lat, max_lon = bounds_poly.bounds
start_lat, start_lon = start_latlon
end_lat, end_lon = end_latlon
raster_indices = dict(Longitude=np.linspace(min_lon,
max_lon,
num=raster_shape[0]),
Latitude=np.linspace(max_lat,
min_lat,
num=raster_shape[1]))
start_x, start_y = snap_coords_to_grid(raster_indices, start_lon,
start_lat)
end_x, end_y = snap_coords_to_grid(raster_indices, end_lon, end_lat)
if cost_grid[start_y, start_x] < 0:
raise ValueError('Start node in blocked area, path impossible')
elif cost_grid[end_y, end_x] < 0:
raise ValueError('End node in blocked area, path impossible')
env = GridEnvironment(cost_grid, diagonals=False)
if algo == 'ra*2':
from seedpod_ground_risk.pathfinding.a_star import RiskGridAStar
algo = RiskGridAStar()
elif algo == 'ra*':
from seedpod_ground_risk.pathfinding.a_star import RiskAStar
algo = RiskAStar()
elif algo == 'rt*':
from seedpod_ground_risk.pathfinding.theta_star import RiskThetaStar
algo = RiskThetaStar()
elif algo == 'ga':
from functools import partial
from seedpod_ground_risk.pathfinding.moo_ga import fitness_min_risk
from seedpod_ground_risk.pathfinding.moo_ga import fitness_min_manhattan_length
from seedpod_ground_risk.pathfinding.moo_ga import GeneticAlgorithm
max_dist = np.sqrt((cost_grid.shape[0]**2) + (cost_grid.shape[1]**2))
fitness_funcs = [
partial(fitness_min_risk, cost_grid, cost_grid.max()),
partial(fitness_min_manhattan_length, max_dist)
]
fitness_weights = [1e11, 1]
algo = GeneticAlgorithm(fitness_funcs, fitness_weights)
path = algo.find_path(env, Node((start_y, start_x)), Node((end_y, end_x)),
**kwargs)
if not path:
print('Path not found')
return None
snapped_path = []
for node in path:
lat = raster_indices['Latitude'][min(node.position[0],
raster_shape[1] - 1)]
lon = raster_indices['Longitude'][min(node.position[1],
raster_shape[0] - 1)]
snapped_path.append((lon, lat))
lla_path = sg.LineString(snapped_path)
if pathwise_cost:
path_cost = []
for idx in range(len(path[:-1])):
n0 = path[idx].position
n1 = path[idx + 1].position
l = line(n0[0], n0[1], n1[0], n1[1])
path_cost.append(cost_grid[l[0], l[1]])
return lla_path, path, np.hstack(path_cost)
else:
return lla_path
def setUp(self) -> None:
super().setUp()
self.small_environment = GridEnvironment(SMALL_TEST_GRID,
diagonals=False)
self.large_environment = GridEnvironment(LARGE_TEST_GRID,
diagonals=False)