def find_path(data, grid_start, grid_goal, drone_altitude, safety_distance): # This is now the routine using Voronoi _, edges = create_grid_and_edges(data, drone_altitude, safety_distance) g = nx.Graph() for p1, p2 in edges: dist = LA.norm(np.array(p2) - np.array(p1)) g.add_edge(p1, p2, weight=dist) start_closest = closest_point(g, grid_start) print(grid_start, start_closest) goal_closest = closest_point(g, grid_goal) print(grid_goal, goal_closest) # 2. Compute the path from start to goal using A* algorithm path, cost = a_star(g, heuristic, start_closest, goal_closest) # insert actual start and goal positions if len(path) > 0: path.insert(0, grid_start) path.append(grid_goal) # convert double coordinates to integers _path = [] for n, e in path: _path.append((int(n), int(e))) return _path, cost
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values firstline = open('colliders.csv').readline().split(',') lat0 = float(firstline[0].strip("lat0 ")) lon0 = float(firstline[1].strip("lon0 ")) print('lat0 = {}, lon0 = {}'.format(lat0, lon0)) # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position local_position = global_to_local(self.global_position, self.global_home) # TODO: convert to current local position using global_to_local() print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) grid, edges, north_offset, east_offset = create_grid_and_edges( data, TARGET_ALTITUDE, SAFETY_DISTANCE) offset = np.array([north_offset, east_offset, 0]) #create the graph with the weight of the edges using euclidean distance between the points G = nx.Graph() for e in edges: p1 = e[0] p2 = e[1] dist = LA.norm(np.array(p2) - np.array(p1)) G.add_edge(p1, p2, weight=dist) start_pos = closest_point(G, local_position - offset) local_goal = global_to_local( [float(self.goal_lon), float(self.goal_lat), float(self.goal_alt)], self.global_home) - offset goal_pos = closest_point(G, local_goal) print('Local Start and Goal: ', start_pos, goal_pos) path, _ = a_star_graph(G, start_pos, goal_pos) path = prune_path(path, 0.5) # Convert path to waypoints waypoints = [[ int(p[0] + north_offset), int(p[1] + east_offset), TARGET_ALTITUDE, 0 ] for p in path] # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values with open('colliders.csv') as f: row1 = f.readline().strip().replace(',','').split(' ') home = {row1[0]: float(row1[1]), row1[2]: float(row1[3])} # TODO: set home position to (lon0, lat0, 0) self.set_home_position(home['lon0'], home['lat0'], 0.0) # TODO: convert to current local position using global_to_local() local_north, local_east, local_down = global_to_local(self.global_position, self.global_home) print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) if self.planning_option == 'GRID': print('creating grid...') # Define a grid for a particular altitude and safety margin around obstacles grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) else: print('creating graph...') grid, edges, north_offset, east_offset = create_grid_and_edges(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format(north_offset, east_offset)) # Define starting point on the grid (this is just grid center) # TODO: convert start position to current position rather than map center grid_start = (int(local_north-north_offset), int(local_east-east_offset)) # Set goal as some arbitrary position on the grid # TODO: adapt to set goal as latitude / longitude position and convert goal_global = [-122.40196856, 37.79673623, 0.0] goal_north, goal_east, goal_down = global_to_local(goal_global, self.global_home) grid_goal = (int(goal_north-north_offset), int(goal_east-east_offset)) # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) print('Local Start and Goal: ', grid_start, grid_goal) if self.planning_option == 'GRID': # Call A* based on grid search path, _ = a_star_grid(grid, heuristic, grid_start, grid_goal) else: # creating a graph first G = nx.Graph() for e in edges: p1 = e[0] p2 = e[1] dist = np.linalg.norm(np.array(p2) - np.array(p1)) G.add_edge(p1, p2, weight=dist) # find nearest from the graph nodes start_ne_g = closest_point(G, grid_start) goal_ne_g = closest_point(G, grid_goal) # Call A* based on graph search path, _ = a_star_graph(G, heuristic, start_ne_g, goal_ne_g) # Append the actual start and goal states to path path = [grid_start] + path + [grid_goal] # TODO: prune path to minimize number of waypoints pruned_path = prune_path(path) # Convert path to waypoints(use integer for waypoints) waypoints = [[int(p[0] + north_offset), int(p[1] + east_offset), TARGET_ALTITUDE, 0] for p in pruned_path] # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # DONE: read lat0, lon0 from colliders into floating point values lat_lon = [] with open('colliders.csv') as f: lat_lon = (f.readline()).split(",") lat0 = float(re.findall(r'-?\d+\.?\d*', lat_lon[0])[1]) lon0 = float(re.findall(r'-?\d+\.?\d*', lat_lon[1])[1]) print("Lat: {0}, Lon: {1}".format(lat0, lon0)) # DONE: set home position to (lon0, lat0, 0) self.set_home_position = (lon0, lat0, 0) # DONE: retrieve current global position # DONE: convert to current local position using global_to_local() start_pos = global_to_local(self.global_position, self.global_home) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles # NOTE: Creating grid with edges to enable graph search below grid, north_offset, east_offset, edges = create_grid_and_edges( data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("found {} edges. Creating graph... ".format(len(edges))) G = create_graph_from_edges(edges) print("Graph created") print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # DONE: convert start position to current position rather than map center grid_start = (int(start_pos[0]) - north_offset, int(start_pos[1]) - east_offset) # DONE: adapt to set goal as latitude / longitude position and convert # Note use: self.poi.get_random() for a random POI goal = global_to_local(self.poi.get("Washington Street"), self.global_home) grid_goal = (int(goal[0]) - north_offset, int(goal[1]) - east_offset) # Run A* to find a path from start to goal # DONE: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) # NOTE: I've implemented the diagonals but then implemented moved to graph search # I left the diagnonal code in. # Figure out the closest node to the the grid start and goal positions nodes = np.array(G.nodes) node_start = np.linalg.norm(np.array(grid_start) - np.array(nodes), axis=1).argmin() node_goal = np.linalg.norm(np.array(grid_goal) - np.array(nodes), axis=1).argmin() g_start = tuple(nodes[node_start]) + (self.global_home[2], ) g_goal = tuple(nodes[node_goal]) + (-1 * (int(goal[2])), ) print('Local Start and Goal: ', g_start, g_goal) print('Starting A*') path, _ = a_star_graph(G, heuristic, g_start, g_goal) # Add gentle gradient for flying car (especially if goal is higher) path = add_altitude_gradient(path, g_start, g_goal) # The graph search ends at the clsoest node so I add the last point if there are no obstacles on the way if len(path) > 0 and (path[-1] != grid_goal) and not (collision_check( path[-1], g_goal, grid)): print("Adding end position") path.append(g_goal) # DONE: prune path to minimize number of waypoints # DONE (if you're feeling ambitious): Try a different approach altogether! # NOTE: I further prune the path by removing nodes not blocked by obstacles # I take height into consideration so it does have paths that fly over # low obstacles path = prune_path(path, grid) # Convert path to waypoints waypoints = [[ int(p[0]) + north_offset, int(p[1]) + east_offset, int(p[2]), 0 ] for p in path] #Asjust waypoint headings for i in range(1, len(waypoints)): wp1 = waypoints[i - 1] wp2 = waypoints[i] wp2[3] = np.arctan2((wp2[1] - wp1[1]), (wp2[0] - wp1[0])) print(waypoints) # Set self.waypoints self.waypoints = waypoints # DONE: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 10 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values first_line = next(open("colliders.csv")) gps = first_line.split(',') lat0 = np.float64(gps[0].lstrip().split(' ')[1]) lon0 = np.float64(gps[1].lstrip().split(' ')[1]) #print("The Lat and Long = ", lat0, lon0) # TODO: set home position to (lat0, lon0, 0) self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position # self.global_position = self.global_position #print("global_home ", self.global_home) # TODO: convert to current local position using global_to_local() current_local_pos = global_to_local(self.global_position, self.global_home) print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format(north_offset, east_offset)) # Define starting point on the grid (this is just grid center) #grid_start = (-north_offset, -east_offset) #(37.795345, -122.398013)? Mine is (633, 393). # TODO: convert start position to current position rather than map center grid_start = (-north_offset + int(current_local_pos[0]), -east_offset + int(current_local_pos[1])) #grid_start = (-north_offset-100, -east_offset -100) grid_goal = self.global_to_local(sys.argv[2],sys.argv[4],north_offset, east_offset) #start_ne = (203, 699) #goal_ne = (690, 300) """ # This section is for Grid Based Implementation # Set goal as some arbitrary position on the grid #grid_goal = (-north_offset + 10, -east_offset + 10) # TODO: adapt to set goal as latitude / longitude position and convert # print("Arbitary Goal = ", self.get_arbitory_goal(data, 10)) goal_latlon = self.get_arbitory_goal(grid, 10) goal_north = int(np.abs(goal_latlon[0])) goal_east = int(np.abs(goal_latlon[1])) #current_goal = global_to_local(goal_latlon, self.global_home) # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) print('Local Start and Goal: ', grid_start, grid_goal) path, _ = a_star(grid, heuristic, grid_start, grid_goal) print("path ", len(path)) # TODO: prune path to minimize number of waypoints # TODO (if you're feeling ambitious): Try a different approach altogether! pruned_path = self.prune_path(path) print("pruned_path = ",pruned_path) # Convert path to waypoints waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path] """ # Graph Implementation # This is now the routine using Voronoi grid, edges = create_grid_and_edges(data, TARGET_ALTITUDE, SAFETY_DISTANCE) G = nx.Graph() for e in edges: p1 = e[0] p2 = e[1] dist = LA.norm(np.array(p2) - np.array(p1)) G.add_edge(p1, p2, weight=dist) #start_ne = (815, 639) # Map to Grid location without any obstacles print('Local Start and Goal: ', grid_start, grid_goal) start_ne_g = self.closest_point(G, grid_start) goal_ne_g = self.closest_point(G, grid_goal) #print(start_ne_g,goal_ne_g) path1, cost = a_star_graph(G, heuristic, start_ne_g, goal_ne_g) print("path1 ",len(path1)) pruned_path = self.prune_path(path1) waypoints = [[int(p[0] + north_offset), int(p[1] + east_offset), TARGET_ALTITUDE, 0] for p in pruned_path] # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 3 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values with open('colliders.csv', newline='') as f: reader = csv.reader(f) row1 = next(reader) # gets the first line lat0, lon0 = float(row1[0][5:]), float(row1[1][5:]) # TODO: set home position to (lat0, lon0, 0) self.set_home_position( lon0, lat0, 0) # set the current location to be the home position # TODO: retrieve current global position current_global_pos = (self._longitude, self._latitude, self._altitude) # TODO: convert to current local position using global_to_local() current_local_pos = global_to_local(current_global_pos, self.global_home) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Get the center of the grid north_offset = int(np.floor(np.min(data[:, 0] - data[:, 3]))) east_offset = int(np.floor(np.min(data[:, 1] - data[:, 4]))) # Define a grid for a particular altitude and safety margin around obstacles print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # This is now the routine using Voronoi grid_graph, edges = create_grid_and_edges(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("GRAPH EDGES:", len(edges)) # Create the graph with the weight of the edges # set to the Euclidean distance between the points G = nx.Graph() for e in edges: p1 = e[0] p2 = e[1] dist = LA.norm(np.array(p2) - np.array(p1)) G.add_edge(p1, p2, weight=dist) # TODO: convert start position to current position rather than map center grid_start = (int(current_local_pos[0] - north_offset), int(current_local_pos[1] - east_offset)) # TODO: adapt to set goal as latitude / longitude position and convert goal_global_pos = (-122.393010, 37.790000, 0) goal_local_pos = global_to_local(goal_global_pos, self.global_home) grid_goal = (int(goal_local_pos[0] - north_offset), int(goal_local_pos[1] - east_offset)) #goal_ne = (840., 100.) start_ne_g = closest_point(G, grid_start) goal_ne_g = closest_point(G, grid_goal) print("START NE GOAL:", start_ne_g) print("GOAL NE GOAL", goal_ne_g) # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) path, cost = a_star_graph(G, heuristic_graph, start_ne_g, goal_ne_g) print("Path Length:", len(path), "Path Cost:", cost) int_path = [[int(p[0]), int(p[1])] for p in path] # TODO: prune path to minimize number of waypoints pruned_path = prune_path(int_path) print("Length Pruned Path:", len(pruned_path)) # Convert path to waypoints waypoints = [[ p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0 ] for p in pruned_path] # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 10 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # DONE: read lat0, lon0 from colliders into floating point values with open('colliders.csv') as f: first_line = f.readline().split(',') lat0 = float(first_line[0].strip().split(' ')[1]) lon0 = float(first_line[1].strip().split(' ')[1]) alt0 = 0. print('start pos: ({}, {}, {})'.format(lon0, lat0, alt0)) # DONE: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, alt0) # DONE: retrieve current global position print('global_position: {}'.format(self.global_position)) # DONE: convert to current local position using global_to_local() n_loc, e_loc, d_loc = global_to_local(self.global_position, self.global_home) print('n_loc: {}, e_loc: {}, d_loc: {}'.format(n_loc, e_loc, d_loc)) print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles grid, edges, north_offset, east_offset = create_grid_and_edges(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print('grid = {}'.format(grid)) # print("edges = {}".format(edges)) print('north_offset: {}, east_offset: {}'.format(north_offset, east_offset)) # create the graph with the weight of the edges set to the Euclidean distance between the points G = nx.Graph() for e in edges: p1 = e[0] p2 = e[1] dist = np.linalg.norm(np.array(p2) - np.array(p1)) G.add_edge(p1, p2, weight=dist) # Define starting point on the grid (this is just grid center) # DONE: convert start position to current position rather than map center #grid_start = [local_pos[0], local_pos[1]] grid_start = (int(np.ceil(n_loc - north_offset)), int(np.ceil(e_loc - east_offset))) graph_start = closest_point(G, grid_start) print('grid_start: {}, graph_start: {}'.format(grid_start, graph_start)) # # redefine home so that the drone does not spawn inside a building # lat1, lon1, alt1 = local_to_global((grid_start[0], grid_start[1], 0.), self.global_home) # self.set_home_position(lon1, lat1, alt1) # Set goal as some arbitrary position on the grid # DONE: adapt to set goal as latitude / longitude position and convert super_duper_burgers = (-122.39400878361174, 37.792827005959616, 0.) grid_goal_global = super_duper_burgers print('grid_goal_global: {}'.format(grid_goal_global)) grid_goal_local = global_to_local(grid_goal_global , self.global_home) print('grid_goal_local: {}'.format(grid_goal_local)) graph_goal = closest_point(G, (grid_goal_local[0], grid_goal_local[1])) print('graph_goal: {}'.format(graph_goal)) # Run A* to find a path from start to goal # DONE: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) path, cost = a_star(G, heuristic, graph_start, graph_goal) self.plot(grid, edges, path, grid_start, graph_start, grid_goal_local, graph_goal, 'full_path.png') # DONE: prune path to minimize number of waypoints pruned_path = prune_path(path) self.plot(grid, edges, pruned_path, grid_start, graph_start, grid_goal_local, graph_goal, 'pruned_path.png') # TODO (if you're feeling ambitious): Try a different approach altogether! # Convert path to waypoints waypoints = [[int(p[0] + north_offset), int(p[1] + east_offset), TARGET_ALTITUDE, 0] for p in pruned_path] print('waypoints: {}'.format(waypoints)) # Set self.waypoints self.waypoints = waypoints # DONE: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
# # plt.imshow(grid_narrow, cmap='Greys', origin='lower') # plt.plot(grid_start[1], grid_start[0], 'x') # plt.plot(grid_goal[1], grid_goal[0], 'x') # if path is not None: # pp = np.array(path) # plt.plot(pp[:, 1], pp[:, 0], 'o', color='r') # plt.plot(pp[:, 1], pp[:, 0], 'g') # plt.xlabel('NORTH') # plt.ylabel('EAST') # plt.show() ###################### Voronoi ###################### # Create grid and voronoi edges from data timer = time.time() grid, offset, edges = create_grid_and_edges(data, TARGET_ALTITUDE, SAFETY_DISTANCE_VORONOI) print('Voronoi {0} edges created in {1}s'.format(len(edges), time.time() - timer)) # Construct graph from voronoi edges G = nx.Graph() for e in edges: G.add_edge(tuple(e[0]), tuple(e[1]), weight=np.linalg.norm(np.array(e[1]) - np.array(e[0]))) print('Graph created') # Iterate several times for different start/goal locations for iteration in range(1): print('> iteration #{0}'.format(iteration))
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a flying altitude (feel free to change this) drone_altitude = 5 safety_distance = 3 grid, north_offset, east_offset, edges = create_grid_and_edges( data, drone_altitude, safety_distance) print('Found %5d edges' % len(edges)) plt.imshow(grid, origin='lower', cmap='Greys') # Stepping through each edge for e in edges: p1 = e[0] p2 = e[1] plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'b-') plt.xlabel('EAST') plt.ylabel('NORTH') plt.show() # Create networkx graph graph = nx.Graph() for e in edges: graph.add_edge(*e) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # Define starting point on the grid (this is just grid center) grid_start = ( 640.7610999999999, 398.7684653999999 ) # random point generated from: grid_start = edges[random.randint(0, len(edges))][0] # Set goal as some arbitrary position on the grid grid_goal = ( 250.761145, 190.76849833333335 ) # random point generated from: grid_goal = edges[random.randint(0, len(edges))][0] # Run bfs to find a path from start to goal print('Local Start and Goal: ', grid_start, grid_goal) path, _ = bfs(graph, _, grid_start, grid_goal) # print(path) # Plot resulted path plt.imshow(grid, origin='lower', cmap='Greys') for e in edges: p1 = e[0] p2 = e[1] plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'b-') for p in path: plt.plot(p[1], p[0], 'ro-', markersize=2) plt.xlabel('EAST') plt.ylabel('NORTH') plt.show() # Convert path to waypoints waypoints = [[ p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0 ] for p in path] # Set self.waypoints self.waypoints = waypoints self.send_waypoints()
def find_path(self, data, TARGET_ALTITUDE, SAFETY_DISTANCE, start_v, goal_v, nmin, emin): """ Path search with different algorithms INPUT: search_alg = [VORONOI, MEDIAL_AXIS] OUTPUT: path """ if self.search_alg == SearchAlg.VORONOI: # Define a grid for a particular altitude and safety margin around obstacles #grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) #print("North offset = {0}, east offset = {1}".format(north_offset, east_offset)) import time t0 = time.time() grid, edges = create_grid_and_edges(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print('Found %5d edges' % len(edges)) #print(flight_altitude, safety_distance) print('graph took {0} seconds to build'.format(time.time() - t0)) # create the graph with the weight of the edges # set to the Euclidean distance between the points G = nx.Graph() for e in edges: p1 = tuple(e[0]) p2 = tuple(e[1]) dist = np.linalg.norm(np.array(p2) - np.array(p1)) G.add_edge(p1, p2, weight=dist) graph_start, graph_goal = start_goal_graph(G, start_v, goal_v) # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) print('Local Start and Goal on Voronoi graph: ', graph_start, graph_goal) path_graph, cost = a_star_graph(G, eucl_heuristic, graph_start, graph_goal) print('Number of edges', len(path_graph), 'Cost', cost) # TODO: prune path to minimize number of waypoints # TODO (if you're feeling ambitious): Try a different approach altogether! waypoints = [[ int(p[0] + nmin), int(p[1] + emin), TARGET_ALTITUDE, 0 ] for p in path_graph] return waypoints elif self.search_alg == SearchAlg.MEDIAL_AXIS: # Define a grid for a particular altitude and safety margin around obstacles grid, north_offset, east_offset = create_grid( data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # Define starting point on the grid (this is just grid center) #grid_start = (-north_offset, -east_offset) #grid_start =(int(start_v[0][0]),int(start_v[0][1])) # TODO: convert start position to current position rather than map center # Set goal as some arbitrary position on the grid #grid_goal = (-north_offset + 10, -east_offset + 10) #grid_goal = (int(goal_v[0][0]),int(goal_v[0][1])) # TODO: adapt to set goal as latitude / longitude position and convert skeleton = medial_axis(invert(grid)) #print(skeleton.shape) skel_start, skel_goal = find_start_goal_skeleton( skeleton, start_v[0:2], goal_v[0:2]) # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) print('Local Start and Goal on Skeleton: ', skel_start, skel_goal) #Manhattan heuristic #print(invert(skeleton).astype(np.int)) path, cost = a_star( invert(skeleton).astype(np.int), manh_heuristic, tuple(skel_start), tuple(skel_goal)) # TODO: prune path to minimize number of waypoints pruned_path = prune_path(path) # TODO (if you're feeling ambitious): Try a different approach altogether! waypoints = [[ int(p[0] + nmin), int(p[1] + emin), TARGET_ALTITUDE, 0 ] for p in pruned_path] return waypoints elif self.search_alg == SearchAlg.PROBABILISTIC: num_samp = 1000 sampler = Sampler(data, TARGET_ALTITUDE, SAFETY_DISTANCE) start = (start_v[0] + nmin, start_v[1] + emin, start_v[2]) goal = (goal_v[0] + nmin, goal_v[1] + emin, goal_v[2]) polygons = sampler._polygons nodes = sampler.sample(num_samp) #for ind in range(len(nodes)): # nodes[ind] = (nodes[ind][0] - nmin, nodes[ind][1] - emin, nodes[ind][2]) print('number of nodes', len(nodes), 'local NE', nodes[1]) nearest_neighbors = 10 import time t0 = time.time() g = create_graph(nodes, nearest_neighbors, polygons) print('graph took {0} seconds to build'.format(time.time() - t0)) print("Number of edges", len(g.edges)) grid, north_offset, east_offset = create_grid( data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) prob_start, prob_goal = start_goal_graph(g, start, goal) print('Local Start and Goal on Probabilistic map: ', prob_start, prob_goal) path, cost = a_star_graph(g, heuristic, prob_start, prob_goal) #path_pairs = zip(path[:-1], path[1:]) """ plt.imshow(grid, cmap='Greys', origin='lower') nmin = np.min(data[:, 0]) emin = np.min(data[:, 1]) # draw nodes for n1 in g.nodes: plt.scatter(n1[1] - emin, n1[0] - nmin, c='red') # draw edges for (n1, n2) in g.edges: plt.plot([n1[1] - emin, n2[1] - emin], [n1[0] - nmin, n2[0] - nmin], 'grey') # TODO: add code to visualize the path path_pairs = zip(path[:-1], path[1:]) for (n1, n2) in path_pairs: plt.plot([n1[1] - emin, n2[1] - emin], [n1[0] - nmin, n2[0] - nmin], 'green') plt.plot(start[1]-emin, start[0]-nmin, 'bv') plt.plot(goal[1]-emin, goal[0]-nmin, 'bx') plt.xlabel('NORTH') plt.ylabel('EAST') plt.show() """ waypoints = [[int(p[0]), int(p[1]), int(p[2]), 0] for p in path] return waypoints
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 4 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values with open('colliders.csv', newline='') as f: reader = csv.reader(f) row1 = next(reader) # gets the first line lat0 = float(row1[0].split()[1]) lon0 = float(row1[1].split()[1]) print('lat0:', lat0, ', lon0:', lon0) # TODO: set home position to (lon0, lat0, 0) self.global_home[0] = lon0 self.global_home[1] = lat0 self.global_home[2] = 0 #self.global_home[0]=self._longitude #self.global_home[1]=self._latitude #self.global_home[2]=0 print('global home {0}'.format(self.global_home)) # TODO: retrieve current global position global_position = self.global_position # TODO: convert to current local position using global_to_local() local_position = global_to_local(global_position, self.global_home) print('local position:', local_position) self.local_position[0] = local_position[0] self.local_position[1] = local_position[1] self.local_position[2] = local_position[2] print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # Define starting point on the grid (this is just grid center) # grid_start = (-north_offset, -east_offset) grid_start = (int(self.local_position[0] - north_offset), int(self.local_position[1] - east_offset)) # TODO: convert start position to current position rather than map center # Set goal as some arbitrary position on the grid # grid_goal = (-north_offset , -east_offset ) # TODO: adapt to set goal as latitude / longitude position and convert goal_north = 100 goal_east = -150 grid_goal = (-north_offset + goal_north, -east_offset + goal_east) if self.choices_list[self.choice_idx] == 'astar': # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) print('Local Start and Goal: ', grid_start, grid_goal) path, _ = a_star(grid, heuristic, grid_start, grid_goal) if self.choices_list[self.choice_idx] == 'medial_axis': skeleton = medial_axis(invert(grid)) skel_start, skel_goal = find_start_goal(skeleton, grid_start, grid_goal) print('Local Start and Goal: ', skel_start, skel_goal) path, _ = a_star( invert(skeleton).astype(np.int), heuristic, tuple(skel_start), tuple(skel_goal)) if self.choices_list[self.choice_idx] == 'voronoi_graph_search': grid, edges, north_offset, east_offset = create_grid_and_edges( data, TARGET_ALTITUDE, SAFETY_DISTANCE) G = create_weighted_graph(edges) start_ne_g = closest_point(G, grid_start) goal_ne_g = closest_point(G, grid_goal) print('Local Start and Goal: ', start_ne_g, goal_ne_g) path, cost = a_star_graph(G, heuristic, start_ne_g, goal_ne_g) print('path found from a_star') if self.if_pruning: # TODO: prune path to minimize number of waypoints # TODO (if you're feeling ambitious): Try a different approach altogether! pruned_path = prune_path(path) print('path found from a_star is pruned') # Convert path to waypoints # waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path] waypoints = [[ p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, np.arctan2((q[1] - p[1]), (q[0] - p[0])) ] for p, q in zip(pruned_path[:-1], pruned_path[1:])] else: waypoints = [[ p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0 ] for p in path] # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TAKEOFF_ALTITUDE = 5 SAFETY_DISTANCE = 3 # Set the takeoff altitude self.target_position[2] = TAKEOFF_ALTITUDE # Read lon0, lat0 from colliders into floating point values with open('colliders.csv', newline='') as f: reader = csv.reader(f) b = next(reader) home_lat = float(b[0].split(" ")[1]) home_long = float(b[1].split(" ")[2]) print("home longitude = {0}, home latitude = {1}".format( home_long, home_lat)) # Set home position to (lon0, lat0, 0) self.set_home_position(home_long, home_lat, 0) # Retrieve current global position global_start = self.global_position # Convert to current local position using global_to_local() local_start = global_to_local(global_start, self.global_home) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, local_start)) # Start position start_altitude = -1 * local_start[2] + TAKEOFF_ALTITUDE print("start altitude = ", start_altitude) start = [local_start[0], local_start[1], start_altitude] # Goal position goal = global_to_local(self.global_goal, self.global_home) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles grid, edges, polygons, north_offset, east_offset = create_grid_and_edges( data, min(start[2], goal[2]), SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # Convert start position to current position on grid grid_start = (start[0] - north_offset, start[1] - east_offset) # Convert goal position to grid location grid_goal = (goal[0] - north_offset, goal[1] - east_offset) print('Local Start and Goal: ', grid_start, grid_goal) # Build a 2D graph G = nx.Graph() for e in edges: p1 = e[0] p2 = e[1] dist = np.linalg.norm(np.array(p2) - np.array(p1)) G.add_edge(p1, p2, weight=dist) # Find closest nodes to start and goal on graph start_node = find_close_point(G, grid_start) goal_node = find_close_point(G, grid_goal) # Run A* to find a path from start to goal t0 = time.time() path, path_cost = a_star_graph(G, heuristic, start_node, goal_node) print("Time to find the path = ", time.time() - t0, "seconds") print("Length of path = ", len(path)) print("Cost of path = ", path_cost) # Prune the path in 2D using Bresenham algorithm t0 = time.time() smooth_path = prune_path_2d(path, grid) print("Time to prune the path= ", time.time() - t0, "seconds") smooth_path.append(grid_goal) # Add goal the list of path print("Length of pruned path = ", len(smooth_path)) # Find the inc/dec for the required in altitude for each consecutive waypoint alt_inc = ((self.global_goal[2] + SAFETY_DISTANCE) - start[2]) / (len(smooth_path) - 2) # Convert smooth_path to 3D waypoints temp_path = [[ int(smooth_path[i][0] + north_offset), int(smooth_path[i][1] + east_offset), int(start_altitude + i * alt_inc) ] for i in range(len(smooth_path) - 1)] # Add the goal point to the 3D list with altitude equal to previous waypoint. # So that drone can move from final waypoint to goal by moving at same altitude. temp_path.append([ int(smooth_path[-1][0] + north_offset), int(smooth_path[-1][1] + east_offset), temp_path[-1][2] ]) smooth_path = temp_path # Prune the path in 3D using 2.5D Map representation t0 = time.time() final_path = prune_path_3d(smooth_path, polygons) print("Time to prune the 3D path= ", time.time() - t0, "seconds") print("Length of pruned 3D path = ", len(final_path)) # Convert path to waypoints waypoints = [[final_path[i][0], final_path[i][1], final_path[i][2], 0] for i in range(len(final_path))] # Set self.waypoints self.waypoints = waypoints # Sends waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values with open('colliders.csv') as f: lat_str, lon_str = f.readline().split(',') lat, lon = float(lat_str.split(' ')[-1]), float( lon_str.split(' ')[-1]) print(lat, lon) # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon, lat, 0) # TODO: retrieve current global position current_global_position = np.array( [self._longitude, self._latitude, self._altitude]) # TODO: convert to current local position using global_to_local() #current_local_pos = global_to_local([self._longitude, self._latitude, self._altitude], self.global_home) current_local_pos = global_to_local(current_global_position, self.global_home) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles # grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) # print("North offset = {0}, east offset = {1}".format(north_offset, east_offset)) # Using graphs grid, edges, north_offset, east_offset = create_grid_and_edges( data, TARGET_ALTITUDE, SAFETY_DISTANCE) # create graph G = nx.Graph() for e in edges: p1 = e[0] p2 = e[1] dist = np.linalg.norm(np.array(p2) - np.array(p1)) G.add_edge(p1, p2, weight=dist) # Define starting point on the grid (this is just grid center) # grid_start = (-north_offset, -east_offset) # TODO: convert start position to current position rather than map center grid_start = (int(-north_offset + current_local_pos[0]), int(-east_offset + current_local_pos[1])) # Set goal as some arbitrary position on the grid # grid_goal = (-north_offset + 10, -east_offset + 10) # TODO: adapt to set goal as latitude / longitude position and convert goal_latlon = [-122.39207985, 37.79685038, TARGET_ALTITUDE] grid_goal_ne = global_to_local(goal_latlon, self.global_home) grid_goal = (int(grid_goal_ne[0]), int(grid_goal_ne[1])) # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) print('Local Start and Goal: ', grid_start, grid_goal) # path, _ = a_star(grid, heuristic, grid_start, grid_goal) start_ne_g = self.closest_point(G, grid_start) goal_ne_g = self.closest_point(G, grid_goal) home_g = self.closest_point(G, [-north_offset, -east_offset]) path, cost = graph_a_star(G, heuristic, start_ne_g, goal_ne_g) # TODO: prune path to minimize number of waypoints # TODO (if you're feeling ambitious): Try a different approach altogether! pruned_path = self.prune_path(path) # Convert path to waypoints # waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path] waypoints = [[ int(p[0] - home_g[0]), int(p[1] - home_g[1]), TARGET_ALTITUDE, 0 ] for p in pruned_path] # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 100 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values parts = open('colliders.csv').readlines()[0].split(',') lat0 = float(parts[0].strip().split(' ')[1]) lon0 = float(parts[1].strip().split(' ')[1]) # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position current_global_position = self.global_position # TODO: convert to current local position using global_to_local() current_local_position = global_to_local(current_global_position, self.global_home) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles grid, edges, north_offset, east_offset = create_grid_and_edges( data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # Define starting point on the grid (this is just grid center) grid_start = (-north_offset, -east_offset) # TODO: convert start position to current position rather than map center # grid_start = global_to_local(self.global_position, self.global_home) grid_start = (int(current_local_position[1]) - north_offset, int(current_local_position[0]) - east_offset) # Set goal as some arbitrary position on the grid grid_goal = (750, 370) # TODO: adapt to set goal as latitude / longitude position and convert lat_lon_grid_goal = local_to_global( (grid_goal[0], grid_goal[1], -TARGET_ALTITUDE), self.global_home) # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) print('Local Start and Goal: ', grid_start, grid_goal) (graph_start, graph_end) = find_start_goal_2d(edges, grid_start, grid_goal) print('Ajusted Start and Goal for a Graph', graph_start, graph_end) G = nx.Graph() for e in edges: p1 = e[0] p2 = e[1] dist = np.linalg.norm(np.array(p2) - np.array(p1)) G.add_edge(p1, p2, weight=dist) path, _ = a_star_2d_graph(G, heuristic, graph_start, graph_end) # TODO: prune path to minimize number of waypoints # TODO (if you're feeling ambitious): Try a different approach altogether! # Convert path to waypoints waypoints = [[ p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0 ] for p in path] # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 10 SAFETY_DISTANCE = 2 self.target_position[2] = TARGET_ALTITUDE # read lat0, lon0 from colliders into floating point values first_line = open('colliders.csv').readline() lat_lon = dict(val.strip().split(' ') for val in first_line.split(',')) # set home position to (lon0, lat0, 0) self.set_home_position(float(lat_lon['lon0']), float(lat_lon['lat0']), 0) # retrieve current global position # convert to current local position using global_to_local() local_position = global_to_local(self.global_position, self.global_home) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles # Create Voronoi graph from obstacles grid, edges, north_offset, east_offset = create_grid_and_edges( data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # Define starting point on the grid # convert start position to current position rather than map center grid_start = (-north_offset + int(local_position[0]), -east_offset + int(local_position[1])) # Set goal as some arbitrary position on the grid # adapt to set goal as latitude / longitude position and convert # some possible destinations on the map destinations = [[-122.39324423, 37.79607305], [-122.39489652, 37.79124152], [-122.40059743, 37.79272177], [-122.39625334, 37.79478161]] # randomly choose one of destinations destination = destinations[np.random.randint(0, len(destinations))] destination_local = global_to_local( [destination[0], destination[1], TARGET_ALTITUDE], self.global_home) # adjust destination coordinates on the grid grid_goal = (-north_offset + int(destination_local[0]), -east_offset + int(destination_local[1])) print('Local Start and Goal: ', grid_start, grid_goal) # create Voronoi graph with the weight of the edges # set to the Euclidean distance between the points voronoi_graph = nx.Graph() for e in edges: p1 = e[0] p2 = e[1] dist = np.linalg.norm(np.array(p2) - np.array(p1)) voronoi_graph.add_edge(p1, p2, weight=dist) # find the nearest points on the graph for start and the goal voronoi_start = nearest_point(grid_start, voronoi_graph) voronoi_finish = nearest_point(grid_goal, voronoi_graph) # run A-star on the graph voronoi_path, voronoi_cost = a_star_graph(voronoi_graph, heuristic, voronoi_start, voronoi_finish) voronoi_path.append(grid_goal) # prune path - from Lesson 6.5 print('Original path len: ', len(voronoi_path)) voronoi_path = prune_path(voronoi_path) print('Pruned path len: ', len(voronoi_path)) # Convert path to waypoints waypoints = [[ int(p[0]) + north_offset, int(p[1]) + east_offset, TARGET_ALTITUDE, 0 ] for p in voronoi_path] # Set self.waypoints self.waypoints = waypoints # send waypoints to sim self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 2 SAFETY_DISTANCE = 1 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values lat0, lon0 = read_home_lat_lon("colliders.csv") # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position current_global_position = self.global_position # TODO: convert to current local position using global_to_local() current_local_position = global_to_local(current_global_position, self.global_home) print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles grid, edges, north_offset, east_offset = create_grid_and_edges(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format(north_offset, east_offset)) # Define starting point on the grid (this is just grid center) grid_start = (-north_offset, -east_offset) # TODO: convert start position to current position rather than map center start = (int(np.ceil(current_local_position[0]) + grid_start[0]), int(np.ceil(current_local_position[1]) + grid_start[1])) # Set goal as some arbitrary position on the grid #goal = (-north_offset + 10, -east_offset + 10) # TODO: adapt to set goal as latitude / longitude position and convert goal_lat = 37.796793 goal_lon = -122.397182 goal_global = [goal_lon, goal_lat, 0.0] goal_local = global_to_local(goal_global, self.global_home) goal = (int(np.ceil(goal_local[0])) - north_offset, int(np.ceil(goal_local[1])) - east_offset) print("grid_goal", goal) # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) print('Local Start and Goal: ', start, goal) G = create_nx_graph(edges) start_node = find_closest_point(G, start) goal_node = find_closest_point(G, goal) path = a_star_graph(G, heuristic, start_node, goal_node) # TODO: prune path to minimize number of waypoints pruned_path = prune_path_colinearity(grid, path) # Prune the path in 2D #smooth_path = prune_path_2d(path, grid) # TODO (if you're feeling ambitious): Try a different approach altogether! ## visualisation #plt.imshow(grid, cmap='Greys', origin='lower') #plt.plot(start[1], start[0], 'x') #plt.plot(goal[1], goal[0], 'x') #p = np.array(path) #pp = np.array(pruned_path) #ppc = np.array(pruned_path_col) #plt.plot(p[:,1], p[:,0], 'r') #plt.plot(pp[:,1], pp[:,0], 'g') #plt.plot(ppc[:,1], ppc[:,0], 'b') #plt.xlabel('EAST') #plt.ylabel('NORTH') #plt.show() # Convert path to waypoints waypoints = [[int(p[0]) + north_offset, int(p[1]) + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path] with open("wp.p", mode="wb") as f: pickle.dump(waypoints, f) with open("wp.p", mode="rb") as f: wp = pickle.load(f) # Set self.waypoints self.waypoints = wp # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # Read lat0, lon0 from colliders into floating point values lat0 = lon0 = None with open('colliders.csv') as f: ns = re.findall("-*\d+\.\d+", f.readline()) assert len( ns ) == 2, "Could not parse lat0 & lon0 from 'colliders.csv'. The file might be broken." lat0, lon0 = float(ns[0]), float(ns[1]) self.set_home_position( lon0, lat0, 0) # set home position as stated in colliders.csv # curLocal - is local position of drone relative home) curLocal = global_to_local( (self._longitude, self._latitude, self._altitude), self.global_home) print('global home {0}'.format(self.global_home.tolist())) print('position {0}'.format(self.global_position.tolist())) print('local position {0}'.format(self.local_position.tolist())) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles timer = time.time() grid, offset, edges = create_grid_and_edges(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print('Voronoi {0} edges created in {1}s'.format( len(edges), time.time() - timer)) print('Offsets: north = {0} east = {1}'.format(offset[0], offset[1])) G = nx.Graph() for e in edges: G.add_edge(tuple(e[0]), tuple(e[1]), weight=np.linalg.norm(np.array(e[1]) - np.array(e[0]))) print('Graph created') # Define starting point on the grid as current location grid_start = (-offset[0] + int(curLocal[0]), -offset[1] + int(curLocal[1])) # Set goal as some arbitrary position on the grid # grid_goal = (-offset[0] + 64, -offset[1] + 85) # grid_goal = (290, 720) if self.goal is None: grid_goal = None while not grid_goal: i, j = random.randint(0, grid.shape[0] - 1), random.randint( 0, grid.shape[1] - 1) if not grid[i, j]: grid_goal = (i, j) else: target_global = global_to_local( (self.goal[1], self.goal[0], TARGET_ALTITUDE), self.global_home) grid_goal = (-offset[0] + int(target_global[0]), -offset[1] + int(target_global[1])) print('Path: {0} -> {1}'.format(grid_start, grid_goal)) graph_start = closest_point(G, grid_start) graph_goal = closest_point(G, grid_goal) print('Graph path: {0} -> {1}'.format( (graph_start[0] - offset[0], graph_start[1] - offset[1]), (graph_goal[0] - offset[0], graph_goal[1] - offset[1]))) timer = time.time() path, cost = a_star_graph(G, graph_start, graph_goal) if path is None: print('Could not find path') return print('Found path on graph of {0} waypoints in {1}s'.format( len(path), time.time() - timer)) # Add to path exact (non-voronoi) start&goal waypoints path = [(int(p[0]), int(p[1])) for p in path] path.insert(0, grid_start) path.insert(len(path), grid_goal) # Prune the path on grid twice timer = time.time() pruned_path = prune_path(path, grid) pruned_path = prune_path(pruned_path, grid) print('Pruned path from {0} to {1} waypoints in {2}s'.format( len(path), len(pruned_path), time.time() - timer)) path = pruned_path # Convert path to waypoints waypoints = [[p[0] + offset[0], p[1] + offset[1], TARGET_ALTITUDE, 0] for p in path] # Set self.waypoints self.waypoints = waypoints self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values with open('colliders.csv') as colliders: position_line = colliders.readline().strip() position_line = position_line.split(',') position_lat = position_line[0].split(' ')[1] position_lon = position_line[1].strip().split(' ')[ 1] # strip needed due to leading space. # TODO: set home position to (lon0, lat0, 0) self.set_home_position(float(position_lon), float(position_lat), 0) # TODO: retrieve current global position global_position = [self._longitude, self._latitude, self._altitude] # TODO: convert to current local position using global_to_local() local_position = global_to_local(global_position, self.global_home) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles grid, north_offset, east_offset, edges = create_grid_and_edges( data, TARGET_ALTITUDE, SAFETY_DISTANCE) graph = nx.Graph() for e in edges: p1 = e[0] p2 = e[1] dist = LA.norm(np.array(p2) - np.array(p1)) graph.add_edge(p1, p2, weight=dist) print("These are the grid and edges", grid, edges) # Define starting point on the grid (this is just grid center) grid_start = (int(local_position[0] - north_offset), int(local_position[1] - east_offset)) graph_start = closest_point(graph, grid_start) print("Grid start and graph start", grid_start, graph_start) # TODO: convert start position to current position rather than map center # done above # Set goal as some arbitrary position on the grid grid_goal = (120, 610) graph_goal = closest_point(graph, grid_goal) print("Grid start and graph goal", grid_goal, graph_goal) # TODO: adapt to set goal as latitude / longitude position and convert # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) # both done print('Local Start and Goal: ', grid_start, grid_goal) path, cost = a_star(graph, heuristic, graph_start, graph_goal) print('The path is', path, cost) # TODO: prune path to minimize number of waypoints # print('Path before pruning', len(path)) # path = prune_path(path) # print('Path after pruning', len(path)) # No need using a graph # TODO (if you're feeling ambitious): Try a different approach altogether! # done! # Convert path to waypoints waypoints = [[ int(p[0] + north_offset), int(p[1] + east_offset), TARGET_ALTITUDE, 0 ] for p in path] # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): print("searching for a path ...") print("goal lon = {0}, lat = {1}".format(self.goal_lon, self.goal_lat)) print("algorithm = {0}".format(self.algorithm)) path = [] north_offset = 0 east_offset = 0 print('global home: {0}, position: {1}, local position: {2}'.format( self.global_home, self.global_position, self.local_position)) if self.algorithm == PathAlgorithm.Simple: grid, north_offset, east_offset = create_grid( self.data, self.default_altitude, self.safety_distance) print("north offset = {0}, east offset = {1}".format( north_offset, east_offset)) # Define starting point on the grid (this is just grid center) # Set goal as some arbitrary position on the grid grid_start = self.to_grid_ne(self.start_lon, self.start_lat, north_offset, east_offset) grid_goal = self.to_grid_ne(self.goal_lon, self.goal_lat, north_offset, east_offset) print("grid start = {0}, goal = {1}".format(grid_start, grid_goal)) path, _ = a_star(grid, heuristic, grid_start, grid_goal) print("count of waypoints: ", len(path)) path = prune_waypoints(path) print("count of waypoints after prune: ", len(path)) if self.plot_maps: plot_map(grid, path, grid_start, grid_goal) elif self.algorithm == PathAlgorithm.Skeleton: grid, north_offset, east_offset = create_grid( self.data, self.default_altitude, self.safety_distance) print("north offset = {0}, east offset = {1}".format( north_offset, east_offset)) # Define starting point on the grid (this is just grid center) # Set goal as some arbitrary position on the grid grid_start = self.to_grid_ne(self.start_lon, self.start_lat, north_offset, east_offset) grid_goal = self.to_grid_ne(self.goal_lon, self.goal_lat, north_offset, east_offset) print("grid start = {0}, goal = {1}".format(grid_start, grid_goal)) skeleton = medial_axis(invert(grid)) skeleton_start, skeleton_goal = find_start_goal( skeleton, grid_start, grid_goal) print("skeleton start = {0}, goal = {1}".format( skeleton_start, skeleton_goal)) path, _ = a_star_skeleton( invert(skeleton).astype(np.int), heuristic, tuple(skeleton_start), tuple(skeleton_goal)) print("count of waypoints: ", len(path)) path = prune_waypoints(path) print("count of waypoints after prune: ", len(path)) if self.plot_maps: plot_map_skeleton(grid, skeleton, path, grid_start, grid_goal) elif self.algorithm == PathAlgorithm.Graphs: grid, edges, north_offset, east_offset = create_grid_and_edges( self.data, self.default_altitude, self.safety_distance) print("north offset = {0}, east offset = {1}".format( north_offset, east_offset)) start_ne = self.to_grid_ne(self.start_lon, self.start_lat, north_offset, east_offset) goal_ne = self.to_grid_ne(self.goal_lon, self.goal_lat, north_offset, east_offset) print("grid start_ne = {0}, goal_ne = {1}".format( start_ne, goal_ne)) g = nx.Graph() for e in edges: p1 = e[0] p2 = e[1] dist = la.norm(np.array(p2) - np.array(p1)) g.add_edge(p1, p2, weight=dist) start_ne_g = closest_point(g, start_ne) goal_ne_g = closest_point(g, goal_ne) path, cost = a_star_graph(g, heuristic, start_ne_g, goal_ne_g) print("count of waypoints: ", len(path)) path = prune_waypoints(path) print("count of waypoints after prune: ", len(path)) if self.plot_maps: plot_map_graph(grid, edges, path, start_ne, goal_ne) # Convert path to waypoints waypoints = [[ int(p[0]) + north_offset, int(p[1]) + east_offset, self.default_altitude, 0 ] for p in path] self.waypoints = waypoints self.send_waypoints() self.flight_state = States.PLANNING