def create_graph(data, drone_altitude, safety_distance):
grid, _, _ = create_grid(data, drone_altitude, safety_distance)
# minimum coordinates
north_min = np.floor(np.min(data[:, 0] - data[:, 3]))
east_min = np.floor(np.min(data[:, 1] - data[:, 4]))
points = []
for i in range(data.shape[0]):
north, east, _, _, _, _ = data[i, :]
points.append([north - north_min, east - east_min])
graph = Voronoi(points)
edges = []
for v in graph.ridge_vertices:
p1 = graph.vertices[v[0]]
p2 = graph.vertices[v[1]]
cells = list(bresenham(int(p1[0]), int(p1[1]), int(p2[0]), int(p2[1])))
hit = False
for c in cells:
# First check if we're off the map
if np.amin(
c) < 0 or c[0] >= grid.shape[0] or c[1] >= grid.shape[1]:
hit = True
break
# Next check if we're in collision
if grid[c[0], c[1]] == 1:
hit = True
break
# If the edge does not hit on obstacle
# add it to the list
if not hit:
# array to tuple for future graph creation step)
p1 = (p1[0], p1[1])
p2 = (p2[0], p2[1])
edges.append((p1, p2))
# create a graph with Euclidean distance as the weight of the edges
graph = nx.Graph()
for e in edges:
p1 = e[0]
p2 = e[1]
dist = np.linalg.norm(np.array(p2) - np.array(p1))
graph.add_edge(p1, p2, weight=dist)
return graph
def create_grid_and_edges(data, drone_altitude, safety_distance):
"""
Returns a grid representation of a 2D configuration space
along with Voronoi graph edges given obstacle data and the
drone's altitude.
"""
# minimum and maximum north coordinates
north_min = np.floor(np.min(data[:, 0] - data[:, 3]))
north_max = np.ceil(np.max(data[:, 0] + data[:, 3]))
# minimum and maximum east coordinates
east_min = np.floor(np.min(data[:, 1] - data[:, 4]))
east_max = np.ceil(np.max(data[:, 1] + data[:, 4]))
# given the minimum and maximum coordinates we can
# calculate the size of the grid.
north_size = int(np.ceil((north_max - north_min + 1)))
east_size = int(np.ceil((east_max - east_min + 1)))
# Initialize an empty grid
grid = np.zeros((north_size, east_size))
# Initialize an empty list for Voronoi points
points = []
# Populate the grid with obstacles
for i in range(data.shape[0]):
north, east, alt, d_north, d_east, d_alt = data[i, :]
if alt + d_alt + safety_distance > drone_altitude:
obstacle = [
int(
np.clip(north - d_north - safety_distance - north_min, 0,
north_size - 1)),
int(
np.clip(north + d_north + safety_distance - north_min, 0,
north_size - 1)),
int(
np.clip(east - d_east - safety_distance - east_min, 0,
east_size - 1)),
int(
np.clip(east + d_east + safety_distance - east_min, 0,
east_size - 1)),
]
grid[obstacle[0]:obstacle[1] + 1, obstacle[2]:obstacle[3] + 1] = 1
# add center of obstacles to points list
points.append([north - north_min, east - east_min])
# TODO: create a voronoi graph based on
# location of obstacle centres
graph = Voronoi(points)
# TODO: check each edge from graph.ridge_vertices for collision
edges = []
for v in graph.ridge_vertices:
p1 = graph.vertices[v[0]]
p2 = graph.vertices[v[1]]
cells = list(bresenham(int(p1[0]), int(p1[1]), int(p2[0]), int(p2[1])))
hit = False
for c in cells:
# First check if we're off the map
if np.amin(
c) < 0 or c[0] >= grid.shape[0] or c[1] >= grid.shape[1]:
hit = True
break
# Next check if we're in collision
if grid[c[0], c[1]] == 1:
hit = True
break
# If the edge does not hit on obstacle
# add it to the list
if not hit:
# array to tuple for future graph creation step)
p1 = (p1[0], p1[1])
p2 = (p2[0], p2[1])
edges.append((p1, p2))
graph = nx.Graph()
for e in edges:
p1 = e[0]
p2 = e[1]
dist = np.linalg.norm(np.array(p2) - np.array(p1))
graph.add_edge(p1, p2, weight=dist)
return grid, graph, int(north_min), int(east_min), int(north_max), int(
east_max)
