def get_path(start=(0, 0, 6), goal=(331, 116, 19)): data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) north_offset = -316 east_offset = -445 sampler = Sampler(data) print("AAA") polygons = sampler._polygons print("BBB") nodes = sampler.sample(3000) # (339, 380) (441, 519) # start = (0, 0, 6) # goal = (331, 116, 19) nodes += [start, goal] print(nodes) g = create_graph(nodes, k=10, polygons=polygons) path, _ = a_star_graph(g, heuristic, start, goal) waypoints = [[int(p[0]), int(p[1]), int(p[2]), 0] for p in path] path_dict = {'path': waypoints} print(waypoints) with open('path.json', 'w') as json_file: json.dump(path_dict, json_file) with open('path.json', 'r') as json_file2: dta = json.load(json_file2) print(dta['path']) return 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): 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
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.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 # 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 = 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 ...") self.target_position[2] = self.TARGET_ALTITUDE lat0 = '' lon0 = '' with open('colliders.csv', newline='') as c: reader = csv.reader(c) lat0, lon0 = next(reader) split = lat0.split() split1 = lon0.split() lat0, lon0 = float(split[1]), float(split1[1]) self.set_home_position(lon0, lat0, 0) print('latitude {0}, longitude {1}'.format(self._latitude, self._longitude)) local_position = global_to_local(self.global_position, self.global_home) print(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, self.TARGET_ALTITUDE, self.SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format(north_offset, east_offset)) start = (int(local_position[0]), int(local_position[1]), self.TARGET_ALTITUDE) #create graph representation self.obstacles = extract_polygons(data, self.SAFETY_DISTANCE) samples = create_samples(data, 400) X = [] for p in self.obstacles: X.append(p[0].bounds) poly_tree = KDTree(X) to_keep = [] for point in samples: if not collides(self.obstacles, point, poly_tree): to_keep.append(point) start_adjusted = start i = 0 while collides(self.obstacles, start_adjusted, poly_tree) and i < len(self.adjustments): print('collision with start point!', i, len(self.adjustments) - 1) start_adjusted = start start_adjusted = (start_adjusted[0] + self.adjustments[i][0],start_adjusted[1] + self.adjustments[i][1], start_adjusted[2] + self.adjustments[i][2]) i += 1 to_keep.append(start_adjusted) print('before collision check', len(samples), 'after collision check', len(to_keep)) print('number of obstacles', len(self.obstacles)) # for choosing a random point as the goal # idx = np.random.randint(0,len(to_keep)-1) # goal = to_keep[idx] # for choosing the goal based on a random latitude and longitude # goal_lon = np.random.randint(np.amin(data[:,0]), np.amax(data[:,0])) # goal_lat = np.random.randint(np.amin(data[:,1]), np.amax(data[:,1])) # goal = [goal_lon, goal_lat] # for setting lat and lon based on terminal inputs lon, lat = sys.argv[1], sys.argv[2] goal = [lon, lat] g = create_graph(to_keep, 8, poly_tree, self.obstacles) # find the nodes closest to the start and goal positions node_tree = KDTree(to_keep) coords = [[start[0],start[1],0],[goal[0],goal[1],0]] ind = node_tree.query(coords, k=1, return_distance=False) start_approx, goal_approx = to_keep[ind[0][0]], to_keep[ind[1][0]] # Run A* to find a path from start to goal print('Local Start and Goal: ', start, goal) print('Approximate Local Start and Goal: ', start_approx, goal_approx) path, path_cost = a_star_graph(g, heuristic, start_approx, goal_approx) print(path) if len(path) == 0: self.plan_path() else: # Convert path to waypoints waypoints = [[int(p[0]), int(p[1]), int(p[2]), 0] for p in path] print(waypoints) # 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 = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # Read lat0, lon0 from colliders.csv into floating point values # I just use `open()` method and `f.readline()` to read the first line, # which line it read depend on the times it calls, I just call it once so it's the first line, # then slice out the number with open('colliders.csv', 'r') as f: first_line_data = f.readline().rstrip().split(', ') lat0 = float(first_line_data[0][5:]) lon0 = float(first_line_data[1][5:]) # Set home position self.set_home_position(lon0, lat0, 0) # Retrieve current global position gp = self.global_position # (lon, lat, up) # Convert to current local position lp = global_to_local(gp, self.global_home) # (north, east, down) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map t0 = time.time() data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) print("Took {0} seconds to load csv file".format(time.time() - t0)) # Convert start position to current position start = (lp[0], lp[1], TARGET_ALTITUDE) t0 = time.time() sampler = Sampler(data, self.global_home) print( "Took {0} seconds to initialize Sampler class".format(time.time() - t0)) t0 = time.time() nodes = sampler.sample(30) # Set goal as some arbitrary position on the map goal = sampler.sample_goal(nodes, 37.793, -122.4) print('Local Start and Goal: ', start, goal) nodes.append(start) nodes.append(goal) print("Took {0} seconds to create {1} sample nodes.".format( time.time() - t0, len(nodes))) t0 = time.time() g = Graph(sampler.polygons, sampler.polygon_center_tree, sampler.max_poly_xy) g.create_graph(nodes, 10) print("Took {0} seconds to build graph".format(time.time() - t0)) print("Number of edges:", len(g.edges)) t0 = time.time() # Convert grid search to graph path, _ = a_star_graph(g.graph, heuristic, start, goal) print("Took {0} seconds to search path".format(time.time() - t0)) # Prune path to minimize number of waypoints t0 = time.time() path = prune_path(path) print("Took {0} seconds to prune path".format(time.time() - t0)) # Convert path to waypoints waypoints = [[int(p[0]), int(p[1]), TARGET_ALTITUDE, 0] for p in path] self.waypoints = waypoints # Send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): print("Searching for a path ...") TARGET_ALTITUDE = 5.0 # read lat0, lon0 from colliders into floating point values with open('colliders.csv') as f: first_line = f.readline().split(',') lat0, lon0 = float(first_line[0].split(' ')[-1]), float( first_line[1].split(' ')[-1]) # set home position to (lon0, lat0, 0) self.set_home_position( lon0, lat0, 0) # set the current location to be the home position # Convert drone global position to local position (relative to global home) 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)) # Load precomputed graph print('Loading graph {0}'.format(len(self.graph.nodes))) # Set grid start position at our current drone global position drone_location = (self.local_position[0] - self.north_offset, self.local_position[1] - self.east_offset, self.local_position[2]) print('Drone location local: {0}, Drone location map: {1}'.format( self.local_position, drone_location)) # Select the nearest node closest to the drone location nearest_start = None closest_distance = sys.float_info.max for n in self.graph.nodes: # heuristic is the Euclidean distance: distance = heuristic(drone_location, n) if distance < closest_distance: closest_distance = distance nearest_start = n if nearest_start == None: print('Error while getting closest starting node') return print('Found starting node = {0}'.format(nearest_start)) # Select a random goal node rnd_goal = np.random.randint(len(self.graph.nodes)) goal = list(self.graph.nodes)[rnd_goal] print('Selecting random goal[{0}]: {1}'.format(rnd_goal, goal)) print('****') path, cost = a_star_graph(self.graph, heuristic, nearest_start, goal) print('A* from {0} to {1} with a length of: {2}'.format( nearest_start, goal, len(path))) print('A* path: ', path) # First waypoint of path curr = path[0] # Save height for Takeoff self.target_position[2] = curr[2] # Add first waypoint waypoints = [[curr[0], curr[1], curr[2], 0]] idx = 1 while idx < len(path): prev_wp = waypoints[len(waypoints) - 1] # idx indicate a good waypoint candidate to be inserted # path[idx] is supposed to be a good candidate already for the A* construction # Check following waypoints while idx + 1 < len(path): candidate_wp = path[idx + 1] hit = False cells = list( bresenham(int(prev_wp[0]), int(prev_wp[1]), int(candidate_wp[0]), int(candidate_wp[1]))) for c in cells: # Check if we're in collision if self.collision_grid[ c[0], c[1]] >= FLYING_ALTITUDE + SAFETY_DISTANCE: hit = True break # If the path does not hit on obstacle, add it to the list if not hit: # It's a good candidate. Update idx idx = idx + 1 else: break # Add the candidate using idx good_candidate = path[idx] curr_wp = [ good_candidate[0], good_candidate[1], good_candidate[2], 0 ] # Set heading of curr_wp based on relative position to prev_wp heading = np.arctan2((curr_wp[1] - prev_wp[1]), (curr_wp[0] - prev_wp[0])) curr_wp[3] = heading # Append it to waypoints waypoints.append(curr_wp) idx = idx + 1 # Set self.waypoints self.waypoints = waypoints #self.send_waypoints() self.flight_state = States.PLANNING
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 self.target_position[2] = TARGET_ALTITUDE with open('colliders.csv') as f: lat_lon_data = f.readline().split() lat0, lon0 = float(lat_lon_data[1].strip(',')), float( lat_lon_data[3]) self.set_home_position(lon0, lat0, 0) north_home, east_home, down_home = global_to_local( self.global_position, self.global_home) # TODO: read lat0, lon0 from colliders into floating point values # TODO: set home position to (lon0, lat0, 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('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) graph, xmin, ymin, xmax, ymax, zmax, obstacle_tree, obstacle_dict, max_radius, valid_nodes = create_graph( data) graph_start = closest_node(graph, self.local_position) graph_goal = closest_node(graph, [ np.random.uniform(0, xmax), np.random.uniform(0, ymax), np.random.uniform(0, zmax) ]) #visualize_graph(data, graph_start, graph_goal, graph, valid_nodes) #graph_goal = closest_node(graph, global_to_local([np.random.randint(xmin, xmax), # np.random.randint(ymin, ymax), # np.random.randint(0, zmax)], self.global_home)) # 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) # TODO: convert start position to current position rather than map center # Set goal as some arbitrary position on the grid # 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) print('Local Start and Goal: ', graph_start, graph_goal) path, _ = a_star_graph(graph, heuristic, graph_start, graph_goal) pruned_path = prune_path(path, obstacle_tree, obstacle_dict, max_radius) #visualize_path(data, graph, pruned_path, graph_start, graph_goal) # TODO: prune path to minimize number of waypoints # TODO (if you're feeling ambitious): Try a different approach altogether! # Convert path to waypoints waypoints = [[int(p[0]), int(p[1]), int(p[2]), 0] for p in pruned_path] print(f'waypoints {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 (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, g_goal): """ Plan a path from current position to specified global position (lat, lon, alt) :param g_goal: goal, geodetic coordinates :return: """ print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # read lat0, lon0, coordinates of the map center filename = 'colliders.csv' with open(filename) as f: for line in f: break (_, lat0, _, lon0) = line.split() lat0 = float(lat0.strip(',')) lon0 = float(lon0.strip(',')) # set home position to center of the map self.set_home_position(lon0, lat0, 0) # retrieve current global position of the Drone. (lon, lat, alt) as np.array # convert current global coordinates to NED frame (centered at home) l_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 map_data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define 2D grid representation of the map at specified altitude, with specified safety margin around obstacles # create_grid discretizes the map at 1 meter resolution. grid, north_offset, east_offset = create_grid(map_data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # Convert local position to grid coordinates def int_round(i): return int(round(i)) def local_to_grid(l_pos, north_offset, east_offset): return (-north_offset + int_round(l_pos[0]), -east_offset + int_round(l_pos[1])) grid_start = local_to_grid(l_pos, north_offset, east_offset) # Set goal in grid coordinates l_goal = global_to_local(g_goal, self.global_home) grid_goal = local_to_grid(l_goal, north_offset, east_offset) # Run A* to find a path from start to goal on a graph using Voronoi regions print('Local Start and Goal: ', grid_start, grid_goal) starttime = time.time() path, _ = a_star_graph(map_data, TARGET_ALTITUDE, SAFETY_DISTANCE, grid_start, grid_goal) print('planned using A* on full graph in {} secs. path length: {}'. format(time.time() - starttime, len(path))) path = prune_path(path) print('pruned path length: {}'.format(len(path))) # Convert path to waypoints heading = 0 waypoints = [] wp1 = None total_distance = 0.0 for p in path: wp2 = [ int_round(p[0]) + north_offset, int_round(p[1]) + east_offset, TARGET_ALTITUDE, heading ] if wp1 is not None: heading = np.arctan2(wp2[1] - wp1[1], wp2[0] - wp1[0]) wp2[3] = heading total_distance += np.linalg.norm(np.array(wp2) - np.array(wp1)) waypoints.append(wp2) wp1 = wp2 print("total planned 3D path length: {:.2f} meters".format( total_distance)) # Set self.waypoints to follow self.waypoints = waypoints # send waypoints to sim for visualization 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 = 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 = 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 = 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 = 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 = 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 = 1 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values home_pos = open('colliders.csv').readline() lat, lon = home_pos.replace(' ', '').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 # Why? cant I just use self.global_position? global_position = np.asarray( [self._longitude, self._latitude, self._altitude]) local_pos = global_to_local(self.global_position, self.global_home) self._north = local_pos[0] self._east = local_pos[1] self._down = local_pos[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)) #goal_pos = global_to_local((-122.397967, 37.792010, 0), self.global_home) #goal_pos = global_to_local((-122.399177, 37.791030, 0), self.global_home) #goal_pos = global_to_local((-122.399646, 37.791320, 0), self.global_home) #goal_pos = global_to_local((-122.399646, 37.791320, 0), self.global_home) goal_pos = global_to_local((-122.392980, 37.792826, 0), self.global_home) # this one is out of bounds #goal_pos = global_to_local((-122.402583, 37.791741, 0), self.global_home) use_grid = False if use_grid: # Set goal as some arbitrary position on the grid grid_goal = (-north_offset + int(goal_pos[0]), -east_offset + int(goal_pos[1])) # Define starting point on the grid (this is just grid center) grid_start = (int(self._north) - north_offset, int(self._east) - east_offset) # Run A* to find a path from start to goal print('Local Start and Goal: ', grid_start, grid_goal) path, _ = a_star(grid, heuristic, grid_start, grid_goal) path = prune_path_bresenham(path, grid) # Convert path to waypoints waypoints = [[ p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0 ] for p in path] else: start_graph = (self._north, self._east, 0.0) goal_graph = tuple(goal_pos) print(start_graph) print(goal_graph) # num samples needs to be low because otherwise simulator does not react to commands after some timout I believe? # how can I speed up the connect edge code? graph, nodes = prm(data, num_samples=400, extra_points=[start_graph, goal_graph]) tree = KDTree(nodes) indicies = tree.query([start_graph], 10, return_distance=False)[0] start_graph = nodes[int(indicies[0])] indicies = tree.query([goal_graph], 10, return_distance=False)[0] goal_graph = nodes[int(indicies[0])] path, _ = a_star_graph(graph, heuristic, start_graph, goal_graph) # Convert path to waypoints # Why do I need to cast this to int? # does the simulator / API not support exact foat valuesfor coordinates? # When I do not change to int this just hangs at takeoff transition waypoints = [[int(p[0]), int(p[1]), 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 = 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_graph(self): """ Construct a configuration space using a graph representation, set destination GPS position, find a path from start (current) position to destination, and minimize and set waypoints. """ self.flight_state = States.PLANNING print("Searching for a path ...") data = np.loadtxt('map_obstacle_course.csv', delimiter=',', dtype='Float64', skiprows=2) TARGET_ALTITUDE = 0.3 / 0.0254 SAFETY_DISTANCE = 0.25 / 0.0254 st = time.time() # grid, G = create_graph_and_edges_crazyflie( # data, 130, 86, TARGET_ALTITUDE, SAFETY_DISTANCE) GRID_NORTH_SIZE = 130 GRID_EAST_SIZE = 86 NUMBER_OF_NODES = 200 OUTDEGREE = 4 grid, G = construct_road_map_crazyflie(data, GRID_NORTH_SIZE, GRID_EAST_SIZE, TARGET_ALTITUDE, SAFETY_DISTANCE, NUMBER_OF_NODES, OUTDEGREE) # visualize_prob_road_map_crazyflie(data, grid, G) time_taken = time.time() - st print(f'create_graph_and_edges() took: {time_taken} seconds') grid_start = (32, 40, 0) grid_goal = (110, 40, 0) # Find closest node on the graph g_start, g_goal = find_start_goal(G, grid_start, grid_goal) print("Start and Goal location:", grid_start, grid_goal) print("Start and Goal location on graph:", g_start, g_goal) path, _ = a_star_graph(G, heuristic, g_start, g_goal) path.append(grid_goal) new_path = [] # new_path.append(grid_start) new_path.append(g_start) new_path.extend(path) print(new_path) unique_path = [] for p in new_path: if p not in unique_path: unique_path.append(p) print(unique_path) # Reduce waypoints reduced_path = condense_waypoints_crazyflie(grid, unique_path) print(f'reduced_path: {reduced_path}') waypoints = [] # modify path coordinates as offset from the takeoff position # i.e., we treat the firt position as origin for p in reduced_path: print(np.array(p) - np.array(grid_start)) offset = (np.array(p) - np.array(grid_start)) * 0.0254 offset[2] = TARGET_ALTITUDE * 0.0254 waypoints.append(list(offset)) print(f'All waypoints: {waypoints}') self.all_waypoints = waypoints visualize_prob_road_map_crazyflie(data, grid, G, grid_start, grid_goal, unique_path, all_nodes=False) visualize_prob_road_map_crazyflie(data, grid, G, grid_start, grid_goal, reduced_path, all_nodes=False) return self.all_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 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 prob_road_map(self): """Probablistic Road Map implementation to create configuration space, set start and goal positions, find path from start to goal, and condense and set waypoints for navigation. """ 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 = self.read_home_lat_lon('colliders.csv') # TODO: set home position to (lon0, lat0, 0) print(f'Setting home to (lon0, lat0, 0): ({lon0, lat0, 0})') self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position global_position = self.global_position # TODO: convert to current local position using global_to_local() local_home = global_to_local(global_position, self.global_home) print(f'Home coordinates in local form: {local_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) print(f'Constructing Probablistic Road Map...') st = time.time() grid, G, north_offset, east_offset = construct_road_map( data, TARGET_ALTITUDE, SAFETY_DISTANCE, 500, 4) time_taken = time.time() - st print(f'construct_road_map() took: {time_taken} seconds') # TODO: convert start position to current position rather than map center grid_start = (int(self.local_position[0] - north_offset), int(self.local_position[1] - east_offset)) # goal_lon_lat = [-122.398525, 37.793510, 0] # goal_lon_lat = [-122.398250, 37.793875, 0] # goal_lon_lat = [-122.398275, 37.796875, 0] # (point3) reachable if SAFETY_DISTANCE = 5 # goal_lon_lat = [-122.399970, 37.795947, 0] # reachable if SAFETY_DISTANCE = 5 # goal_lon_lat = [-122.393284, 37.790545, 0] # (point2) goal_lon_lat = [-122.401173, 37.795514, 0] # (point1) # goal_lon_lat = [-122.393805, 37.795825, 0] # not reachable if TARGET_ALTITUDE = 5 # (point1) -> (point2) -> (point3) this sequence results in some # trees getting in the way of the drone. This is a good example # of imprecise mapping and we may have to use receding horizon # planning which utilizes a 3D state space for a limited region # around the drone. grid_goal = global_to_local(goal_lon_lat, self.global_home) grid_goal = (int(grid_goal[0] - north_offset), int(grid_goal[1] - east_offset)) # Find closest node on probablistic road map graph g_start, g_goal = find_start_goal(G, grid_start, grid_goal) print("Start and Goal location:", grid_start, grid_goal) print("Start and Goal location on graph:", g_start, g_goal) path, _ = a_star_graph(G, heuristic, g_start, g_goal) # Add the final goal state (instead of the approximated # goal on the graph) path.append(grid_goal) print(path) # Reduce waypoints path = condense_waypoints(grid, path) # 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()
dist = distance(p2, p1) G.add_edge(p1,p2,weight=dist) plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'b-') dist_s = distance(start_ne, p1) if dist_s < close_start_dist : close_start_dist = dist_s close_start_pt = p1 dist_g = distance(goal_ne, p1) if dist_g < close_goal_dist : close_goal_dist = dist_g close_goal_pt = p1 print("graph digested", time.clock()) path = a_star_graph(G, heuristic, close_start_pt, close_goal_pt) print("A* graph found", time.clock()) shortestpath = nx.dijkstra_path(G, close_start_pt, close_goal_pt, 'weight') print("dijkstra graph found", time.clock()) plt.plot(start_ne[1], start_ne[0], 'rx') plt.plot(goal_ne[1], goal_ne[0], 'rx') plt.plot(close_start_pt[1], close_start_pt[0], 'yo') plt.plot(close_goal_pt[1], close_goal_pt[0], 'yo') # plot dijkstra path in cyan for pt in shortestpath : plt.plot(pt[1], pt[0], 'g^' ) # plot path in yellow for p in range(0, len(path[0])-1) :
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) # Prune path on grid timer = time.time() pruned_path = prune_path(path, grid_narrow) pruned_path = prune_path(pruned_path, grid_narrow) print('Pruned path from {0} to {1} waypoints in {2}s'.format(
def perform_astar(Gr, Cg, nmin=0, emin=0): #drone_location = (-emin, -nmin, 5.0) # map coordinates drone_location = (445.04762260615826, 315.94609723985195, 5.0) print('Find Start node from {0}'.format(drone_location)) nearest_start = None closest_distance = sys.float_info.max for n in Gr.nodes: # heuristic is the Euclidean distance: distance = heuristic(drone_location, n) if distance < closest_distance: closest_distance = distance nearest_start = n if nearest_start == None: print('Error while getting closest starting node') return print('Found starting node = {0}'.format(nearest_start)) ########## goal_location = (240.7685, 360.76114, 5.0) # map coordinates print('Find Goal node from {0}'.format(goal_location)) nearest_goal = None closest_distance = sys.float_info.max for n in Gr.nodes: # heuristic is the Euclidean distance: distance = heuristic(goal_location, n) if distance < closest_distance: closest_distance = distance nearest_goal = n ################ start = nearest_start print('Start: ', start) goal = nearest_goal print('Goal: ', goal) path, cost = a_star_graph(Gr, heuristic, start, goal) print(len(path), path) if len(path) == 0: return waypoints = [[p[0], p[1], p[2], 0] for p in path] print("start") fig = plt.figure(figsize=(10,10)) plt.imshow(Cg, cmap='Greys', origin='lower') path_pairs = zip(waypoints[:-1], waypoints[1:]) for (n1, n2) in path_pairs: plt.plot([n1[1], n2[1]], [n1[0], n2[0]], 'green') plt.scatter(drone_location[0], drone_location[1], c='blue') # (0,0) plt.scatter(emin - emin, nmin - nmin , c='green') # Lowest point plt.scatter(100, 0, c='purple') # (0,0) plt.xlabel('EAST') plt.ylabel('NORTH') plt.show()