def GetPath(self, north_offset, east_offset, grid, graph, start, goal): if (grid[goal[0], goal[1]] == 1): print("Goal is unreachable") return None start_g = pu.closest_point(graph, start) goal_g = pu.closest_point(graph, goal) print(goal_g) path, _ = pu.a_starGraph(graph, start_g, goal_g) # If it is not possible to find a path in tha graph, let's find a path in the grid if (path is None): print('Searching in grid') newpath, _ = pu.a_star(grid, start, goal) newpath = pu.prune_path(newpath) else: start_g = (int(start_g[0]), int(start_g[1])) goal_g = (int(goal_g[0]), int(goal_g[1])) print('start={0}, start_g={1}'.format(start, start_g)) pathToBegin, _ = pu.a_star(grid, start, start_g) pathFromEnd, _ = pu.a_star(grid, goal_g, goal) if (pathToBegin is None or pathFromEnd is None): return None newpath = pathToBegin + path + pathFromEnd newpath = pu.prune_path(newpath) return [[ int(p[0]) + north_offset, int(p[1]) + east_offset, pu.TARGET_ALTITUDE, 0 ] for p in newpath]
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()
for iteration in range(1): print('> iteration #{0}'.format(iteration)) def findRandomFreePosition(): p = None while not p: i, j = random.randint(0, grid.shape[0] - 1), random.randint( 0, grid.shape[1] - 1) if not grid[i, j]: p = (i, j) return p # grid_start = findRandomFreePosition() # grid_goal = findRandomFreePosition() # Find closest points in voronoi graph graph_start = closest_point(G, grid_start) graph_goal = closest_point(G, grid_goal) print('Graph path: {0} -> {1}'.format(graph_start, graph_goal)) # Find path on graph timer = time.time() path, cost = a_star_graph(G, graph_start, graph_goal) if path is None: continue print('Found path on graph of {0} waypoints in {1}s'.format( len(path), time.time() - timer)) # Add to path 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)
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5.0 SAFETY_DISTANCE = 5.0 self.target_position[2] = -self.local_position[2] + TARGET_ALTITUDE # Read lat0, lon0 from colliders into floating point values with open('colliders.csv') as f: first_line = f.readline() lat0 = float(first_line.split(',')[0].strip().split(' ')[1]) lon0 = float(first_line.split(',')[1].strip().split(' ')[1]) # Set home position to (lat0, lon0, 0) self.set_home_position(lon0, lat0, 0.0) # Retrieve current global position # global_position = (self._longitude, self._latitude, self._altitude) # Convert current global position (lon, lat, up) to a local position (north, east, down) # local_position = global_to_local(global_position, self.global_home) print('global home (lat, lon, alt) {}'.format(self.global_home)) print('global position (lat, lon, alt) {}'.format( self.global_position)) print('local position (nor, est, dwn) {}'.format(self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # # Create Voronoi Graph for a particular altitude and safety margin around obstacles # # Only need to run once, save the graph to a file. Next time just load graph file # t1 = time.time() # edges= create_voronoi(data, TARGET_ALTITUDE, SAFETY_DISTANCE) # t_edges = time.time() - t1 # print("It takes {} seconds to create voronoi graph".format(np.round(t_edges,2))) # t2 = time.time() # 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) # t_graph = time.time() - t2 # print("It takes {} seconds to create load edges to networkx graph".format(np.round(t_graph,2))) # nx.write_gpickle(G, "voronoi.gpickle") G = nx.read_gpickle("voronoi.gpickle") # Convert start position to current position rather than map center north_offset, east_offset, _, _ = calc_offset_gridsize(data) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) grid_start = (int(self.local_position[0]) - north_offset, int(self.local_position[1]) - east_offset) # Adapt to set goal as latitude / longitude position and convert # goal_global = (-122.400835, 37.795407, 0.0) # goal_global = (-122.4003, 37.7953, 34.0) goal_local = global_to_local(goal_global, self.global_home) grid_goal = (int(goal_local[0]) - north_offset, int(goal_local[1]) - east_offset) print('Local Start: ', grid_start) print('Local Goal: ', grid_goal) start_ne_g = closest_point(G, grid_start) goal_ne_g = closest_point(G, grid_goal) print("Start localtion on graph:", start_ne_g) print("Goal localtion on graph:", goal_ne_g) goal_g_local = (goal_ne_g[0] + north_offset, goal_ne_g[1] + east_offset, 0) goal_g_global = local_to_global(goal_g_local, self.global_home) print("Goal global location:", goal_g_global) # A* graph search for a path t3 = time.time() path, _ = a_star_graph(G, heuristic, start_ne_g, goal_ne_g) t_search = time.time() - t3 print("It takes {} seconds to find a path".format(np.round( t_search, 2))) # prune path to minimize number of waypoints print(len(path), "waypoints before pruning") path = prune_path(path) print(len(path), "waypoints after pruning") # calculate waypoints altitude path_tmp = np.array([path[0]] + path) dist = np.linalg.norm(path_tmp[1:, :] - path_tmp[:-1, :], axis=1) goal_alt = goal_global[2] heights = np.cumsum(dist / sum(dist)) * goal_alt + TARGET_ALTITUDE # calculate waypoints heading # Set heading based on next waypoint position relative previous waypoint position headings = np.arctan2(path_tmp[1:, 1] - path_tmp[:-1, 1], path_tmp[1:, 0] - path_tmp[:-1, 0]) # Convert path to waypoints waypoints = [[p[0] + north_offset, p[1] + east_offset, alt, heading] for p, alt, heading in zip(path, heights, headings)] takeoff_point = [ grid_start[0] + north_offset, grid_start[1] + east_offset, int(-self.local_position[2]), waypoints[0][3] ] waypoints.insert(0, takeoff_point) landing_point = [ grid_goal[0] + north_offset, grid_goal[1] + east_offset, goal_alt + TARGET_ALTITUDE, waypoints[-1][3] ] waypoints.append(landing_point) waypoints = [[ int(waypoint[0]), int(waypoint[1]), int(waypoint[2]), waypoint[3] ] for waypoint in waypoints] print(waypoints) # 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 = 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 lat0, lon0 = np.loadtxt('colliders.csv', delimiter=',', dtype='str', usecols=(0, 1))[0] lat0 = float(lat0.split()[1]) lon0 = float(lon0.split()[1]) # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) global_home = self.global_home # TODO: retrieve current global position global_position = self.global_position # 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)) local_position = global_to_local(global_position, 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, north_offset, east_offset = create_grid_graph( 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 = (local_position[0] - north_offset, local_position[1] - east_offset) # 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_global_position = (-122.39827005, 37.79639587, 0) goal_local_position = global_to_local(goal_global_position, global_home) grid_goal = (int(goal_local_position[0] - north_offset), int(goal_local_position[1] - east_offset)) print('Local Start and Goal: ', grid_start, grid_goal) # Add weight to graph G = nx.Graph() for e in edges: p1 = tuple(e[0]) p2 = tuple(e[1]) dist = LA.norm(np.array(p2) - np.array(p1)) G.add_edge(p1, p2, weight=dist) # Find the closest point in the graph to our current location, same thing for the goal location. start_g = closest_point(G, grid_start) goal_g = closest_point(G, grid_goal) print('Local Start and Goal in graph: ', start_g, goal_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(G, heuristic, start_g, goal_g) # TODO: prune path to minimize number of waypoints # TODO (if you're feeling ambitious): Try a different approach altogether! pruned_path = prune_path(path) pruned_path.append(grid_goal) pruned_path.insert(0, grid_start) # Convert path to waypoints waypoints = [[ int(p[0] + north_offset), int(p[1] + east_offset), TARGET_ALTITUDE, 0 ] for p in pruned_path] # waypoints = [[0, 0, 5, 0], [0, 1, 5, 0], [1, 1, 5, 0], [1, 2, 5, 0], [2, 2, 5, 0], [2, 3, 5, 0], [3, 3, 5, 0], [3, 4, 5, 0], [4, 4, 5, 0], [4, 5, 5, 0], [5, 5, 5, 0], [5, 6, 5, 0], [6, 6, 5, 0], [6, 7, 5, 0], [7, 7, 5, 0], [7, 8, 5, 0], [8, 8, 5, 0], [8, 9, 5, 0], [9, 9, 5, 0], [9, 10, 5, 0], [10, 10, 5, 0]] # waypoints = [[10, 10, 5, 0], [7, 0, 5, 0], [26, 14, 5, 0], [32, 17, 5, 0], [34, 15, 5, 0], [44, 5, 5, 0], [54, -1, 5, 0], [64, -6, 5, 0], [84, -11, 5, 0], [93, -15, 5, 0], [94, -16, 5, 0], [98, -17, 5, 0], [104, -24, 5, 0], [129, -39, 5, 0], [149, -19, 5, 0], [154, -16, 5, 0], [174, -26, 5, 0], [184, -29, 5, 0], [204, -29, 5, 0], [204, -29, 5, 0], [212, -29, 5, 0], [214, -30, 5, 0], [224, -34, 5, 0], [244, -34, 5, 0], [254, -37, 5, 0], [258, -40, 5, 0], [274, -44, 5, 0], [279, -41, 5, 0], [294, -44, 5, 0], [314, -44, 5, 0], [324, -46, 5, 0], [327, -45, 5, 0], [344, -49, 5, 0], [362, -49, 5, 0], [364, -50, 5, 0], [374, -54, 5, 0], [397, -54, 5, 0], [404, -62, 5, 0], [407, -67, 5, 0], [409, -74, 5, 0], [412, -81, 5, 0], [428, -82, 5, 0], [432, -80, 5, 0], [434, -75, 5, 0]] # Set self.waypoints self.waypoints = waypoints print(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 = 7 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]))) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) sampler = Sampler(data) polygons = sampler._polygons # Example: sampling 1200 points and removing # ones conflicting with obstacles. nodes = sampler.sample(1200) print("Non collider nodes:", len(nodes)) t0 = time.time() # Uncomment line 162 to generate the graph from the sampled points, and comment out line 163. # Increase the Mavlink timer to avoid disconnecting from the simulator. #G = create_graph(nodes, 10, polygons) G = nx.read_gpickle( "graph_1200_SD_nodes.gpickle" ) # Comment out this line if generating the graph instead of using the saved graph print('graph took {0} seconds to build'.format(time.time() - t0)) print("Number of edges", len(G.edges)) # 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), TARGET_ALTITUDE) # TODO: adapt to set goal as latitude / longitude position and convert goal_global_pos = (-122.394700, 37.789825, 13) 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_global_pos[2]) print("goal_local_N:", goal_local_pos[0], "goal_local_E:", goal_local_pos[1], "goal_local_alt:", goal_local_pos[2]) #goal_ne = (455., 635., 20.) 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) # Run A* to find a path from start to goal 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]), int(p[2])] 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)) print("PRUNED PATH:", pruned_path) # Convert path to waypoints waypoints = [[p[0] + north_offset, p[1] + east_offset, p[2], 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 = 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 latitude, longitude = self.get_home_position() # TODO: set home position to (lon0, lat0, 0) self.set_home_position(longitude, latitude, 0.0) # TODO: retrieve current global position # TODO: convert to current local position using global_to_local() current_global_position = np.array( [self._longitude, self._latitude, self._altitude]) self._north, self._east, self._down = global_to_local( current_global_position, self.global_home) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Get a graph representation with edges for a particular altitude and safety margin around obstacles graph, north_offset, east_offset = create_graph( data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # TODO: convert start position to current position rather than map center current_position = self.local_position start = (-north_offset + int(current_position[0]), -east_offset + int(current_position[1])) # Set goal as some arbitrary position on the grid # TODO: adapt to set goal as latitude / longitude position and convert goal_longitude = -122.397745 goal_latitude = 37.793837 #goal_longitude = -122.394324 #goal_latitude = 37.791684 #goal_longitude = -122.401216 #goal_latitude = 37.796713 target_location = global_to_local([goal_longitude, goal_latitude, 0], self.global_home) goal = (int(-north_offset + target_location[0]), int(-east_offset + target_location[1])) # Compute the closest points to the start and goal positions closest_start = closest_point( graph, (-north_offset + int(current_position[0]), -east_offset + int(current_position[1]))) closest_goal = closest_point(graph, (-north_offset + int(target_location[0]), -east_offset + int(target_location[1]))) print("Local Start and Goal : ", start, goal) # Run A* to find a path from start to goal path, _ = a_star_graph(graph, closest_start, closest_goal) print("Found a path of length : ", len(path)) # Check for collinearity between points and prune the obtained path path = prune_path(path) # Convert path to waypoints self.waypoints = [[ p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0 ] for p in path] # TODO: send waypoints to simulator print("Sending waypoints to the simulator") self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 RANDOM_START = False GRAPH_SEARCH = False self.target_position[2] = TARGET_ALTITUDE # Read lat0, lon0 from colliders into floating point values with open('colliders.csv') as f: line = f.readline() latLonArray = map(lambda x: x.strip(), line.split(',')) lat0, lon0 = map(lambda x: float(x.split(' ')[1]), latLonArray) # Set the home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # Retrieve current global position current_global_position = np.array( [self._longitude, self._latitude, self._altitude]) # 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, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) if (RANDOM_START): random_start_pos = self.randomPoint(grid, north_offset, east_offset, 0, self.global_home) self.set_home_position(random_start_pos[1], random_start_pos[0], 0) current_global_position = np.array( [self._longitude, self._latitude, self._altitude]) current_local_position = global_to_local(current_global_position, self.global_home) # Convert start position to current position grid_start = (int(current_local_position[0] - north_offset), int(current_local_position[1]) - east_offset) # Random Start if (RANDOM_START): local_start = global_to_local(random_start_pos, self.global_home) grid_start = (int(local_start[0]) - north_offset, int(local_start[1]) - east_offset) # Set goal as latitude / longitude position: np.array([-122.39747, 37.79275, TARGET_ALTITUDE]) goal_pos = self.randomPoint(grid, north_offset, east_offset, TARGET_ALTITUDE, self.global_home) # Convert lat/lon to local grid: (325, 455) local_goal = global_to_local(goal_pos, self.global_home) grid_goal = (int(local_goal[0]) - north_offset, int(local_goal[1]) - east_offset) print('Local Start and Goal: ', grid_start, grid_goal) if not GRAPH_SEARCH: # Run A* to find a path from start to goal path, _ = a_star(grid, heuristic, grid_start, grid_goal) # Prune path to minimize number of waypoints pruned_path = [p for p in path] i = 1 while i < len(pruned_path) - 1: pt1 = pruned_path[i - 1] pt2 = pruned_path[i] pt3 = pruned_path[i + 1] if self.collinearity_check(pt1, pt2, pt3): del pruned_path[i:i + 1] else: i = i + 1 # Try a different approach altogether! - Probabilistic Graph if (GRAPH_SEARCH): # Get samples and polygons samples = random_samples(data, 200) polygons = extract_polygons(data) to_keep = [] for point in samples: if not collides(polygons, point): to_keep.append(point) # # Create the graph and determine start and goal points on graph graph = create_graph(to_keep, 10, polygons) print(graph) graph_start = closest_point(graph, current_local_position) graph_goal = closest_point(graph, goal_pos) # Run A* path, _ = a_star_graph(graph, heuristic, graph_start, graph_goal) # Add original start and goal path.append(local_goal) path.insert(0, current_local_position) # If no path found, just takeoff and land if len(path) == 2: path.pop() # Adjust for offset - should refactor pruned_path = [[int(p[0]) - north_offset, int(p[1]) - east_offset] for p in 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 # 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 = 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()
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 ...") TARGET_ALTITUDE = 10 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values filename = 'colliders.csv' data = np.genfromtxt(filename, delimiter=',', dtype='str', max_rows=1) lat0 = float(data[0].split(' ')[-1]) lon0 = float(data[1].split(' ')[-1]) # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0.0) # TODO: retrieve current global position # 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(filename, 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, 0, 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 = self.global_position # TODO: convert start position to current position rather than map center grid_start = self.global_to_map( grid_start, grid_start, grid.shape, north_offset, east_offset, int(np.ceil(TARGET_ALTITUDE - self.local_position[2]))) # Set goal as some arbitrary position on the grid # generate a random goal x_min = 0 x_max = grid.shape[0] y_min = 0 y_max = grid.shape[1] grid_goal = random_goal(grid, x_min, x_max, y_min, y_max) # If a goal is input manually, use the global variable GOAL_COORD in (long, lat) # grid_goal = GOAL_COORD # grid_goal = self.global_to_map(grid_goal, self.global_position, grid.shape, north_offset, east_offset, 60) # 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) # Use Voronoi graph # load a graph g = load_graph('map.gpickle') print("Number of edges", len(g.edges)) # find closest points for start and goal start_closest = closest_point(g, grid_start) goal_closest = closest_point(g, grid_goal) # a-star. If a path cannot be found, add graph points and paths around start/goal. path_found = False for i in range(3): path, cost, path_found = a_star(g, heuristic, start_closest, goal_closest) if path_found: break g = random_graph_pts(grid, g, grid_start, samples=100) g = random_graph_pts(grid, g, grid_goal, samples=100) start_closest = closest_point(g, grid_start) goal_closest = closest_point(g, grid_goal) # TODO: prune path to minimize number of waypoints pruned_path = prune_path(path, grid) print('unpruned_path: ', path, type(path)) print('pruned_path: ', pruned_path, type(pruned_path)) # append destination to path, if destination is more than 1 meter away from last waypoint if LA.norm(np.array(pruned_path[-1]) - np.array(grid_goal)) > 1.0: pruned_path.append((grid_goal[0], grid_goal[1], grid_goal[2])) # assign a heading at each waypoint based on direction of next waypoint path_with_heading = get_heading(pruned_path) # Convert path to waypoints waypoints = [[p[0] + north_offset, p[1] + east_offset, p[2], p[3]] for p in path_with_heading] # 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 = int(self.goal_position[2] * 1.1) + 5 SAFETY_DISTANCE = 5 # Assuming home is always set at 0 self.target_position[2] = self.goal_position[2] init_pos = self.get_home_coordinates() self.set_home_position(init_pos[1], init_pos[0], 0) local_position = global_to_local(self.global_position, self.global_home) goal_position = global_to_local(self.goal_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) graph = None north_offset = None east_offset = None alt_dim = False if(self.probabilistic_mode): alt_dim = True print('********************************************') print(" Using probabalistic mode") print('********************************************') t0 = time.time() graph = create_graph_probabilistic(data, TARGET_ALTITUDE, int(SAFETY_DISTANCE / 2)) print('graph took {0} seconds to build'.format(time.time() - t0)) north_offset, east_offset = get_min_north_east(data) else: print('********************************************') print(" Using graph with voronoi") print('********************************************') graph, north_offset, east_offset = create_graph_voronoi(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print(north_offset, east_offset) graph_start = self.get_relative_coords(local_position, north_offset, east_offset, alt_dim) graph_goal = self.get_relative_coords(goal_position, north_offset, east_offset, alt_dim) # Get closest points on the graph graph_start_c = closest_point(graph, graph_start) graph_goal_c = closest_point(graph, graph_goal) # Create Graph # Run A* to find a path from start to goal print('Local Start and Goal and closest points: ', graph_start, graph_start_c, graph_goal, graph_goal_c) path, _ = a_star(graph, heuristic, graph_start_c, graph_goal_c) # Convert path to waypoints waypoints = [ [ graph_start[0] + north_offset, graph_start[1] + east_offset, TARGET_ALTITUDE, 0 ] ] waypoints = waypoints + [[ int(np.ceil(p[0] + north_offset)), int(np.ceil(p[1] + east_offset)), TARGET_ALTITUDE, 0] for p in path] waypoints = waypoints + [ [ int(np.ceil(graph_goal[0] + north_offset)), int(np.ceil(graph_goal[1] + east_offset)), TARGET_ALTITUDE, 0 ] ] # 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 # read the first line and extract only the latitude and longitude vals with open('colliders.csv', 'r') as f: temp = f.readline() lat, lon = temp.replace('lat0 ', '').replace('lon0', '').split(', ') lat, lon = float(lat), float(lon) # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon, lat, 0) # TODO: retrieve current global position curr_global_pos = self.global_position # TODO: convert to current local position using global_to_local() curr_local_pos = global_to_local(curr_global_pos, self.global_home) print("Current Local position {}".format(curr_local_pos)) # start_local = int(curr_local_pos[0]), int(curr_local_pos[1]) #int(start_local[0]), int(start_local[1]) # print(start_local, end_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) # 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 = start_local # # TODO: convert start position to current position rather than map center grid_start = (int(curr_local_pos[0]-north_offset), int(curr_local_pos[1]-east_offset)) # # Set goal as some arbitrary position on the grid # grid_goal = end_local # TODO: adapt to set goal as latitude / longitude position and convert end_geo = self.goal end_local = global_to_local(end_geo, self.global_home) grid_goal = end_local[0]-north_offset, end_local[1]-east_offset grid_goal = int(grid_goal[0]), int(grid_goal[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('Grid Start and Goal: ', grid_start, grid_goal) # TODO (if you're feeling ambitious): Try a different approach altogether! if self.planner == 1: path, _ = a_star(grid, heuristic, grid_start, grid_goal) # TODO: prune path to minimize number of waypoints path = prune_path(path) else: if self.planner == 2: sampler = Sampler(data) polygons = sampler._polygons # Example: sampling 100 points and removing # ones conflicting with obstacles. nodes = sampler.sample(300) print(nodes[0]) elif self.planner == 3: pass elif self.planner == 4: pass #create the graph and calculate the a_star t0 = time.time() print('building graph ... ', ) g = create_graph(nodes, 10, polygons) print('graph took {0} seconds to build'.format(time.time()-t0)) start = closest_point(g, (grid_start[0], grid_start[1], TARGET_ALTITUDE)) goal = closest_point(g, (grid_goal[0], grid_goal[1], TARGET_ALTITUDE)) print(start, goal) # print(start, start_ne) print('finding path ... ', ) path, cost = a_star_graph(g, heuristic, start, goal) print('done. path size and cost: {}'.format((len(path), cost))) # print(len(path), path) # path_pairs = zip(path[:-1], path[1:]) # Convert path to waypoints waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path] print(waypoints) # 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 = 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()