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)