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 In lat0, lon0 From CSV Into Floating Point Values -- First Line with open('colliders.csv') as f: first_line = f.readline() lat_lon=[] for col in [x.strip() for x in first_line.split(',')]: lat_lon.append(float(col.split(' ')[1])) # Set Home Position To (lon0, lat0, 0) # Debugging: Print Home Position self.set_home_position(lat_lon[1], lat_lon[0], 0) print("Home Position - Longitude, Latitude, Altitude: ", self.global_home) # Retrieve Current Global Position # Debugging: Print Global Position global_pos = self.global_position print("Global Position- Longitude, Latitude, Altitude: ", global_pos) # Convert To Current Local Position Using global_to_local() # Debugging: Print Local Position local_pos = global_to_local(global_pos, self.global_home) # Print Global Home, Global Position, Local Position print('Global Home: {0} \nGlobal Position: {1} \nLocal 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 Grid For A Particular Altitude And Safety Distance 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 Grid (Grid Center) grid_start = (-north_offset, -east_offset) # Tmp Goal goal_lat = 37.795049 goal_lon = -122.396362 # 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 Grid grid_goal = (-north_offset+50, -east_offset+50) # Adapt To Set Goal As Lat/Lon Position And Convert global_goal = [-122.396362, 37.795049, self._altitude] local_goal = global_to_local(global_goal, 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 print('Local Start And Goal: ', grid_start, grid_goal) path, _ = a_star(grid, heuristic, grid_start, grid_goal) # Prune Path 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 # Send Waypoints To Sim (For Visualization Of Waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 7 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values with open('colliders.csv', 'r') as f: first_line = f.readline().strip('\n') fl_list = first_line.split() lat0 = float(fl_list[1].strip(',')) lon0 = float(fl_list[3]) # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position self.global_position[0] = self._longitude self.global_position[1] = self._latitude self.global_position[2] = self._altitude # TODO: convert to current local position using global_to_local() local_position = global_to_local(self.global_position, self.global_home) self._north = local_position[0] self._east = local_position[1] self._down = 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) # TODO: convert start position to current position rather than map center start_north = int(self._north) - north_offset start_east = int(self._east) - east_offset grid_start = (start_north, start_east) # Set goal as some arbitrary position on the grid, last used for diag tests # grid_goal = (start_north - 5, start_east + 5) # TODO: adapt to set goal as latitude / longitude position and convert # updated to include parsed args (for srource see line 2) 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))) # 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 # or move to a different search space such as a graph (not done here) print('Local Start and Goal: ', grid_start, grid_goal) print('local._north and local._east: ', self._north, self._east) ori_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! # added prune_path to planning_utlis pruned_path = prune_path(ori_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 = 10 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values # The first line of colliders.csv provides the latitude and longitude that we will call home f = open('colliders.csv') line_list = f.readline().rstrip().split(', ') line_list_lat = line_list[0].split() line_list_lon = line_list[1].split() lat0 = float(line_list_lat[1]) lon0 = float(line_list_lon[1]) # TODO: set home position to (lon0, lat0, 0) # After extracting those latitude and longitude values from the file, we actually set it as the home. self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position # Let's gather where (latitude and longitude) we are on the map (e.g. using GPS) # current_position_global = self.global_position current_position_global = [ self._longitude, self._latitude, self._altitude ] # TODO: convert to current local position using global_to_local() # Now, let's determine how far (north and east) we are from the home position current_position_local = global_to_local(current_position_global, self.global_home) # current_position_local = self.local_position current_position_local_north = int(np.round(current_position_local[0])) current_position_local_east = int(np.round(current_position_local[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)) # 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 # Subtract offets from current local position, rather than the local home position grid_start = (current_position_local_north - north_offset, current_position_local_east - 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 # grid_goal = (current_position_local_north + grid_goal[0], current_position_local_east + grid_goal[1]) # Define a goal latitude and longitude, convert to the local frame, and then adjust by the offsets goal_position_global = (-122.396855, 37.794185, 0) goal_position_local = global_to_local(goal_position_global, self.global_home) goal_position_local_north = int(np.round(goal_position_local[0])) goal_position_local_east = int(np.round(goal_position_local[1])) grid_goal = (goal_position_local_north - north_offset, goal_position_local_east - east_offset) # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) print('Local Start and Goal: ', grid_start, grid_goal) 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! # We send the path generated by A* to the path pruner. 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 # TODO: read lat0, lon0 from colliders into floating point values with open('colliders.csv', newline='') as f: reader = csv.reader(f) row1 = next(reader) lat = re.findall("\d+\.\d+", row1[0]) lon = re.findall("[-+]\d+\.\d+", row1[1]) lat0 = [float(s) for s in lat] lon0 = [float(s) for s in lon] # TODO: set home position to (lon0, lat0, 0) global_home = [lon0, lat0, 0] # TODO: retrieve current global position # TODO: convert to current local position using global_to_local() current_local_position = global_to_local(self.global_position, self.global_home) # print(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_start1 = (int(current_local_position[0] - north_offset), int(current_local_position[1] - east_offset)) # printf("{}".format(self.grid_start)) # geodetic_current_coordinates = local_to_global(grid_goal, geodetic_home_coordinates) # Set goal as some arbitrary position on the grid grid_goal = (-north_offset + 50, -east_offset + 50) # TODO: adapt to set goal as latitude / longitude position and convert geodetic_goal = (-122.39632967, 37.79755765, 0.) # getting error # geodetic_current_coordinates = local_to_global(self.grid_goal,self.global_home ) # print(geodetic_current_coordinates) # 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 IN 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) path, _ = a_star(grid, heuristic, grid_start, grid_goal) # TODO: prune path to minimize number of waypoints p1 = prune_path(path) print(len(p1)) print(p1) # 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 p1] # 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): """ path_planing function """ self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values with open('colliders.csv') as f: origin_pos_data = f.readline().split(',') lat0 = float(origin_pos_data[0].strip().split(' ')[1]) lon0 = float(origin_pos_data[1].strip().split(' ')[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 print(global_position) print(lon0, lat0) # TODO: convert to current local position using global_to_local() local_position = global_to_local(global_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 self.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.data, TARGET_ALTITUDE, SAFETY_DISTANCE) print(grid.shape) 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_position[0]) - north_offset, int(local_position[1]) - east_offset) # Set goal as some arbitrary position on the grid #grid_goal = (-north_offset + 10, -east_offset + 10) # TODO: adapt to set goal as latitude / longitude position and convert #global_goal = (-122.397850, 37.792580, 0.0) # (37.792480, -122.397450) global_goal = self.global_goal local_goal = global_to_local(global_goal, self.global_home) print(local_goal) grid_goal = (int(local_position[0] - north_offset + local_goal[0]), int(local_position[1] - east_offset + local_goal[1])) #grid_goal = (290, 445) # 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): 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 = read_home('colliders.csv') # 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_north, local_east, _ = global_to_local(self.global_position, self.global_home) print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format(north_offset, east_offset)) # Define starting point on the grid (this is just grid center) grid_start = (-north_offset, -east_offset) # TODO: convert start position to current position rather than map center grid_start = (int(np.ceil(grid_start[0] + local_north)), int(np.ceil(grid_start[1] + local_east))) # 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 global_goal_position = np.array([-122.395148, 37.794142, 5]) local_goal_north, local_goal_east, _ = global_to_local(global_goal_position, self.global_home) print("Local Grid Goal : " + str(local_goal_north) + "-" + str(local_goal_east)) grid_goal = (int(np.ceil(local_goal_north + grid_goal[0])), int(np.ceil(local_goal_east + grid_goal[1]))) print("Grid Goal : " + str(grid_goal[0]) + "-" + str(grid_goal[1])) plt.imshow(grid, origin='lower') plt.plot(grid_start[0], grid_start[1], 'rx') plt.plot(grid_goal[0], grid_goal[1], 'bx') #plt.plot(grid_goal[0], grid_goal[1], 'bx') plt.show() # 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): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 6 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values with open('colliders.csv') as f: s = f.readline() s = s.replace(',', ' ') data = s.split() lat0 = float(data[1]) lon0 = float(data[3]) print(lat0, lon0) # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position self._global_position = [self._latitude, self._longitude, self._altitude] # TODO: convert to current local position using global_to_local() self._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, 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(self.local_position[0])-north_offset, int(self.local_position[1])-east_offset) # Set goal as some arbitrary position on the grid #grid_goal = (-north_offset + 10, -east_offset + 10) # TODO: adapt to set goal as latitude / longitude position and convert #LAT=37.797337 lON = -122.401122 goal_gps = [-122.401122, 37.797337, 0] #goal_gps = [-122.397629, 37.793520, 0] goal_local = global_to_local(goal_gps, self.global_home) grid_goal = (int(goal_local[0])-north_offset, int(goal_local[1])-east_offset) """ #Show goal as location seems weird plt.imshow(grid, cmap='Greys', origin='lower') plt.plot(grid_start[1], grid_start[0], 'x') plt.plot(grid_goal[1], grid_goal[0], 'x') plt.xlabel('EAST') plt.ylabel('NORTH') plt.show() """ # 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) print('Start A*') path, _ = a_star(grid, heuristic, grid_start, grid_goal) # TODO: prune path to minimize number of waypoints print('Pruning') pruned_path = prune_path(path) plt.imshow(grid, cmap='Greys', origin='lower') # For the purposes of the visual the east coordinate lay along # the x-axis and the north coordinates long the y-axis. plt.plot(grid_start[1], grid_start[0], 'x') plt.plot(grid_goal[1], grid_goal[0], 'x') if path is not None: pp = np.array(path) plt.plot(pp[:, 1], pp[:, 0], 'g') plt.xlabel('EAST') plt.ylabel('NORTH') plt.show() # 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] for i in range(1, len(waypoints)): print(i) wp1 = waypoints[i-1] wp2 = waypoints[i] # Set heading of wp2 based on relative position to wp1 waypoints[i][3] = np.arctan2((wp2[1]-wp1[1]), (wp2[0]-wp1[0])) # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values lat0 = 0.0 lon0 = 0.0 with open('colliders.csv', 'r') as f: lines = f.readlines() head = lines[0] half_h = head.split(',') lat0 = float(half_h[0].strip().split(' ')[1]) lon0 = float(half_h[1].strip().split(' ')[1]) # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position my_global_position = self.global_position # TODO: convert to current local position using global_to_local() my_local_position = global_to_local(my_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) # TODO: convert start position to current position rather than map center grid_start = (int(my_local_position[0] - north_offset), int(my_local_position[1] - east_offset)) # Set goal as some arbitrary position on the grid # TODO: adapt to set goal as latitude / longitude position and convert global_goal = (-122.397450 - 0.0017, 37.792480 + 0.0038, 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)) # 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) # TODO: prune path to minimize number of waypoints print('Local Start and Goal: ', grid_start, grid_goal) path, _ = a_star(grid, heuristic, grid_start, grid_goal) print('Initial path ', len(path), ' points') path = prune_path(path) print('After pruning path: ', len(path), ' points') l = len(path) dl = 10 while dl != 0: path = pick_uncrossed(grid, path) dl = l - len(path) l = len(path) print('After removing uncrossed points: ', len(path), ' points') # TODO (if you're feeling ambitious): Try a different approach altogether! # Convert path to waypoints waypoints = [] for p in path: waypoints.append( [p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0]) if len(waypoints) > 1: waypoints[-1][3] = np.arctan2( (waypoints[-1][1] - waypoints[-2][1]), (waypoints[-1][0] - waypoints[-2][0])) # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values 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(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # ------------------------------------- # YW [1]: read lat0, lon0 from colliders into floating point values # ------------------------------------- with open('colliders.csv') as f: first_line = f.readline() geo_lat_lon = [] for col in [x.strip() for x in first_line.split(',')]: geo_lat_lon.append(float(col.split(' ')[1])) print("Global home coordinate in Lat-Long: ", geo_lat_lon) # YW: set home position to (lon0, lat0, 0). Beware of the lat0, lon0 in collider.csv! self.set_home_position(geo_lat_lon[1], geo_lat_lon[0], 0) # ------------------------------------- # YW [2]: retrieve current global position # ------------------------------------- global_pose = self.global_position print("Current global pose (lon, lat, alt): ", global_pose) print("Global home coord (lon, lat, alt): ", self.global_home) print("\n") # YW: convert to current local position using global_to_local() cur_pose_local = global_to_local(global_pose, self.global_home) print("------------------------") print("local pose: ", cur_pose_local) print('global home {0} \nglobal position {1} \nlocal position {2}'. format(self.global_home, self.global_position, self.local_position)) print("------------------------\n") # ------------------------------------- # YW [3]: Read in obstacle map # ------------------------------------- data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) int_cur_pose_local = cur_pose_local.astype(int) print("local int_cur_pose_local: ", int_cur_pose_local) global_goal = [-122.396580, 37.796085, self._altitude] print("global_goal (long-lat-alt): ", global_goal) local_goal = global_to_local(global_goal, self.global_home) int_local_goal = local_goal.astype(int) print("local_goal : ", int_local_goal) # Define a grid for a particular altitude and safety margin around obstacles grid, north_min, east_min = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North min = {0}, east min = {1}".format(north_min, east_min)) # plt.imshow(grid, origin='lower') # plt.xlabel('EAST') # plt.ylabel('NORTH') # plt.show() grid_start = (int_cur_pose_local[0] - north_min, int_cur_pose_local[1] - east_min) # YW: convert start position to current position rather than map center # Don't add tuple and numpy array print("grid_start: ", grid_start) # Set goal as some arbitrary position on the grid # ------------------------------------- # YW: adapt to set goal as latitude / longitude position and convert # use the manual mode to get the appropriate goal coordinate # ------------------------------------- # convert the local NORTH EAST direction to the grid map grid_goal = (int_local_goal[0] - north_min, int_local_goal[1] - east_min) #grid_goal = (-north_min + 20, - east_min ) print("grid_goal: ", grid_goal) print("Grid size: ", grid.shape) # Run A* to find a path from start to goal # YW: 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) -> planning_utils print('Local Start and Goal: ', grid_start, " -> ", grid_goal) print("------------------------\n") if self.mode == 'grid': path, _ = a_star(grid, heuristic, grid_start, grid_goal) path = prune_path(path) print("Path: ", path) # Convert path to waypoints waypoints = [[ p[0] + north_min, p[1] + east_min, TARGET_ALTITUDE, 0 ] for p in path] print("Waypoints: ", waypoints) elif self.mode == "graph": # Use KdSampler instead of create grid graph_start = tuple(int_cur_pose_local.tolist()) graph_goal = tuple(int_local_goal.tolist()) print("Start: {0}, goal: {1}".format(graph_start, graph_goal)) kdtree_sampler = prmap.KdSampler(data, 140, graph_start, graph_goal, SAFETY_DISTANCE) print("Number of samples: ", len(kdtree_sampler._kept_samples)) print("Number of rejected samples: ", len(kdtree_sampler._removed_samples)) t0 = time.time() # YW NOTE: given a large amount of k allows the algorithm to connect # the nodes that has a long distance g = prmap.create_graph(kdtree_sampler, 10) print('graph took {0} seconds to build'.format(time.time() - t0)) path, cost = prmap.a_star_graph(g, heuristic, graph_start, graph_goal) print("A* graph length {0}, path: {1} \ncost: {2}".format( len(path), path, cost)) prmap.visualize_graph(grid, data, g, kdtree_sampler, path) # Convert path to waypoints waypoints = [[p[0], p[1], TARGET_ALTITUDE, 0] for p in path] else: print("Error: Unknown planning mode!") print("Waypoints: ", waypoints) # Set self.waypoints self.waypoints = waypoints # send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 10 SAFETY_DISTANCE = 2 self.target_position[2] = TARGET_ALTITUDE # read lat0, lon0 from colliders into floating point values first_line = open('colliders.csv').readline() lat_lon = dict(val.strip().split(' ') for val in first_line.split(',')) # set home position to (lon0, lat0, 0) self.set_home_position(float(lat_lon['lon0']), float(lat_lon['lat0']), 0) # retrieve current global position # convert to current local position using global_to_local() local_position = global_to_local(self.global_position, self.global_home) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles # Create Voronoi graph from obstacles grid, edges, north_offset, east_offset = create_grid_and_edges( data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # Define starting point on the grid # convert start position to current position rather than map center grid_start = (-north_offset + int(local_position[0]), -east_offset + int(local_position[1])) # Set goal as some arbitrary position on the grid # adapt to set goal as latitude / longitude position and convert # some possible destinations on the map destinations = [[-122.39324423, 37.79607305], [-122.39489652, 37.79124152], [-122.40059743, 37.79272177], [-122.39625334, 37.79478161]] # randomly choose one of destinations destination = destinations[np.random.randint(0, len(destinations))] destination_local = global_to_local( [destination[0], destination[1], TARGET_ALTITUDE], self.global_home) # adjust destination coordinates on the grid grid_goal = (-north_offset + int(destination_local[0]), -east_offset + int(destination_local[1])) print('Local Start and Goal: ', grid_start, grid_goal) # create Voronoi graph with the weight of the edges # set to the Euclidean distance between the points voronoi_graph = nx.Graph() for e in edges: p1 = e[0] p2 = e[1] dist = np.linalg.norm(np.array(p2) - np.array(p1)) voronoi_graph.add_edge(p1, p2, weight=dist) # find the nearest points on the graph for start and the goal voronoi_start = nearest_point(grid_start, voronoi_graph) voronoi_finish = nearest_point(grid_goal, voronoi_graph) # run A-star on the graph voronoi_path, voronoi_cost = a_star_graph(voronoi_graph, heuristic, voronoi_start, voronoi_finish) voronoi_path.append(grid_goal) # prune path - from Lesson 6.5 print('Original path len: ', len(voronoi_path)) voronoi_path = prune_path(voronoi_path) print('Pruned path len: ', len(voronoi_path)) # Convert path to waypoints waypoints = [[ int(p[0]) + north_offset, int(p[1]) + east_offset, TARGET_ALTITUDE, 0 ] for p in voronoi_path] # Set self.waypoints self.waypoints = waypoints # send waypoints to sim self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values with open('colliders.csv') as f: line = np.array(f.readline().replace(',', ' ').split()) line = line[[1,3]] lat0, lon0 = np.asarray(line, dtype=np.float64) # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0.0) # TODO: retrieve current global position current_global = self.global_position # TODO: convert to current local position using global_to_local() current_local = global_to_local(current_global, 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)) print("grid shape", grid.shape) # 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(current_local[0])-north_offset, int(current_local[1])-east_offset) print("grid_start", grid_start) # 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 lat_goal = 37.794766 #37.796077 lon_goal = -122.399456 #-122.398163 goal_global = [lon_goal, lat_goal, 0.] goal_local = global_to_local(goal_global, self.global_home) #grid_goal = (goal_local[0], goal_global[1]) grid_goal = (int(goal_local[0])-north_offset, int(goal_local[1])-east_offset) print("grid_goal", 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 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 = prune_path(path) path = bresenham_prune(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 ...") #NearBy # TARGET_LAT = 37.792653 # TARGET_LON = -122.397075 #around the corner TARGET_LAT = 37.792504 TARGET_LON = -122.397980 TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # TODO: read lat0, lon0 from colliders into floating point values print( "# TODO: read lat0, lon0 from colliders into floating point values" ) homeGPSLat, homeGPSLon = self.getFirstRowOfCSV() print("homeGPSLon = " + str(homeGPSLon)) print("homeGPSLat = " + str(homeGPSLat)) # TODO: set home position to (lon0, lat0, 0) print("# TODO: set home position to (lon0, lat0, 0)") self.set_home_position(homeGPSLon, homeGPSLat, 0) print("# home position set") # TODO: retrieve current global position print("# TODO: retrieve current global position") currentGPSLat = self._latitude currentGPSLon = self._longitude currentGPSAlt = self._altitude print("currentGPSLon = " + str(currentGPSLon)) print("currentGPSLat = " + str(currentGPSLat)) print("currentGPSAlt = " + str(currentGPSAlt)) # TODO: convert to current local position using global_to_local() print( "# TODO: convert to current local position using global_to_local()" ) geodetic_current_coordinates = [ currentGPSLon, currentGPSLat, currentGPSAlt ] geodetic_home_coordinates = [homeGPSLon, homeGPSLat, 0] print("geodetic_current_coordinates " + str(geodetic_current_coordinates)) print("geodetic_home_coordinates " + str(geodetic_home_coordinates)) local_coordinates_NED = global_to_local(geodetic_current_coordinates, geodetic_home_coordinates) print("local_coordinates_NED " + str(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) # print("original grid_start " + str(grid_start)) # TODO: convert start position to current position rather than map center print( "# TODO: convert start position to current position rather than map center" ) grid_start = (int(-north_offset + local_coordinates_NED[0]), int(-east_offset + local_coordinates_NED[1])) print("Overriding start location to " + str(grid_start)) # Set goal as some arbitrary position on the grid # grid_goal = (-north_offset + 10, -east_offset + 20) # TODO: adapt to set goal as latitude / longitude position and convert geodetic_goal_cordinates = [TARGET_LON, TARGET_LAT, TARGET_ALTITUDE] # center of MAP print("geodetic_goal_cordinates " + str(geodetic_goal_cordinates)) goal_coordinates_NED = global_to_local(geodetic_goal_cordinates, geodetic_home_coordinates) print("goal_coordinates_NED " + str(goal_coordinates_NED)) grid_goal = (int(-north_offset + goal_coordinates_NED[0]), int(-east_offset + goal_coordinates_NED[1])) print("grid_goal" + str(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 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 len before pruning " + str(len(path))) path = prune_path(path) print("path len after pruning " + str(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 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 filename = 'colliders.csv' f = open(filename) line1 = f.readline() f.close() print(line1) line1 = line1.split(" ") lat0 = np.float(line1[1][:-1]) lon0 = np.float(line1[3]) print("lat0", lat0) print("lon0", lon0) # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0.0) # TODO: retrieve current global position current_global_position = self.global_position # TODO: convert to current local position using global_to_local() current_local_position = global_to_local(self.global_position, self.global_home) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # Define starting point on the grid (this is just grid center) grid_start = (-north_offset, -east_offset) # TODO: 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 #grid_goal = (-north_offset + 10, -east_offset + 10) # TODO: adapt to set goal as latitude / longitude position and convert # Corner of Front St & Market st goal_lon = -122.398494 goal_lat = 37.791558 goal_home_position = (goal_lon, goal_lat, 0) goal_local_position = global_to_local(goal_home_position, self.global_home) 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 # 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 = prune_path(path) print('Pruned Path:', pruned_path) # Convert path to waypoints waypoints = [[ p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0 ] for p in pruned_path] # 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 filename = 'colliders.csv' with open(filename) as f: origin_pos_data = f.readline().split(',') lat0 = float(origin_pos_data[0].strip().split(' ')[1]) lon0 = float(origin_pos_data[1].strip().split(' ')[1]) # set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # retrieve current global position current_global_position = [ self._longitude, self._latitude, self._altitude ] # convert to current local position using global_to_local() current_local_position = global_to_local(current_global_position, self.global_home) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # 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.round(current_local_position[0])) - north_offset, int(np.round(current_local_position[1])) - east_offset) # Set goal as some arbitrary position on the grid grid_goal = (-north_offset + 10, -east_offset + 10) # TODO: adapt to set goal as latitude / longitude position and convert # Path not found # goal_lon = -122.396342 # goal_lat = 37.796520 # OK # goal_lon = -122.397532 # goal_lat = 37.792973 goal_lon = -122.399219 goal_lat = 37.794262 goal_global = (goal_lon, goal_lat, 0) goal_local = global_to_local(goal_global, self.global_home) grid_goal = (int(np.round(goal_local[0])) - north_offset, int(np.round(goal_local[1])) - east_offset) # Run A* to find a path from start to 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) path, _ = a_star(grid, heuristic_grid, grid_start, grid_goal) # prune path to minimize number of waypoints print('prunning path', path) pruned_path = prune_path(path) print('prunned path', pruned_path) # Convert path to waypoints waypoints = [[p[0] + north_offset, p[1] + east_offset, 5, 0] for p in pruned_path] print(waypoints) # Set self.waypoints self.waypoints = waypoints # send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE #read lat0, lon0 from colliders into floating point values lat0, lon0 = self.get_home() #set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # TODO: convert to current local position using global_to_local() local_north, local_east, _ = 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 # convert start position to current position rather than map center grid_start = (int(local_north) - north_offset, int(local_east) - east_offset) # Set goal as some arbitrary position on the grid #set goal as latitude / longitude position and convert goal_lat = 37.79456 goal_lon = -122.397437 local_goal = global_to_local((goal_lon, goal_lat, 0), self.global_home) # convert to grid coordinates grid_goal = (int(local_goal[0]) - north_offset, int(local_goal[1]) - east_offset) # Run A* to find a path from start to goal # I added diagonal motions with a cost of sqrt(2) to your A* implementation print('Local Start and Goal: ', grid_start, grid_goal) path, _ = a_star(grid, heuristic, grid_start, grid_goal) print('Path:', path) # prune path to minimize number of waypoints prunned_path = prune_path(path) print('Prunned path:', prunned_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 # TODO: read lat0, lon0 from colliders into floating point values latitude, longitude = self.get_home_position() # TODO: set home position to (lon0, lat0, 0) self.set_home_position(longitude, latitude, 0.0) # TODO: retrieve current global position # TODO: convert to current local position using global_to_local() current_global_position = np.array( [self._longitude, self._latitude, self._altitude]) self._north, self._east, self._down = global_to_local( current_global_position, self.global_home) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Get a graph representation with edges for a particular altitude and safety margin around obstacles graph, north_offset, east_offset = create_graph( data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # TODO: convert start position to current position rather than map center current_position = self.local_position start = (-north_offset + int(current_position[0]), -east_offset + int(current_position[1])) # Set goal as some arbitrary position on the grid # TODO: adapt to set goal as latitude / longitude position and convert goal_longitude = -122.397745 goal_latitude = 37.793837 #goal_longitude = -122.394324 #goal_latitude = 37.791684 #goal_longitude = -122.401216 #goal_latitude = 37.796713 target_location = global_to_local([goal_longitude, goal_latitude, 0], self.global_home) goal = (int(-north_offset + target_location[0]), int(-east_offset + target_location[1])) # Compute the closest points to the start and goal positions closest_start = closest_point( graph, (-north_offset + int(current_position[0]), -east_offset + int(current_position[1]))) closest_goal = closest_point(graph, (-north_offset + int(target_location[0]), -east_offset + int(target_location[1]))) print("Local Start and Goal : ", start, goal) # Run A* to find a path from start to goal path, _ = a_star_graph(graph, closest_start, closest_goal) print("Found a path of length : ", len(path)) # Check for collinearity between points and prune the obtained path path = prune_path(path) # Convert path to waypoints self.waypoints = [[ p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0 ] for p in path] # TODO: send waypoints to simulator print("Sending waypoints to the simulator") self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 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 """ 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 = 8 # extra margin of safety around obstacles self.target_position[2] = TARGET_ALTITUDE # debug flag debug = True # Requirement #1: # * Read lat0, lon0 from colliders into floating point values # * set home position to (lon0, lat0, 0) lat0, lon0 = get_latlon_fromfile('colliders.csv') self.set_home_position(lon0, lat0, 0) # Requirement #2: # * retrieve current global position # * convert to current local position using global_to_local() curr_global_pos = (self._longitude, self._latitude, self._altitude) curr_global_home = self.global_home curr_local_pos = global_to_local(curr_global_pos, self.global_home) if debug: print("curr_global_pos: ", curr_global_pos) print("curr_global_home: ", curr_global_home) print("curr_local_pos: ", curr_local_pos) # 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) # save offsets as tuple offsets = (north_offset, east_offset) # Requirement #3: convert start position to current position rather than map center grid_start = get_gridrelative_position(curr_local_pos[0:2], offsets) # Requirement #4: set goal as latitude/longitude # specified as command line args custom_goal_pos = (self.goal_longitude, self.goal_latitude, 0) goal_local = global_to_local(custom_goal_pos, self.global_home)[0:2] grid_goal = get_gridrelative_position(goal_local, offsets) # Run A* to find a path from start to goal print('Grid Start and Goal: ', grid_start, grid_goal) print('START: Path planning....') path, _ = a_star(grid, heuristic, grid_start, grid_goal) print('Path planning complete!') # Requirement 6: 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] if debug: print("waypoints:") for i, waypoint in enumerate(waypoints): print("{}: {}".format(i, waypoint)) # Set self.waypoints self.waypoints = waypoints # Send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # Read lat0, lon0 from colliders into floating point values lat0 = lon0 = None with open('colliders.csv') as f: ns = re.findall("-*\d+\.\d+", f.readline()) assert len( ns ) == 2, "Could not parse lat0 & lon0 from 'colliders.csv'. The file might be broken." lat0, lon0 = float(ns[0]), float(ns[1]) self.set_home_position( lon0, lat0, 0) # set home position as stated in colliders.csv # curLocal - is local position of drone relative home) curLocal = global_to_local( (self._longitude, self._latitude, self._altitude), self.global_home) print('global home {0}'.format(self.global_home.tolist())) print('position {0}'.format(self.global_position.tolist())) print('local position {0}'.format(self.local_position.tolist())) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles timer = time.time() grid, offset, edges = create_grid_and_edges(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print('Voronoi {0} edges created in {1}s'.format( len(edges), time.time() - timer)) print('Offsets: north = {0} east = {1}'.format(offset[0], offset[1])) G = nx.Graph() for e in edges: G.add_edge(tuple(e[0]), tuple(e[1]), weight=np.linalg.norm(np.array(e[1]) - np.array(e[0]))) print('Graph created') # Define starting point on the grid as current location grid_start = (-offset[0] + int(curLocal[0]), -offset[1] + int(curLocal[1])) # Set goal as some arbitrary position on the grid # grid_goal = (-offset[0] + 64, -offset[1] + 85) # grid_goal = (290, 720) if self.goal is None: grid_goal = None while not grid_goal: i, j = random.randint(0, grid.shape[0] - 1), random.randint( 0, grid.shape[1] - 1) if not grid[i, j]: grid_goal = (i, j) else: target_global = global_to_local( (self.goal[1], self.goal[0], TARGET_ALTITUDE), self.global_home) grid_goal = (-offset[0] + int(target_global[0]), -offset[1] + int(target_global[1])) print('Path: {0} -> {1}'.format(grid_start, grid_goal)) graph_start = closest_point(G, grid_start) graph_goal = closest_point(G, grid_goal) print('Graph path: {0} -> {1}'.format( (graph_start[0] - offset[0], graph_start[1] - offset[1]), (graph_goal[0] - offset[0], graph_goal[1] - offset[1]))) timer = time.time() path, cost = a_star_graph(G, graph_start, graph_goal) if path is None: print('Could not find path') return print('Found path on graph of {0} waypoints in {1}s'.format( len(path), time.time() - timer)) # Add to path exact (non-voronoi) start&goal waypoints path = [(int(p[0]), int(p[1])) for p in path] path.insert(0, grid_start) path.insert(len(path), grid_goal) # Prune the path on grid twice timer = time.time() pruned_path = prune_path(path, grid) pruned_path = prune_path(pruned_path, grid) print('Pruned path from {0} to {1} waypoints in {2}s'.format( len(path), len(pruned_path), time.time() - timer)) path = pruned_path # Convert path to waypoints waypoints = [[p[0] + offset[0], p[1] + offset[1], TARGET_ALTITUDE, 0] for p in path] # Set self.waypoints self.waypoints = waypoints self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values with open('colliders.csv', newline='') as f: reader = csv.reader(f) row1 = next(reader) for item in row1: if 'lat0' in item: lat0 = np.float(item.strip().split(' ')[1]) elif 'lon0' in item: lon0 = np.float(item.strip().split(' ')[1]) # 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, 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_north = int(np.ceil(local_position[0] + grid_start[0])) grid_start_east = int(np.ceil(local_position[1] + grid_start[1])) grid_start = (grid_start_north, grid_start_east) # 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 goal_lon = -122.40199327 goal_lat = 37.79245808 goal_global_pos = global_to_local((goal_lon, goal_lat, 0), self.global_home) grid_goal_north = int(np.ceil(goal_global_pos[0] + grid_start[0])) grid_goal_east = int(np.ceil(goal_global_pos[1] + grid_start[1])) grid_goal = (grid_goal_north, grid_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 # TODO (if you're feeling ambitious): Try a different approach altogether! pruned_path = prune_path(path) plot_path(grid, pruned_path, north_offset, east_offset, grid_start, grid_goal) # 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 with open('colliders.csv', newline='') as f: r = csv.reader(f) r1 = next(r) # TODO: set home position to (lon0, lat0, 0) lat0 = float((r1[0].strip('lat0'))) lon0 = float((r1[1].strip(' lon0'))) print(lat0,lon0) self.set_home_position(lon0,lat0,0.0) # TODO: retrieve current global position global_position = (self._longitude,self._latitude,self._altitude) # TODO: convert to current local position using global_to_local() local_position = global_to_local(global_position,self.global_home) print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles grid, north_offset, east_offset = 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 n_start = int(local_position[0]) e_start = int(local_position[1]) grid_start = ((n_start + -north_offset),(e_start + -east_offset)) # Set goal as some arbitrary position on the grid grid_goal = (-north_offset + 10, -east_offset + 10) print("north_start:",n_start,"easth_start:",e_start) print ("Grid_Start:",grid_start) # TODO: adapt to set goal as latitude / longitude position and conver goal_lon = -122.396640 goal_lat = 37.796232 goal_global = [goal_lon ,goal_lat , 0] goal_local = global_to_local (goal_global,self.global_home) north_goal = int(goal_local[0]) east_goal = int(goal_local[1]) grid_goal = ((north_goal+-north_offset) ,(east_goal+-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 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): 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 home = np.genfromtxt('colliders.csv', delimiter=',', dtype=None, encoding=None, max_rows=1) home_lat = home[0].split()[1] home_long = home[1].split()[1] # TODO: set home position to (lat0, lon0, 0) self.set_home_position(float(home_long), float(home_lat), 0) # TODO: retrieve current global position # TODO: convert to current local position using global_to_local() current_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 (this is just grid center) # TODO: convert start position to current position rather than map center grid_start = (int(current_local[0]) - north_offset, int(current_local[1]) - east_offset) # Set goal as some arbitrary position on the grid global_goal = (-122.396504, 37.795887, 0) # TODO: adapt to set goal as latitude / longitude position and convert local_goal = global_to_local(global_goal, 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) # 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 print('Found waypoints: {}'.format(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 # Done: read lat0, lon0 from colliders into floating point values f = open('colliders.csv') header = f.readline().strip('\n').split(',') long_lat = {} for h in header: line = h.strip().split(' ') long_lat[line[0]] = np.float(line[1]) print("HOME Position %s", long_lat) # DONE: set home position to (lon0, lat0, 0) self.set_home_position(long_lat['lon0'], long_lat['lat0'], 0) # retrieve current global position # DONE: convert to current local position using global_to_local() local_north, local_east, _ = global_to_local(self.global_position, self.global_home) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # Define starting point on the grid (this is just grid center) # grid_start = (-north_offset, -east_offset) print("LOCAL POS %s %s" % (local_north, local_east)) # DONE: convert start position to current position rather than map center grid_start = (int(local_north - north_offset), int(local_east - east_offset)) # Set goal as some arbitrary position on the grid # DONE: Adapt to set goal as latitude / longitude position and convert local_goal_north, local_goal_east, _ = global_to_local( self._global_goal_position, self.global_home) grid_goal = (int(local_goal_north - north_offset), int(local_goal_east - east_offset)) # Run A* to find a path from start to goal # DONE: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) print('Local Start and Goal: ', grid_start, grid_goal) path, _ = a_star(grid, heuristic, grid_start, grid_goal) # DONE: prune path to minimize number of waypoints path = prune_path(path, grid) print("No. of points %s" % len(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 = 20 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # Read lat0, lon0 from colliders into floating point values with open('colliders.csv', newline='') as f: reader = csv.reader(f) row1 = next(reader) lat0 = float(row1[0].split()[1]) lon0 = float(row1[1].split()[1]) # Set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # Retrieve current global position global_pos = self.global_position # 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) # Create a voxmap (2.5D map) centered at TARGET_ALTITUDE # and with a resolution of: VOXMAP_RES = 10 voxmap, north_offset, east_offset = create_voxmap(data, TARGET_ALTITUDE, SAFETY_DISTANCE, VOXMAP_RES) # Create a 1m3 resolution voxmap (for local planning) self.voxmap, self.north_offset, self.east_offset = create_voxmap(data, TARGET_ALTITUDE, SAFETY_DISTANCE, 1) print("North offset = {0}, east offset = {1}".format(north_offset, east_offset)) # Define starting point on the grid grid_start = (int(local_pos[0]-north_offset), int(local_pos[1]-east_offset), TARGET_ALTITUDE) # Define starting point on the voxmap voxmap_start = (grid_start[0] // VOXMAP_RES, grid_start[1] // VOXMAP_RES, grid_start[2] // VOXMAP_RES) # Set goal as some arbitrary position on the grid/voxmap while True: n_goal = random.randint(0, voxmap.shape[0] - 1) e_goal = random.randint(0, voxmap.shape[1] - 1) alt_goal = random.randint(0, voxmap.shape[2] - 1) if voxmap[n_goal, e_goal, alt_goal] == 0: break # Voxmap goal voxmap_goal = (n_goal, e_goal, alt_goal) # Run A* to find a path from start to goal print('Voxmap Start and Goal: ', voxmap_start, voxmap_goal) # Grid Search #path, _ = a_star(grid, heuristic, grid_start, grid_goal) # Graph Search #path, _ = graph_a_star(G, heuristic, graph_start, graph_goal) # 3D A* Search path, _ = a_star_3D(voxmap, heuristic, voxmap_start, voxmap_goal) # Prune path to minimize number of waypoints path = prune_path(path) print("Path: ", path) # Convert path to waypoints waypoints = [] for i in range(len(path)): p = path[i] if i: last_p = path[i-1] # Set heading based on relative position to last wp heading = np.arctan2((p[1]-last_p[1]), (p[0]-last_p[0])) else: heading = 0 # Append waypoint waypoints.append([p[0] * VOXMAP_RES + north_offset, p[1] * VOXMAP_RES + east_offset, p[2] * VOXMAP_RES, heading]) print("Waypoints: ", waypoints) # Set self.global_waypoints self.global_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 = 3 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values with open('colliders.csv', newline='') as f: reader = csv.reader(f) row1 = next(reader) # gets the first line lat0, lon0 = float(row1[0][5:]), float(row1[1][5:]) # TODO: set home position to (lat0, lon0, 0) self.set_home_position( lon0, lat0, 0) # set the current location to be the home position # TODO: retrieve current global position current_global_pos = (self._longitude, self._latitude, self._altitude) # TODO: convert to current local position using global_to_local() current_local_pos = global_to_local(current_global_pos, self.global_home) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Get the center of the grid north_offset = int(np.floor(np.min(data[:, 0] - data[:, 3]))) east_offset = int(np.floor(np.min(data[:, 1] - data[:, 4]))) # Define a grid for a particular altitude and safety margin around obstacles print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # This is now the routine using Voronoi grid_graph, edges = create_grid_and_edges(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("GRAPH EDGES:", len(edges)) # Create the graph with the weight of the edges # set to the Euclidean distance between the points G = nx.Graph() for e in edges: p1 = e[0] p2 = e[1] dist = LA.norm(np.array(p2) - np.array(p1)) G.add_edge(p1, p2, weight=dist) # TODO: convert start position to current position rather than map center grid_start = (int(current_local_pos[0] - north_offset), int(current_local_pos[1] - east_offset)) # TODO: adapt to set goal as latitude / longitude position and convert goal_global_pos = (-122.393010, 37.790000, 0) goal_local_pos = global_to_local(goal_global_pos, self.global_home) grid_goal = (int(goal_local_pos[0] - north_offset), int(goal_local_pos[1] - east_offset)) #goal_ne = (840., 100.) start_ne_g = closest_point(G, grid_start) goal_ne_g = closest_point(G, grid_goal) print("START NE GOAL:", start_ne_g) print("GOAL NE GOAL", goal_ne_g) # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) path, cost = a_star_graph(G, heuristic_graph, start_ne_g, goal_ne_g) print("Path Length:", len(path), "Path Cost:", cost) int_path = [[int(p[0]), int(p[1])] for p in path] # TODO: prune path to minimize number of waypoints pruned_path = prune_path(int_path) print("Length Pruned Path:", len(pruned_path)) # Convert path to waypoints waypoints = [[ p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0 ] for p in pruned_path] # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim self.send_waypoints()
def plan_local_path(self): "Plans a local path inside a local volume around the current location" # Define current position in the (global) voxmap if len(self.local_waypoints): last_wp = self.local_waypoints[-1] current_voxmap = (int(last_wp[0]-self.north_offset), int(last_wp[1]-self.east_offset), int(last_wp[2])) else: current_voxmap = (int(self.local_position[0]-self.north_offset), int(self.local_position[1]-self.east_offset), int(-self.local_position[2])) # Define size of local map (40m x 40m x 10 m) d_north = 20 d_east = 20 d_alt = 5 vox_shape = self.voxmap.shape north_min = int(np.clip(current_voxmap[0] - d_north, 0, vox_shape[0])) north_max = int(np.clip(current_voxmap[0] + d_north, 0, vox_shape[0])) east_min = int(np.clip(current_voxmap[1] - d_east, 0, vox_shape[1])) east_max = int(np.clip(current_voxmap[1] + d_east, 0, vox_shape[1])) alt_min = int(np.clip(current_voxmap[2] - d_alt, 0, vox_shape[2])) alt_max = int(np.clip(current_voxmap[2] + d_alt, 0, vox_shape[2])) local_voxmap = self.voxmap[north_min:north_max, east_min:east_max, alt_min:alt_max] # Define next global wp next_wp = self.global_waypoints[0] # Define start in the center of voxmap local_start = (d_north, d_east, d_alt) # Define goal to a node in the direction of the next waypoint local_goal = (np.clip(next_wp[0]-self.north_offset-north_min, 0, 2*d_north-1), np.clip(next_wp[1]-self.east_offset-east_min, 0, 2*d_east-1), np.clip(next_wp[2]-alt_min, 0, 2*d_alt-1)) # 3D A* Search path, _ = a_star_3D(local_voxmap, heuristic, local_start, local_goal) if (len(path) == 0): # Go to the next waypoint if failed to find a path del self.global_waypoints[0] if len(self.global_waypoints): self.plan_local_path() else: return # Prune path to minimize number of waypoints path = prune_path(path) # Convert path to waypoints for i in range(1, len(path)): p = path[i] # Append waypoint to local waypoints self.local_waypoints.append([p[0] + self.north_offset + north_min, p[1] + self.east_offset + east_min, p[2] + alt_min, next_wp[3]]) # Delete next global wp if already reached if np.linalg.norm(np.array(self.local_waypoints[-1]) - np.array(self.global_waypoints[0])) < 1.0: del self.global_waypoints[0] print("Waypoints: ", self.local_waypoints)
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values with open('colliders.csv') as f: row1 = f.readline().strip().replace(',','').split(' ') home = {row1[0]: float(row1[1]), row1[2]: float(row1[3])} # TODO: set home position to (lon0, lat0, 0) self.set_home_position(home['lon0'], home['lat0'], 0.0) # TODO: convert to current local position using global_to_local() local_north, local_east, local_down = global_to_local(self.global_position, self.global_home) print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) if self.planning_option == 'GRID': print('creating grid...') # Define a grid for a particular altitude and safety margin around obstacles grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) else: print('creating graph...') grid, edges, north_offset, east_offset = create_grid_and_edges(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format(north_offset, east_offset)) # Define starting point on the grid (this is just grid center) # TODO: convert start position to current position rather than map center grid_start = (int(local_north-north_offset), int(local_east-east_offset)) # Set goal as some arbitrary position on the grid # TODO: adapt to set goal as latitude / longitude position and convert goal_global = [-122.40196856, 37.79673623, 0.0] goal_north, goal_east, goal_down = global_to_local(goal_global, self.global_home) grid_goal = (int(goal_north-north_offset), int(goal_east-east_offset)) # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) print('Local Start and Goal: ', grid_start, grid_goal) if self.planning_option == 'GRID': # Call A* based on grid search path, _ = a_star_grid(grid, heuristic, grid_start, grid_goal) else: # creating a graph first G = nx.Graph() for e in edges: p1 = e[0] p2 = e[1] dist = np.linalg.norm(np.array(p2) - np.array(p1)) G.add_edge(p1, p2, weight=dist) # find nearest from the graph nodes start_ne_g = closest_point(G, grid_start) goal_ne_g = closest_point(G, grid_goal) # Call A* based on graph search path, _ = a_star_graph(G, heuristic, start_ne_g, goal_ne_g) # Append the actual start and goal states to path path = [grid_start] + path + [grid_goal] # TODO: prune path to minimize number of waypoints pruned_path = prune_path(path) # Convert path to waypoints(use integer for waypoints) waypoints = [[int(p[0] + north_offset), int(p[1] + east_offset), TARGET_ALTITUDE, 0] for p in pruned_path] # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 10 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # DONE: read lat0, lon0 from colliders into floating point values with open('colliders.csv') as f: first_line = f.readline().split(',') lat0 = float(first_line[0].strip().split(' ')[1]) lon0 = float(first_line[1].strip().split(' ')[1]) alt0 = 0. print('start pos: ({}, {}, {})'.format(lon0, lat0, alt0)) # DONE: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, alt0) # DONE: retrieve current global position print('global_position: {}'.format(self.global_position)) # DONE: convert to current local position using global_to_local() n_loc, e_loc, d_loc = global_to_local(self.global_position, self.global_home) print('n_loc: {}, e_loc: {}, d_loc: {}'.format(n_loc, e_loc, d_loc)) print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles grid, edges, north_offset, east_offset = create_grid_and_edges(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print('grid = {}'.format(grid)) # print("edges = {}".format(edges)) print('north_offset: {}, east_offset: {}'.format(north_offset, east_offset)) # create the graph with the weight of the edges set to the Euclidean distance between the points G = nx.Graph() for e in edges: p1 = e[0] p2 = e[1] dist = np.linalg.norm(np.array(p2) - np.array(p1)) G.add_edge(p1, p2, weight=dist) # Define starting point on the grid (this is just grid center) # DONE: convert start position to current position rather than map center #grid_start = [local_pos[0], local_pos[1]] grid_start = (int(np.ceil(n_loc - north_offset)), int(np.ceil(e_loc - east_offset))) graph_start = closest_point(G, grid_start) print('grid_start: {}, graph_start: {}'.format(grid_start, graph_start)) # # redefine home so that the drone does not spawn inside a building # lat1, lon1, alt1 = local_to_global((grid_start[0], grid_start[1], 0.), self.global_home) # self.set_home_position(lon1, lat1, alt1) # Set goal as some arbitrary position on the grid # DONE: adapt to set goal as latitude / longitude position and convert super_duper_burgers = (-122.39400878361174, 37.792827005959616, 0.) grid_goal_global = super_duper_burgers print('grid_goal_global: {}'.format(grid_goal_global)) grid_goal_local = global_to_local(grid_goal_global , self.global_home) print('grid_goal_local: {}'.format(grid_goal_local)) graph_goal = closest_point(G, (grid_goal_local[0], grid_goal_local[1])) print('graph_goal: {}'.format(graph_goal)) # Run A* to find a path from start to goal # DONE: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) path, cost = a_star(G, heuristic, graph_start, graph_goal) self.plot(grid, edges, path, grid_start, graph_start, grid_goal_local, graph_goal, 'full_path.png') # DONE: prune path to minimize number of waypoints pruned_path = prune_path(path) self.plot(grid, edges, pruned_path, grid_start, graph_start, grid_goal_local, graph_goal, 'pruned_path.png') # TODO (if you're feeling ambitious): Try a different approach altogether! # Convert path to waypoints waypoints = [[int(p[0] + north_offset), int(p[1] + east_offset), TARGET_ALTITUDE, 0] for p in pruned_path] print('waypoints: {}'.format(waypoints)) # Set self.waypoints self.waypoints = waypoints # DONE: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()