def GetPath(self, north_offset, east_offset, grid, graph, start, goal): if (grid[goal[0], goal[1]] == 1): print("Goal is unreachable") return None start_g = pu.closest_point(graph, start) goal_g = pu.closest_point(graph, goal) print(goal_g) path, _ = pu.a_starGraph(graph, start_g, goal_g) # If it is not possible to find a path in tha graph, let's find a path in the grid if (path is None): print('Searching in grid') newpath, _ = pu.a_star(grid, start, goal) newpath = pu.prune_path(newpath) else: start_g = (int(start_g[0]), int(start_g[1])) goal_g = (int(goal_g[0]), int(goal_g[1])) print('start={0}, start_g={1}'.format(start, start_g)) pathToBegin, _ = pu.a_star(grid, start, start_g) pathFromEnd, _ = pu.a_star(grid, goal_g, goal) if (pathToBegin is None or pathFromEnd is None): return None newpath = pathToBegin + path + pathFromEnd newpath = pu.prune_path(newpath) return [[ int(p[0]) + north_offset, int(p[1]) + east_offset, pu.TARGET_ALTITUDE, 0 ] for p in newpath]
def grid_plan(self, data,local_start, local_goal, TARGET_ALTITUDE, SAFETY_DISTANCE): print('Simple Grid-based Planning selected') # 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)) # convert start position to current position rather than map center grid_start = ( int(local_start[0]) - north_offset, int(local_start[1]) - east_offset) grid_goal = ( int(local_goal[0]) - north_offset, int(local_goal[1]) - east_offset ) # make sure goal point is not inside an obstacle or safety margin if (grid[grid_goal[0]][grid_goal[1]] == 1): print("Invalid goal point: Aborting") self.landing_transition() return # 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) # 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 = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path] return waypoints
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 plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 8 self.target_position[2] = TARGET_ALTITUDE # TODO: 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]) print(lat0, lon0) # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position global_pos = [self._longitude, self._latitude, self._altitude] # TODO: convert to current local position using global_to_local() local_pos = global_to_local(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) # 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 grid_start = (int(local_pos[0]-north_offset), int(local_pos[1]- east_offset)) # Set goal as some arbitrary position on the grid grid_goal = (-north_offset + 10, -east_offset + 10) # TODO: adapt to set goal as latitude / longitude position and convert goal_pos = global_to_local([-122.400886, 37.795174, self._altitude], self.global_home) grid_goal = (int(goal_pos[0]-north_offset), int(goal_pos[1]-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) path, _ = a_star(grid, heuristic, grid_start, grid_goal) # TODO: prune path to minimize number of waypoints path = prune_path(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 plan_path(self, start, goal): if not self._graph: h = max(self._plib.get_height_at_point(start), self._plib.get_height_at_point(goal)) self.create_voronoi(h) start = (start[0], start[1]) goal = (goal[0], goal[1]) # Find the nearest node on the graph to the start position & add it # to the graph: near_start = self._find_nearest_point(start) print('Adding starting edge to graph: {} <-> {}'.format( start, near_start)) self._graph.add_edge( start, near_start, weight=LA.norm(np.array(start) - np.array(near_start))) # Do the same for the goal position: near_goal = self._find_nearest_point(goal) print('Adding goal edge to graph: {} <-> {}'.format(near_goal, goal)) self._graph.add_edge( near_goal, goal, weight=LA.norm(np.array(near_goal) - np.array(goal))) path, _ = a_star(lambda node: graph_get_children(self._graph, node), heuristic, start, goal) print(len(path), path) return path
def main(): print('main') data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) grid, north_offset, east_offset = planning_utils.create_grid(data, 5, 5) print('data: {}'.format(grid.shape)) #grid_start = (-north_offset, -east_offset) # grid_goal = (-north_offset + 50, -east_offset + 50) grid_start = planning_utils.random_free_location_in_grid(grid) grid_goal = planning_utils.random_free_location_in_grid(grid) print('grid_start: {} grid_goal: {}'.format(grid_start, grid_goal)) path, path_cost = planning_utils.a_star(grid, planning_utils.heuristic, grid_start, grid_goal) print('path: {} path_cost: {}'.format(len(path), path_cost)) # path = planning_utils.prune_path(path) # print("pruned_path: {}".format(len(path))) smoothed_path = planning_utils.smooth_path(grid, path) print("smoothed_path: {}".format(len(smoothed_path))) greedy_path = planning_utils.greedy_smooth_path(grid, smoothed_path) print('greedy_path: {}'.format(len(greedy_path))) if True: plt.imshow(grid, cmap='Greys', origin='lower') for i in range(len(path) - 1): p1 = path[i] p2 = path[i + 1] plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'b-') for i in range(len(smoothed_path) - 1): p1 = smoothed_path[i] p2 = smoothed_path[i + 1] plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'g-') for i in range(len(greedy_path) - 1): p1 = greedy_path[i] p2 = greedy_path[i + 1] plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'r-') # plot in x and y plt.plot(grid_start[1], grid_start[0], 'rx') plt.plot(grid_goal[1], grid_goal[0], 'gx') # plt.plot([grid_start[1], grid_goal[1]], [grid_start[0], grid_goal[0]], 'g-') # zoom up to the area # plt.xlim((grid_start[1] - 10, grid_goal[1] + 10)) # plt.ylim((grid_start[0] - 10, grid_goal[0] + 10)) plt.xlabel('EAST') plt.ylabel('NORTH') #plt.show() plt.rcParams['figure.figsize'] = 12, 12 plt.savefig('misc/path2.png')
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 10 SAFETY_DISTANCE = 5 GOAL_LAT = 37.797638 GOAL_LON = -122.394808 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values data = np.loadtxt('colliders.csv', delimiter=',', dtype='U16', usecols = (0, 1)) lat0 = np.float64(data[0, 0].split(' ')[1]) lon0 = np.float64(data[0, 1].split(' ')[2]) # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position global_pos = self.global_position # TODO: convert to current local position using global_to_local() local_pos = global_to_local(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) # 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) # TODO: convert start position to current position rather than map center grid_start = (int(-north_offset + local_pos[0]), int(-east_offset + local_pos[1])) # Set goal as some arbitrary position on the grid # TODO: adapt to set goal as latitude / longitude position and convert local_goal = global_to_local([GOAL_LON, GOAL_LAT, 0], self.global_home) grid_goal = (int(-north_offset + local_goal[0]), int(-east_offset + local_goal[1])) # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) print('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 # TODO (if you're feeling ambitious): Try a different approach altogether! pruned_path = copy.deepcopy(path) for i in range(1, len(path) - 1): p1 = path[i-1] p2 = path[i] p3 = path[i+1] det = p1[0]*(p2[1] - p3[1]) + p2[0]*(p3[1] - p1[1]) + p3[0]*(p1[1] - p2[1]) if det == 0: pruned_path.remove(p2) path = pruned_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 = 7 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values fileptr = open('colliders.csv') latitudeLongitudeArr = fileptr.readline().rstrip().replace('lat0','').replace('lon0','').split(',') lat0 = float(latitudeLongitudeArr[0]) lon0 = float(latitudeLongitudeArr[1]) # 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() north, east, down = 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 = (int(np.ceil(north - north_offset)), int(np.ceil(east - east_offset))) # TODO: convert start position to current position rather than map center if len(self.target_global_position) == 0: lon0t = (lon0 + self.targetlongitudeDelta) lat0t = (lat0 + self.targetlatitudeDelta) print('Target Longitude and latitude: ', lon0t, lat0t) northg, eastg, downg = global_to_local([lon0t,lat0t,self.global_home[2]],self.global_home) else: self.target_global_position[2] = self.global_home[2] northg, eastg, downg = global_to_local(self.target_global_position,self.global_home) # Set goal as some arbitrary position on the grid grid_goal = (int(np.ceil(northg - north_offset)),int(np.ceil(eastg - east_offset))) # 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: ', grid_start, grid_goal) path, _ = a_star(grid, heuristic, grid_start, grid_goal) # TODO: prune path to minimize number of waypoints # TODO (if you're feeling ambitious): Try a different approach altogether! path = collinearity_prune(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 = read_home('colliders.csv') # Set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # Convert to current local position using global_to_local() current_position = [self._longitude, self._latitude, self._altitude] self._north, self._east, self._down = global_to_local( current_position, self.global_home) print('global home: {0}, global pos: {1}, local pos: {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 grid_start = (int(np.ceil(-north_offset + self.local_position[0])), int(np.ceil(-east_offset + self.local_position[1]))) # Set goal as some arbitrary position on the grid local_target_position = global_to_local(self.global_target_position, self.global_home) grid_goal = (int(np.ceil(-north_offset + local_target_position[0])), int(np.ceil(-east_offset + local_target_position[1]))) # 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) # Prune path to minimize number of waypoints pruned_path = prune_path(path) print('Length of pruned path: {}'.format(len(pruned_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 # 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 lat_lon_data = np.loadtxt('colliders.csv', usecols=(0, 1), delimiter=',', dtype='str') # read lat0, lon0 from colliders into floating point values lat0 = float(lat_lon_data[0,0][5:]) lon0 = float(lat_lon_data[0,1][5:]) # set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # retrieve current global position current_global_position = self.global_position # 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: ", current_local_position[0], " ", current_local_position[1]) 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)) # 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 latitude / longitude position and convert lon_goal = -122.3961 lat_goal = 37.7940 goal_global = np.array([lon_goal, lat_goal, 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) # 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) # prune path to minimize number of waypoints pruned_path = prune_path(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 colliders_file = 'colliders.csv' # TODO: read lat0, lon0 from colliders into floating point values lat0, lon0 = read_home(colliders_file) print(f'Home lat : {lat0}, lon : {lon0}') # # # TODO: set home position to (lat0, lon0, 0) self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position local_north, local_east, local_down = global_to_local(self.global_position, self.global_home) print(f'Local => north : {local_north}, east : {local_east}, down : {local_down}') # 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_file, delimiter=',', dtype='Float64', skiprows=3) # 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 = int(np.ceil(local_north - north_offset)) grid_start_east = int(np.ceil(local_east - east_offset)) grid_start = (grid_start_north, grid_start_east) # TODO: convert start position to current position rather than map center # Set goal as some arbitrary position on the grid goal_north, goal_east, goal_alt = global_to_local(self.goal_global_position, self.global_home) grid_goal = (int(np.ceil(goal_north - north_offset)), int(np.ceil(goal_east - east_offset))) # 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: ', grid_start, grid_goal) path, _ = a_star(grid, heuristic, grid_start, grid_goal) # TODO: prune path to minimize number of waypoints path = collinearity_prune(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, 0] for p in path] # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 3 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values # TODO: set home position to (lat0, lon0, 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=3) # Determine offsets between grid and map north_offset = int(np.abs(np.min(data[:, 0]))) east_offset = int(np.abs(np.min(data[:, 1]))) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # Define a grid for a particular altitude and safety margin around obstacles grid = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) # 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(current_local_pos[0]+north_offset), int(current_local_pos[1]+east_offset)) # Set goal as some arbitrary position on the grid grid_goal = (north_offset + 10, east_offset + 10) # TODO: adapt to set goal as latitude / longitude position and convert # 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) # TODO: prune path to minimize number of waypoints # TODO (if you're feeling ambitious): Try a different approach altogether! # Convert path to waypoints waypoints = [(p[0] - north_offset, p[1] - east_offset, TARGET_ALTITUDE + 1) for p in path] # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values file = open('colliders.csv', 'r') reader = csv.reader(file) header = next(reader) lat0, lon0 = [float(header[i].split()[1]) for i, j in enumerate(header)] # TODO: set home position to (lon0, lat0, 0) global_home = (lon0, lat0, 0) print("global home is ", global_home) self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position global_position = (self._longitude, self._latitude, self._altitude) # print(global_position) # TODO: convert to current local position using global_to_local() local_position = global_to_local(global_position, global_home) local_north, local_east, local_down = local_position print(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) # TODO: convert start position to current position rather than map center grid_start = (-north_offset + int(local_north), -east_offset + int(local_east)) # Set goal as some arbitrary position on the grid # TODO: adapt to set goal as latitude / longitude position and convert goal_north, goal_east, goal_alt = global_to_local([-122.400150, 37.796005, TARGET_ALTITUDE], self.global_home) grid_goal = (int(np.ceil((-north_offset + goal_north))), int(np.ceil(-east_offset + goal_east))) # 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) # TODO: prune path to minimize number of waypoints pruned_path = prune_path(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 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): print("Searching for a path ...") self.flight_state = States.PLANNING self.target_position[2] = self.TARGET_ALTITUDE # Read lat0, lon0 from colliders into floating point values, and set home position with open('colliders.csv', 'r') as f: latlon_string = f.readline() f.close() m = re.search('lat0 (?P<lat>-*\d+.\d+), lon0 (?P<lon>-*\d+.\d+)\n', latlon_string) lat0, lon0 = [m.group('lat'), m.group('lon')] self.set_home_position(lon0, lat0, 0) # Convert current posiiton to local frame 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 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)) # Define starting point on the grid (this is just grid center) grid_start = local_to_grid(local_position, north_offset, east_offset) # Set goal as some arbitrary position on the grid local_goal = global_to_local(self.GOAL_LATLON, self.global_home) grid_goal = local_to_grid(local_goal, north_offset, 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) pruned_path = prune(path) # Convert path to waypoints print("Path computed, publishing waypoints...") waypoints = [[ p[0] + north_offset, p[1] + east_offset, self.TARGET_ALTITUDE, 0 ] for p in pruned_path] # Set self.waypoints self.waypoints = waypoints self.send_waypoints()
def button_callback(event): global pp path, cost = a_star(grid=grid, start=home_grid, goal=goal_grid) pp = np.array(path) # ax.imshow(grid, cmap='Greys') ax.plot(pp[:, 1], pp[:, 0], linewidth=3) pruned_path = np.array(prune_path(path)) ax.plot(pruned_path[:, 1], pruned_path[:, 0], 'g') ax.scatter(pruned_path[:, 1], pruned_path[:, 0]) plt.draw()
def get_medial_axis_path(grid, grid_goal, grid_start): skeleton = medial_axis(invert(grid)) skel_start, skel_goal = find_start_goal(skeleton, np.array(grid_start), np.array(grid_goal)) # path, _ = a_star(grid, heuristic, skel_start, skel_goal) path, _ = a_star( invert(skeleton).astype(np.int), euclidean_distance, skel_start, skel_goal) path = path + [grid_goal] return path, skeleton
def plan_path(self): def local_to_grid(local_coord): """ Convert a local frame to a grid frame location """ return (int(np.ceil(local_coord[0] - north_offset)), int(np.ceil(local_coord[1] - east_offset))) self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE self.load_home('colliders.csv') local = 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 grid_start = local_to_grid(local) # Set goal as some arbitrary position on the grid grid_goal = (grid_start[0] + 10, grid_start[1] - 10 ) #(-north_offset + 10, -east_offset + 10) # 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: ', grid_start, grid_goal) path, _ = a_star(grid, heuristic, grid_start, grid_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 = [[ 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 = 6 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE #posX,posY,posZ,halfSizeX,halfSizeY,halfSizeZ # TODO: read lat0, lon0 from colliders into floating point values lat_lon = pd.read_csv('colliders.csv') lat = float(lat_lon.columns[0][5:]) lon = float(lat_lon.columns[1][6:]) # TODO: set home position to (lon0, lat0, 0) self.set_home_position = (lon, lat, 0) print("home position set to {0}, {1}".format(lon,lat)) # TODO: retrieve current global position local_north, local_east, local_down = global_to_local(self.global_position, self.global_home) print("local position set to {0}, {1}, {2}".format(local_north,local_east,local_down)) # TODO: convert to current local position using global_to_local() local_coordinates_NED = global_to_local( self.global_position, self.global_home) print('local_coordinates_NED {0}'.format(local_coordinates_NED)) 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 current position rather than map center grid_start = (int(np.ceil(local_north - north_offset)), int(np.ceil(local_east - east_offset))) # Set goal as some arbitrary position on the grid goal = global_to_local(self.goal_position,self.global_home) grid_goal = ( int(np.ceil(goal[0]-north_offset)), int(np.ceil(goal[1]-east_offset))) # 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: ', grid_start, grid_goal) path, _ = a_star(grid, heuristic, grid_start, grid_goal) # TODO: prune path to minimize number of waypoints print('path: ',path) pruned_path = prune_path(path) print('pruned path: ',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 pruned_path] # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # read lat0, lon0 from colliders into floating point values with open('colliders.csv') as f: first_line = f.readline() items = first_line.split(',') lat0 = float(items[0].split()[1]) lon0 = float(items[1].split()[1]) # set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # retrieve current global position print ("current global position:", self.global_position) local_pos = global_to_local(self.global_position, self.global_home) print("current local position", local_pos) 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)) # Set goal: latitude / longitude goal_lon = -122.398470 goal_lat = 37.793445 local_goal = global_to_local((goal_lon, goal_lat, 0), self.global_home) grid_start = (int(local_pos[0])-north_offset, int(local_pos[1])-east_offset) grid_goal = (int(local_goal[0]-north_offset), int(local_goal[1]-east_offset)) # Adapt to set goal as position and convert # Run A* to find a path from start to goal print('Local Start and Goal: ', grid_start, grid_goal) if grid_start[0] != grid_goal[0] or grid_start[1] != grid_goal[1]: path, _ = a_star(grid, heuristic, grid_start, grid_goal) # prune path to minimize number of waypoints pruned_path = self.prune_path(path) print ("prunned path is :", pruned_path) if 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 # 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 = 20 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # Setting the home position based on the latitude and longitude given # on the first line of colliders.csv file = 'colliders.csv' latlon = [] latlon = open(file).readline().split() lat_h, lon_h = float(latlon[1].replace(',', '')), float(latlon[3]) self.set_home_position(lon_h, lat_h, 0.0) global_position = self.global_position # creating a 2D grid from the colliders.csv 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) # Acquiring start and goal position grid_start_latlon = (self._longitude, self._latitude, self._altitude) grid_start_l = global_to_local(grid_start_latlon, self.global_home) grid_start = (int(grid_start_l[0]) - north_offset, int(grid_start_l[1]) - east_offset) goal_lon = input('Goal longitude: ') #goal_lon = -122.396071 goal_lat = input('Goal latitude: ') #goal_lat = 37.793077 goal_position = (float(goal_lon), float(goal_lat), 0.0) grid_goal_l = global_to_local(goal_position, self.global_home) grid_goal = (int(grid_goal_l[0]) - north_offset, int(grid_goal_l[1]) - east_offset) print('Local Start: ', grid_start) print('Local Goal: ', grid_goal) # path planning and pruning using collinearity check and bresenham path, _ = a_star(grid, heuristic, grid_start, grid_goal) pruned_path = prune_path(path, grid) print('Path length: ', len(pruned_path)) print(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 (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values lat0, lon0 = self.get_global_home_from_file() # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position # TODO: 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, 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) # TODO: convert start position to current position rather than map center grid_start = (int(local_position[0])-north_offset, int(local_position[1])-east_offset) # Set goal as some arbitrary position on the grid # grid_goal = (grid_start[0] + 10, grid_start[1] + 10) # TODO: adapt to set goal as latitude / longitude position and convert goal_geo = np.array([-122.401763, 37.795809, 0]) local_goal = global_to_local(goal_geo, self.global_home) grid_goal = (int(local_goal[0])-north_offset, int(local_goal[1])-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) path, _ = a_star(grid, heuristic, grid_start, grid_goal) print('Path length: {0}'.format(len(path))) # TODO: prune path to minimize number of waypoints # TODO (if you're feeling ambitious): Try a different approach altogether! """ I have implemented both Collinearity and Bresenham methods to prune the path. You can call either below to test """ # pruned_path = prune_coll(path) pruned_path = prune_bres(path, grid) print('Pruned-path length: {0}'.format(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 (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 = 20 SAFETY_DISTANCE = 10 self.target_position[2] = TARGET_ALTITUDE #initial localtion in world coordinate frame global_home =(-122.397450,37.792480) lat0 ,lon0 = global_home[1],global_home[0] #intialize the drone position to the global home then that is going to referance the local frame from there local_home (0,0,0) self.set_home_position(lon0,lat0,0) #self.set_home_as_current_position() global_loc = self.global_position local_position = global_to_local(global_loc , 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) grid_start=(int(local_position[0] - north_offset) ,int(local_position[1] -east_offset) ) # Set goal as some arbitrary position on the grid grid_goal = (-north_offset + 750, -east_offset + 370) grid_goal =(int(750) ,int( 370) ) # 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) cleared_path = prune_path(path) # Convert path to waypoints waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in cleared_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 = 4 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values pos = open("colliders.csv").readline().split(",") lat0 = float(pos[0].replace("lat0 ","")) lon0 = float(pos[1].replace("lon0 ","")) # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0.0) # TODO: retrieve current global position #print('global position {}'.format(self.global_position)) # TODO: convert to current local position using global_to_local() local_pos = global_to_local(self.global_position, self.global_home) print('global home {0}, \n position {1}, \n 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)) # convert start position to current position rather than map center grid_start = [int(np.ceil(local_pos[0] - north_offset)), int(np.ceil(local_pos[1] - east_offset))] # Set goal as some arbitrary position on the grid #grid_goal = (-north_offset + 300, -east_offset + 300) # adapt to set goal as latitude / longitude position and convert local_goal_pos = global_to_local(self.global_goal_position, self.global_home) grid_goal = (int(np.ceil(local_goal_pos[0] - north_offset)), int(np.ceil(local_goal_pos[1] - east_offset))) print(grid_goal) # 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) # prune path to minimize number of waypoints reduced_path = prune_path(path) # Convert path to waypoints waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in reduced_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 lat0, lon0 = open('colliders.csv').readline().replace("lat0", "").replace("lon0", "").strip().split(",") lat0 = float(lat0) lon0 = float(lon0) self.set_home_position(lon0, lat0, 0) current_local_position = global_to_local(self.global_position, self.global_home) goal_local_position = global_to_local(self.global_goal, 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 = (int(current_local_position[0]-north_offset), int(current_local_position[1]-east_offset)) # Set goal as some arbitrary position on the grid grid_goal = (int(goal_local_position[0]-north_offset), int(goal_local_position[1]-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(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 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 grid_start = (-north_offset, -east_offset) # Set goal as some arbitrary position on the grid grid_goal = (-north_offset + 10, -east_offset + 10) # 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) # Convert path to waypoints waypoints = [[ p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0 ] for p in path] # Set self.waypoints self.waypoints = waypoints self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = args.altitude SAFETY_DISTANCE = args.safe_dist self.target_position[2] = TARGET_ALTITUDE origin = open('colliders.csv') line = origin.readline().rstrip().split(', ') origin.close() lat0 = float(line[0].split()[1]) lon0 = float(line[1].split()[1]) self.set_home_position(lon0, lat0, 0) local_origin = 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)) 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)) grid_start = (int(local_origin[0] - north_offset), int(local_origin[1] - east_offset)) grid_goal = global_to_local((args.longitude, args.latitude, 0), self.global_home) grid_goal = (int(grid_goal[0] - north_offset), int(grid_goal[1] - east_offset)) print('Local Start and Goal: ', grid_start, grid_goal) path, _ = a_star(grid, heuristic, grid_start, grid_goal) path = prune_path(path) self.waypoints = [[ p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0 ] for p in path] self.send_waypoints()
def proj_min_req(self, data, global_goal, TARGET_ALTITUDE, SAFETY_DISTANCE): grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) 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) print('Local Start and Goal: ', grid_start, grid_goal) path, _ = a_star(grid, heuristic, grid_start, grid_goal) pruned_path = prune_path_bres(path, grid) waypoints = [[ p[0] + north_offset, 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_simple_path(self, target_global, altitude, safety): print("Departing from:", self.local_position) # Use grid to determine path from start -> goal grid_start = (int(self.local_position[0] - self.north_offset), int(self.local_position[1] - self.east_offset)) # Set the target self.landing_position = global_to_local(np.array(target_global), self.global_home) print("Landing at:", self.landing_position) grid_goal = (int(self.landing_position[0] - self.north_offset), int(self.landing_position[1] - self.east_offset)) path, cost = a_star(self.grid._grid, heuristic, grid_start, grid_goal, -self.local_position[2]) print('cur->goal: ', grid_start, grid_goal, cost) # prune path if self.cull: path = prune(path, self.grid, -self.local_position[2]) # and convert to waypoints, climbing if necessary waypoints = [] last_alt = int(-self.local_position[2]) + safety # current altitude prevpoint = None for p in path: climb = (self.grid.get_altitude(p[0], p[1]) + safety) - last_alt if climb < 0: climb = 0 altitude = last_alt + climb last_alt = altitude if prevpoint is not None: heading = self.compute_heading(prevpoint, (p[0], p[1])) else: heading = 0.0 prevpoint = (p[0], p[1]) waypoints.append([ p[0] + self.north_offset, p[1] + self.east_offset, int(altitude), heading ]) return waypoints
def plan_path_grid(self, TARGET_ALTITUDE, SAFETY_DISTANCE): self.read_in() # convert to current local position using global_to_local() 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)) # Make grid given a particular altitude and margin around obstacles grid, north_offset, east_offset = create_grid(self.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) # convert start position to current position rather than map center grid_start = (int(local_pos[0]) + grid_start[0], int(local_pos[1]) + grid_start[1]) # Set goal as lat lon and convert to local coordinates (from bottom left) grid_goal_global = (-122.397450, 37.79380, 0) grid_goal = global_to_local(grid_goal_global, self.global_home) grid_goal = (int(grid_goal[0]) - north_offset, int(grid_goal[1]) - east_offset) # Run A* and prune path path, _ = a_star(grid, heuristic, grid_start, grid_goal) path = prune_path_grid(path, grid) # Convert path to waypoints and send to simulator waypoints = [[ p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0 ] for p in path] self.waypoints = waypoints
def media_axis_method(grid, grid_start, grid_goal, check_linear=True): #check_linear specify how we do path pruning, if True, we choose colinearity #if False, we use bresenham, usually bresenham yield a much better result skeleton = medial_axis(invert(grid)) skel_start, skel_goal = find_start_goal(skeleton, grid_start, grid_goal) path, _ = a_star( invert(skeleton).astype(np.int), heuristic, tuple(skel_start), tuple(skel_goal)) if len(path) == 0: # print("warning, no path is found, please select another point") return path #insert the start and end point if necessary if tuple(skel_start) != grid_start: path.insert(0, grid_start) if tuple(skel_goal) != grid_goal: path.append(grid_goal) #prune the path print("path point num = {}, path={}".format(len(path), path)) path = prune_path(path, grid=grid, check_linear=check_linear) print("pruned path point num = {}, path={}".format(len(path), path)) path = np.array(path).astype(int).tolist() return path, skeleton, grid_start, grid_goal
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values # read the first line and extract only the latitude and longitude vals with open('colliders.csv', 'r') as f: temp = f.readline() lat, lon = temp.replace('lat0 ', '').replace('lon0', '').split(', ') lat, lon = float(lat), float(lon) # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon, lat, 0) # TODO: retrieve current global position curr_global_pos = self.global_position # TODO: convert to current local position using global_to_local() curr_local_pos = global_to_local(curr_global_pos, self.global_home) print("Current Local position {}".format(curr_local_pos)) # start_local = int(curr_local_pos[0]), int(curr_local_pos[1]) #int(start_local[0]), int(start_local[1]) # print(start_local, end_local) print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format(north_offset, east_offset)) # # Define starting point on the grid (this is just grid center) # grid_start = start_local # # TODO: convert start position to current position rather than map center grid_start = (int(curr_local_pos[0]-north_offset), int(curr_local_pos[1]-east_offset)) # # Set goal as some arbitrary position on the grid # grid_goal = end_local # TODO: adapt to set goal as latitude / longitude position and convert end_geo = self.goal end_local = global_to_local(end_geo, self.global_home) grid_goal = end_local[0]-north_offset, end_local[1]-east_offset grid_goal = int(grid_goal[0]), int(grid_goal[1]) # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) print('Grid Start and Goal: ', grid_start, grid_goal) # TODO (if you're feeling ambitious): Try a different approach altogether! if self.planner == 1: path, _ = a_star(grid, heuristic, grid_start, grid_goal) # TODO: prune path to minimize number of waypoints path = prune_path(path) else: if self.planner == 2: sampler = Sampler(data) polygons = sampler._polygons # Example: sampling 100 points and removing # ones conflicting with obstacles. nodes = sampler.sample(300) print(nodes[0]) elif self.planner == 3: pass elif self.planner == 4: pass #create the graph and calculate the a_star t0 = time.time() print('building graph ... ', ) g = create_graph(nodes, 10, polygons) print('graph took {0} seconds to build'.format(time.time()-t0)) start = closest_point(g, (grid_start[0], grid_start[1], TARGET_ALTITUDE)) goal = closest_point(g, (grid_goal[0], grid_goal[1], TARGET_ALTITUDE)) print(start, goal) # print(start, start_ne) print('finding path ... ', ) path, cost = a_star_graph(g, heuristic, start, goal) print('done. path size and cost: {}'.format((len(path), cost))) # print(len(path), path) # path_pairs = zip(path[:-1], path[1:]) # Convert path to waypoints waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path] print(waypoints) # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()