def plan_medial_axis(self, data,local_start, local_goal, TARGET_ALTITUDE, SAFETY_DISTANCE): print('Medial-Axis planning') # create grid and skeleton grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format(north_offset, east_offset)) skeleton = medial_axis(invert(grid)) print('Skeleton generated') # calculate the closest start/goal points on the "graph" start_ne = ( local_start[0] - north_offset, local_start[1] - east_offset) goal_ne = ( local_goal[0] - north_offset, local_goal[1] - east_offset) skel_start, skel_goal = find_start_goal(skeleton, start_ne, goal_ne) # run A* to search for the goal along the road network path, cost = a_star(invert(skeleton).astype(np.int), heuristic, tuple(skel_start), tuple(skel_goal)) # Prune path to minimize number of waypoints if (args.prune == 'collinearity'): print('Pruning path with collinearity check') path = collinearity_prune(path) elif (args.prune == 'bresenham'): print('Pruning path with bresenham') path = bresenham_prune(path,grid) else: print('Unrecognized prune option returning full path') # Convert path to waypoints waypoints = [[int(p[0]) + north_offset,int(p[1]) + east_offset, TARGET_ALTITUDE, 0] for p in path] return waypoints
def medial_axis_Astar(self, data, global_goal, TARGET_ALTITUDE, SAFETY_DISTANCE): grid, north_offset, east_offset = create_grid_bfs( data, TARGET_ALTITUDE, SAFETY_DISTANCE) skeleton = medial_axis(invert(grid)) 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) local_goal = global_to_local(global_goal, self.global_home) grid_goal = (int(local_goal[0]) - north_offset, int(local_goal[1]) - east_offset) skel_start, skel_goal = find_start_goal(skeleton, grid_start, grid_goal) mapa = breadth_first( invert(skeleton).astype(np.int), tuple(skel_goal), tuple(skel_start)) print('Local Start and Goal: ', grid_start, grid_goal) path, _ = a_star_bfs( invert(skeleton).astype(np.int), mapa, tuple(skel_start), tuple(skel_goal)) path.append(grid_goal) pruned_path = prune_path_bres(path, grid) waypoints = [[ int(p[0] + north_offset), int(p[1] + east_offset), TARGET_ALTITUDE, 0 ] for p in pruned_path] for i in range(1, len(waypoints)): heading = np.arctan2( waypoints[i][0] - waypoints[i - 1][0], waypoints[i][1] - waypoints[i - 1][1]) - pi / 2 waypoints[i][3] = -heading return 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 lat0, lon0 = read_home_location() # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position current_global_position = [ self._longitude, self._latitude, self._altitude ] print("\nself.global_position: {0}".format(self.global_position)) print("current_global_position: {0}".format(current_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("current_local_position: {0}".format(current_local_position)) print('\nglobal home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map self.collider_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(self.collider_map_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) #hardcoded to middle of the grid, replacing it with below: # TODO: convert start position to current position rather than map center grid_start = (int(np.ceil(current_local_position[0] - north_offset)), int(np.ceil(current_local_position[1] - east_offset))) print("grid_start: {0}".format(grid_start)) # Set goal as some arbitrary position on the grid #grid_goal = (-north_offset + 10, -east_offset + 10) #hardcoded as some location 10 m north and 10 m east of map center as dfault, replacing it with below: # TODO: adapt to set goal as latitude / longitude position and convert #goal_global_position = new_global_position(self.global_home, 15, 25) #adding 20 x 10 meters to the global_home position. goal_global_position = [-122.401055, 37.795461, 0] goal_local_position = global_to_local(goal_global_position, self.global_home) grid_goal = (int(goal_local_position[0] - north_offset), int(goal_local_position[1] - east_offset)) print("goal_local_position: {0}".format(goal_local_position)) print("grid_goal: {0}".format(grid_goal)) print("grid.shape: {0}".format(grid.shape)) if grid_goal[0] > grid.shape[0] or grid_goal[1] > grid.shape[ 1] or grid_goal[0] < 0 or grid_goal[1] < 0: print( "\n * NOTE: Destination is outside the grid map. setting it back to start " ) grid_goal = grid_start self.landing_transition() #if grid[grid_goal[0]][grid_goal[1]] > 0: # checking that the destination is not colliding with any obstacle # print("\n * NOTE: Destination is inside an obstacle. Setting it to start position. ") # grid_goal = grid_start # self.landing_transition() # REZA: Adding Skeleton median axis skeleton = medial_axis(invert(grid)) skel_start, skel_goal = find_start_goal(skeleton, grid_start, grid_goal) print('\nLocal Skelton Start and Goal: ', grid_start, grid_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 Grid Start and Goal: ', grid_start, grid_goal) # Choose one of the following A* #path, _ = a_star(grid, heuristic, grid_start, grid_goal) #Run A* on the grid path, _ = a_star( invert(skeleton).astype(np.int), heuristic, tuple(skel_start), tuple(skel_goal)) # TODO: prune path to minimize number of waypoints # TODO (if you're feeling ambitious): Try a different approach altogether! print("path: ", path) path = prune_path(path) print("revised path: ", 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 # *** Initiating gridsize3d = np.array([6, 6, 6]) print("Initiating a 3D grid, size: " + str(gridsize3d)) self.grid3D = HorisonMap3D(gridsize3d, self.local_position, 2) # 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', 'r') as f: latlon = f.readline() ll = latlon.strip().replace(',', '').split(' ') lat0, lon0 = float(ll[1]), float(ll[3]) # TODO: set home position to (lon0, lat0, 0) # self.set_home_position(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() current_local_position = global_to_local(global_position, self.global_home) print('new local position:{}'.format(current_local_position)) self._north, self._east, self._down = current_local_position print('self.local_position:{}'.format(self.local_position)) 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_off = (-north_offset, -east_offset) # TODO: convert start position to current position rather than map center # grid_start = (int(current_local_position[0] + grid_start[0]), int(current_local_position[1] + grid_start[1])) grid_start = tuple( map(sum, zip(map(int, current_local_position), grid_start_off))) # 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 # arbitrary_global_position = (10, 10) # lat/lon # arbitrary_local_position = global_to_local(arbitrary_global_position, self.global_home) arbitrary_local_position = self.grid_goal print('grid_goal:{}'.format(arbitrary_local_position)) grid_goal = tuple( map(sum, zip(grid_start_off, arbitrary_local_position))) # 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) # graph A* and graph_goal/start and medial_axis skeleton = medial_axis(invert(grid)) skel_start, skel_goal = find_start_goal(skeleton, grid_start, grid_goal) print('skel start and goal: {}, {}'.format(skel_start, skel_goal)) # todo graph A* print('Local Start and Goal: ', grid_start, grid_goal) path, _ = a_star(grid, heuristic, grid_start, grid_goal) # TODO: prune path to minimize number of waypoints print('len(path):{}'.format(len(path))) path = prune_path(path) print('new len(path):{}'.format(len(path))) # TODO (if you're feeling ambitious): Try a different approach altogether! # Convert path to waypoints waypoints = [[ p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0 ] for p in path] # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def 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()
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 = load_lat_lon() # set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # retrieve current global position glob_p = self.global_position # convert to current local position using global_to_local() loc_p = global_to_local(glob_p, 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) # convert start position to current position rather than map center grid_start = (-north_offset + int(loc_p[0]), -east_offset + int(loc_p[1])) # Set goal as some arbitrary position on the grid # set goal as latitude / longitude position and convert 37.792480, lon0 -122.397450 loc_goal = global_to_local(self.glob_goal, self.global_position) grid_goal = (-north_offset + int(loc_goal[0]), -east_offset + int(loc_goal[1])) # Run A* to find a path from start to goal # path, _ = a_star(grid, heuristic, grid_start, grid_goal) # 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) skeleton = medial_axis(invert(grid)) skel_start, skel_goal = find_start_goal(skeleton, grid_start, grid_goal) print(skel_start, skel_goal) # Run A* on the skeleton path, cost = a_star( invert(skeleton).astype(np.int), heuristic_func, tuple(skel_start), tuple(skel_goal)) # prune path to minimize number of waypoints path = prune_path(path) plotter.plot_all(grid, grid_start, grid_goal, path, skeleton) # 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 # send waypoints to sim (this is just for visualization of waypoints) self.connection.start() 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 with open('colliders.csv') as f: home_pos_data = f.readline().split(',') lat0 = float(home_pos_data[0].strip().split(' ')[1]) lon0 = float(home_pos_data[1].strip().split(' ')[1]) # DONE: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # DONE: retrieve current global position current_global_position = [ self._longitude, self._latitude, self._altitude ] # DONE: 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)) print( 'global home {0}, position {1}, current local position {2}'.format( self.global_home, current_global_position, current_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, points = create_grid( data, TARGET_ALTITUDE, SAFETY_DISTANCE) edges = create_edges_from_grid(grid, points) print("Grid = {}, north offset = {}, east offset = {}".format( grid.shape, north_offset, east_offset)) # Define starting point on the grid # DONE: convert start position to current position rather than map center grid_start = (int(current_local_position[0] - north_offset), int(current_local_position[1] - east_offset)) # Set goal as some arbitrary position on the grid # DONE: adapt to set goal as latitude / longitude position and convert # global_goal = local_to_global((750 + north_offset, 370 + east_offset, 0), self.global_home) # print('global_goal', global_goal) global_goal = (-122.39827335, 37.79639627, 0) local_goal = global_to_local(global_goal, self.global_home) grid_goal = (int(local_goal[0] - north_offset), int(local_goal[1] - east_offset) ) #(750., 370.) #(920, 920) # Run A* to find a path from start to goal print('Local Start and Goal: ', grid_start, grid_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) """ edges = create_edges_from_grid(grid, 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) closest_grid_start = closest_point(G, grid_start) closest_grid_goal = closest_point(G, grid_goal) """ skeleton = medial_axis(invert(grid)) closest_grid_start, closest_grid_goal = find_start_goal( skeleton, grid_start, grid_goal) print('Closest Local Start and Goal: ', closest_grid_start, closest_grid_goal) # A* #path, _ = a_star(grid, heuristic, grid_start, grid_goal) # A* for Voronoi Graph #path, _ = a_star_graph(G, heuristic, closest_grid_start, closest_grid_goal) # A* for Medial Axis path, _ = a_star( invert(skeleton).astype(np.int), heuristic, tuple(closest_grid_start), tuple(closest_grid_goal)) # DONE: prune path to minimize number of waypoints # DONE (if you're feeling ambitious): Try a different approach altogether! pruned_path = prune_path(path) # Convert path to 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 print(self.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 = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values """ probably the np.genfromtxt fuction could have done this, but I was not able to find the way, used csv.reader to get it""" latlondata = csv.reader(open('colliders.csv', newline=''), delimiter=',') for row in latlondata: lat0, lon0 = row[:2] break lat0 = lat0.replace("lat0 ", "") lon0 = lon0.replace(" lon0 ", "") lat0 = float(lat0) lon0 = float(lon0) # # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position Globalcurrent = np.array( (self.global_position[0], self.global_position[1], self.global_position[2])) # TODO: convert to current local position using global_to_local() Localcurrent = global_to_local(Globalcurrent, self.global_home) #self.local_position = [Localcurrent[0],Localcurrent[1],Localcurrent[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) Zmax = np.max(data[:, 2] + data[:, 5]) # Define a grid for a particular altitude and safety margin around obstacles """ Create grid comes from the planning_utils.py so we can call it directly here""" grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) """ Using Medial-Axis solution for motion planning 3""" skeleton = medial_axis(invert(grid)) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # Define starting point on the grid (this is just grid center) grid_center = (north_offset, east_offset) # TODO: convert start position to current position rather than map center """ Get local grid position and determine the new grid start from global home then add it to grid center """ Localcurrent = global_to_local(self.global_position, self.global_home) grid_start = (int(-grid_center[0] + Localcurrent[0]), int(-grid_center[1] + Localcurrent[1])) #if local position is not at altitude 0, update the altitute for takeoff if -Localcurrent[2] > TARGET_ALTITUDE: TARGET_ALTITUDE = int(TARGET_ALTITUDE - Localcurrent[2]) self.target_position[2] = TARGET_ALTITUDE # Set goal as some arbitrary position on the grid if UseLatLonAlt == False: grid_goal = (-north_offset + Localcurrent[0] + 10, -east_offset + Localcurrent[1] + 10) print("using grid position for the goal") else: print("using Lat, Lon, Alt position for the goal") # TODO: adapt to set goal as latitude / longitude position and convert """ See global variable grid_lat_lon at the top of the file""" """ code to get the grid goal based on LAT LONG position, assume altitude is 0 for goal""" grid_goal = global_to_local(grid_goal_lat_lon_alt, self.global_home) grid_goal = tuple((int(grid_goal[0] - north_offset), int(grid_goal[1] - east_offset))) print(grid_start, grid_goal) # if Altitude is not ground level adapt to new altitude if grid_goal_lat_lon_alt[2] > TARGET_ALTITUDE: TARGET_ALTITUDE = grid_goal_lat_lon_alt[2] + SAFETY_DISTANCE self.target_position[2] = TARGET_ALTITUDE #self.global_home[2] = grid_goal_lat_lon_alt[2] grid, north_offset, east_offset = create_grid( data, TARGET_ALTITUDE, SAFETY_DISTANCE) """ Using Medial-Axis solution for motion planning 3""" skeleton = medial_axis(invert(grid)) # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation """ Done on Planning_utils.py """ # or move to a different search space such as a graph (not done here) print('Local Start and Goal: ', grid_start, grid_goal) """ Using Medial-Axis solution to find the skeleton start and goal""" skel_start, skel_goal = find_start_goal(skeleton, grid_start, grid_goal) print(grid_start, grid_goal) print(skel_start, skel_goal) """ a_star, heuristic come from the planning_utils.py so we can call it directly""" #path, _ = a_star(grid, heuristic, grid_start, grid_goal) path, _, FoundPath = a_star( invert(skeleton).astype(np.int), heuristic, tuple(skel_start), tuple(skel_goal)) while FoundPath == False: # increase target altitude by 5 ft, get new skeeton and try again TARGET_ALTITUDE += 20 print("TRY AGAIN, NEW CRUISE ALTITUDE") print(TARGET_ALTITUDE) self.target_position[2] = TARGET_ALTITUDE grid, north_offset, east_offset = create_grid( data, TARGET_ALTITUDE, SAFETY_DISTANCE) skeleton = medial_axis(invert(grid)) skel_start, skel_goal = find_start_goal(skeleton, grid_start, grid_goal) path, _, FoundPath = a_star( invert(skeleton).astype(np.int), heuristic, tuple(skel_start), tuple(skel_goal)) if TARGET_ALTITUDE > Zmax: break # TODO: prune path to minimize number of waypoints pruned_path = prune_path(path) """ Add the grid goal so that it can fly to the exact location""" pruned_path.append([grid_goal[0], grid_goal[1]]) sampler = Sampler(data, SAFETY_DISTANCE) polygons = sampler._polygons # Second Prunning to try to minimize waypoints by doing direct to # Also add Altitude Path3D, TARGET_ALTITUDE = get3DPath(pruned_path,TARGET_ALTITUDE,self.local_position[2],\ grid_goal_lat_lon_alt[2],SAFETY_DISTANCE, polygons) # TODO (if you're feeling ambitious): Try a different approach altogether! #add heading to all the waypoints Path_with_heading = [] Path_with_heading.append([Path3D[0][0], Path3D[0][1], 0]) for i in range(0, len(Path3D) - 1): heading = np.arctan2((Path3D[i + 1][1] - Path3D[i][1]), (Path3D[i + 1][0] - Path3D[i][0])) Path_with_heading.append( [Path3D[i + 1][0], Path3D[i + 1][1], int(heading)]) if FoundPath == False: # if path not found then takeoff and land on the spot localpos = global_to_local(self.global_position, self.global_home) waypoints = [[int(localpos[0]),int(localpos[1]),5,0],\ [int(localpos[0]),int(localpos[1]),0,0]] else: # Convert path to waypoints waypoints = [[ int(p[0] + north_offset), int(p[1] + east_offset), TARGET_ALTITUDE, int(p[2]) ] for p in Path_with_heading] print(waypoints) # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) """ This function sends the waypoints to the 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 = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values """ probably the np.genfromtxt fuction could have done this, but I was not able to find the way, used csv.reader to get it""" latlondata = csv.reader(open('colliders.csv', newline=''), delimiter=',') for row in latlondata: lat0, lon0 = row[:2] break lat0 = lat0.replace("lat0 ", "") lon0 = lon0.replace(" lon0 ", "") lat0 = float(lat0) lon0 = float(lon0) # # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position Globalcurrent = np.array( (self.global_position[0], self.global_position[1], self.global_position[2])) # TODO: convert to current local position using global_to_local() Localcurrent = global_to_local(Globalcurrent, self.global_home) #self.local_position = [Localcurrent[0],Localcurrent[1],Localcurrent[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 """ Create grid comes from the planning_utils.py so we can call it directly here""" grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) """ Using Medial-Axis solution for motion planning 3""" skeleton = medial_axis(invert(grid)) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # Define starting point on the grid (this is just grid center) grid_center = (north_offset, east_offset) # TODO: convert start position to current position rather than map center """ Get local grid position and determine the new grid start from global home then add it to grid center """ Localcurrent = global_to_local(self.global_position, self.global_home) grid_start = (int(-grid_center[0] + Localcurrent[0]), int(-grid_center[1] + Localcurrent[1])) # Set goal as some arbitrary position on the grid #grid_goal = (-north_offset + 10, -east_offset + 10) # TODO: adapt to set goal as latitude / longitude position and convert """ code to get the grid goal based on LAT LONG position, assume altitude is 0 for goal""" grid_goal_lat_lon = (-122.39966, 37.793593, 0) #grid_goal_lat_lon = (-122.397185, 37.792857, 0) grid_goal = global_to_local(grid_goal_lat_lon, self.global_home) grid_goal = tuple((int(grid_goal[0] - north_offset), int(grid_goal[1] - east_offset))) print(grid_start, grid_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 """ Done on Planning_utils.py """ # or move to a different search space such as a graph (not done here) print('Local Start and Goal: ', grid_start, grid_goal) """ Using Medial-Axis solution to find the skeleton start and goal""" skel_start, skel_goal = find_start_goal(skeleton, grid_start, grid_goal) print(grid_start, grid_goal) print(skel_start, skel_goal) """ a_star, heuristic come from the planning_utils.py so we can call it directly""" #path, _ = a_star(grid, heuristic, grid_start, grid_goal) path, _ = a_star( invert(skeleton).astype(np.int), heuristic, tuple(skel_start), tuple(skel_goal)) # TODO: prune path to minimize number of waypoints pruned_path = prune_path(path) """ Add the grid goal so that it can fly to the exact location""" pruned_path.append([grid_goal[0], grid_goal[1]]) # TODO (if you're feeling ambitious): Try a different approach altogether! """ USE RRT to replan """ # num_vertices = 300 # dt = 1 # x_init = (50, 50) # rrt = generate_RRT(grid, x_init, num_vertices, dt) # Convert path to waypoints waypoints = [[ p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0 ] for p in pruned_path] #print (waypoints) # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) """ This function sends the waypoints to the 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 = 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) for row in reader: lhs, rhs = row[0], row[1] break _, lat = lhs.split() _, lon = rhs.split() lat0, lon0 = float(lat), float(lon) # TODO: set home position to (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_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 = 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 curren0t position rather than map center grid_start = (abs(int(local_position[1])) - north_offset, abs(int(local_position[0])) - east_offset) # Set goal as some arbitrary position on the grid # grid_goal = (-north_offset + 10, -east_offset + 10) #grid_goal = (200, 200) # TODO: adapt to set goal as latitude / longitude position and convert goal_y, goal_x = lat_lon_goal(37.796391, -122.398300, self.global_home) grid_goal = (int(goal_y - north_offset), int(goal_x - east_offset)) #Need to cehck if start and goal are in free space skeleton = medial_axis(invert(grid)) skel_start, skel_goal = find_start_goal(skeleton, grid_start, grid_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) grid_start = int(skel_start[0]), int(skel_start[1]) grid_goal = int(skel_goal[0]), int(skel_goal[1]) print('Local Start and Goal: ', grid_start, grid_goal) path, _ = a_star(grid, heuristic, grid_start, grid_goal) # TODO: prune path to minimize number of waypoints pruned_path = prune_path(path) print(len(pruned_path)) # TODO (if you're feeling ambitious): Try a different approach altogether! # Convert path to waypoints # waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path] 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_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 = 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 goal_set_latlon = True self.target_position[2] = TARGET_ALTITUDE # 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() file = open("colliders.csv", 'r') lines = file.readlines() words = [i.split(' ') for i in lines[0].split(',')] lat0 = float(words[0][1]) lon0 = float(words[1][2]) #lat0 = 37.792480 #lon0 = -122.397450 self.set_home_position(lon0, lat0, 0) 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) skeleton = medial_axis(invert(grid)) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # TODO: convert start position to current position rather than map center local_position = global_to_local( (self._longitude, self._latitude, self._altitude), self.global_home) grid_start = (int(-north_offset + local_position[1]), int(-east_offset + local_position[0])) # Set goal as some arbitrary position on the grid if goal_set_latlon: lat_goal = 37.794050 lon_goal = -122.392900 local_goal = global_to_local((lon_goal, lat_goal, 0), self.global_home) grid_goal = (-north_offset + local_goal[0], -east_offset + local_goal[1]) else: grid_goal = (490, 850) skel_start, skel_goal = find_start_goal(skeleton, grid_start, grid_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) print('Local Start and Goal: ', skel_start, skel_goal) #path, _ = a_star(grid, heuristic, grid_start, grid_goal) path, _ = a_star(np.invert(skeleton), heuristic, tuple(skel_start), tuple(skel_goal)) path = [(int(i[0]), int(i[1])) for i in path] # TODO: prune path to minimize number of waypoints # TODO (if you're feeling ambitious): Try a different approach altogether! pruned_path = prune_path(grid, path, 20) # Convert path to waypoints waypoints = find_waypoints( pruned_path, [self.local_position[0], self.local_position[1]], north_offset, east_offset, TARGET_ALTITUDE) # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints() self.path_planned = True plt.figure() plt.imshow(grid, cmap='Greys', origin='lower') plt.plot(grid_start[1], grid_start[0], 'x') plt.plot(grid_goal[1], grid_goal[0], 'o') pp = np.array(pruned_path) plt.plot(pp[:, 1], pp[:, 0], 'g') plt.scatter(pp[:, 1], pp[:, 0]) plt.xlabel('EAST') plt.ylabel('NORTH') plt.savefig("Path.png") plt.close()
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 # for max_rows attribute a numpy vers. >= 1.16 is required, make sure to have an up to date scikit-image package # data_pos = np.loadtxt('colliders.csv',dtype='str', max_rows=1) # (lat0,lon0) = [float(data_pos[1][:-1]),float(data_pos[3][:-1])] # for numpy vers <1.16 with open('colliders.csv') as f: latLonStrArr = f.readline().rstrip().replace('lat0', '').replace( 'lon0 ', '').split(',') lat0 = float(latLonStrArr[0]) lon0 = float(latLonStrArr[1]) # DONE: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # DONE: retrieve current global position global_position = [self._longitude, self._latitude, self._altitude] # DONE: convert to current local position using global_to_local() current_local_pos = 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) grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) skeleton = medial_axis(invert(grid)) # DONE: convert start position to current position rather than map center # Define starting point on the grid (this is just grid center) start_ne = (int(self.local_position[0] - north_offset), int(self.local_position[1] - east_offset)) # Set goal as some arbitrary position on the grid # arb_goal = (750, 370, 0) # Set random goal on map in local coordinate system found = False while not found: goal_ne = (randrange(0, len(grid[:, 1] - 1)), randrange(0, len(grid[1, :] - 1))) if grid[goal_ne] == 0: found = True # DONE: adapt to set goal as latitude / longitude position and convert (can be) # global_goal = local_to_global(arb_goal, self.global_home) # local_goal to show expected transformation # goal_ne = global_to_local(global_goal, self.global_home) print( "Drone is starting from {0} and the goal was randomly set to {1}". format(start_ne, goal_ne)) skel_start, skel_goal = find_start_goal(skeleton, start_ne, goal_ne) # 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( invert(skeleton).astype(np.int), tuple(skel_start), tuple(skel_goal)) print("Path length = {0}, path cost = {1}".format(len(path_), cost)) # DONE: prune path to minimize number of waypoints path = collinearity(path_) # 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 ] for p in path] # get heading angle for next point theta = heading(path) for i in range(len(waypoints)): waypoints[i].append(theta[i]) 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 #---------------------------------------------------------------------------- # HOME POSITION # TODO: read lat0, lon0 from colliders into floating point values # Open coliders.csv and read lat0 and lon0 colliders_file = 'colliders.csv' with open(colliders_file) as f: latlon = f.readline().strip().split(',') lat0 = float(latlon[0].replace("lat0 ", "")) lon0 = float(latlon[1].replace("lon0 ", "")) print('lat: ', lat0) print('lon: ', lon0) # finished but latitude longitude are hardcoded for now: set home position to (lat0, lon0, 0) # I have an issue where float() truncates the last 0 and therefore provides a different value #self.set_home_position(lon0, lat0, TARGET_ALTITUDE) self.set_home_position(-122.397450, 37.792480, TARGET_ALTITUDE) print('Home position is set to local position: ', self.local_position) # END HOME POSITION #---------------------------------------------------------------------------- #---------------------------------------------------------------------------- # CURRENT POSITION # finished: convert self.global_position to current local position using global_to_local() current_global_position = [ self._longitude, self._latitude, self._altitude ] print('current_global_position converted using global_to_local: {0}'. format(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)) # END CURRENT 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) skeleton = medial_axis(invert(grid)) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # create_grid_and_edges uses Voronoi, it is slower than the previous create_grid() but it's suppose to create a safer path #grid, edges, north_offset, east_offset = create_grid_and_edges(data, TARGET_ALTITUDE, SAFETY_DISTANCE) #skeleton = medial_axis(invert(grid)) #print("North offset = {0}, east offset = {1}".format(north_offset, east_offset)) #---------------------------------------------------------------------------- # START POINT # Define starting point on the grid (this is just grid center) # start_position = (-north_offset, -east_offset) # finished: convert start position to current position rather than map center grid_start = (-north_offset + int(self.local_position[0]), -east_offset + int(self.local_position[1])) print('start: ', grid_start) # END START POINT #---------------------------------------------------------------------------- #---------------------------------------------------------------------------- # SET GOAL POINT # Set goal as some arbitrary position on the grid #grid_goal = (-north_offset + 10, -east_offset + 10) # finished: adapt to set goal as latitude / longitude position and convert # some lat lon that are working # first path goal_lat = float(37.795240) goal_lon = float(-122.393136) # second path #goal_lat = float(37.796874) #goal_lon = float(-122.399683) # third path #goal_lat = float(37.793155) #goal_lon = float(-122.402035) # fourth path and back to center #goal_lat = float(37.792480) #goal_lon = float(-122.397450) goal_position = global_to_local((goal_lon, goal_lat, 0), self.global_home) print('goal position converted using global_to_local: {0}'.format( global_to_local((goal_lon, goal_lat, 0), self.global_home))) grid_goal = (-north_offset + int(goal_position[0]), -east_offset + int(goal_position[1])) # END START POINT #---------------------------------------------------------------------------- #---------------------------------------------------------------------------- # RUN A* USING MEDIAN PATH AND COST OF SQRT(2) #Run A* to find a path from start to goal # finished: 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) skel_start, skel_goal = find_start_goal(skeleton, grid_start, grid_goal) print('original start and goal: ', grid_start, grid_goal) print('calibrated start and goal on skeleton: ', skel_start, skel_goal) median_path, cost = a_star( invert(skeleton).astype(np.int), heuristic, tuple(skel_start), tuple(skel_goal)) print('Median path and cost: ', len(median_path)) pruned_path = prune_path(median_path) print('pruned path and cost: ', len(pruned_path)) # END RUN A* #---------------------------------------------------------------------------- # 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, 0 ] for p in pruned_path] print(waypoints) # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim self.send_waypoints()