def find_path(data, grid_start, grid_goal, drone_altitude, safety_distance):
    # This is now the routine using Voronoi
    _, edges = create_grid_and_edges(data, drone_altitude, safety_distance)

    g = nx.Graph()

    for p1, p2 in edges:
        dist = LA.norm(np.array(p2) - np.array(p1))
        g.add_edge(p1, p2, weight=dist)

    start_closest = closest_point(g, grid_start)
    print(grid_start, start_closest)
    goal_closest = closest_point(g, grid_goal)
    print(grid_goal, goal_closest)

    # 2. Compute the path from start to goal using A* algorithm
    path, cost = a_star(g, heuristic, start_closest, goal_closest)

    # insert actual start and goal positions
    if len(path) > 0:
        path.insert(0, grid_start)
        path.append(grid_goal)

    # convert double coordinates to integers
    _path = []
    for n, e in path:
        _path.append((int(n), int(e)))

    return _path, cost
Пример #2
0
    def plan_path(self):
        self.flight_state = States.PLANNING
        print("Searching for a path ...")
        TARGET_ALTITUDE = 5
        SAFETY_DISTANCE = 5

        self.target_position[2] = TARGET_ALTITUDE

        # TODO: read lat0, lon0 from colliders into floating point values
        firstline = open('colliders.csv').readline().split(',')
        lat0 = float(firstline[0].strip("lat0 "))
        lon0 = float(firstline[1].strip("lon0 "))
        print('lat0 = {}, lon0 = {}'.format(lat0, lon0))

        # TODO: set home position to (lon0, lat0, 0)
        self.set_home_position(lon0, lat0, 0)

        # TODO: retrieve current global position
        local_position = global_to_local(self.global_position,
                                         self.global_home)

        # TODO: convert to current local position using global_to_local()

        print('global home {0}, position {1}, local position {2}'.format(
            self.global_home, self.global_position, self.local_position))
        # Read in obstacle map
        data = np.loadtxt('colliders.csv',
                          delimiter=',',
                          dtype='Float64',
                          skiprows=2)

        grid, edges, north_offset, east_offset = create_grid_and_edges(
            data, TARGET_ALTITUDE, SAFETY_DISTANCE)
        offset = np.array([north_offset, east_offset, 0])

        #create the graph with the weight of the edges using euclidean distance between the points
        G = nx.Graph()
        for e in edges:
            p1 = e[0]
            p2 = e[1]
            dist = LA.norm(np.array(p2) - np.array(p1))
            G.add_edge(p1, p2, weight=dist)

        start_pos = closest_point(G, local_position - offset)

        local_goal = global_to_local(
            [float(self.goal_lon),
             float(self.goal_lat),
             float(self.goal_alt)], self.global_home) - offset
        goal_pos = closest_point(G, local_goal)

        print('Local Start and Goal: ', start_pos, goal_pos)
        path, _ = a_star_graph(G, start_pos, goal_pos)
        path = prune_path(path, 0.5)

        # Convert path to waypoints
        waypoints = [[
            int(p[0] + north_offset),
            int(p[1] + east_offset), TARGET_ALTITUDE, 0
        ] for p in path]

        # Set self.waypoints
        self.waypoints = waypoints
        # TODO: send waypoints to sim (this is just for visualization of waypoints)
        self.send_waypoints()
Пример #3
0
    def plan_path(self):
        self.flight_state = States.PLANNING
        print("Searching for a path ...")
        TARGET_ALTITUDE = 5
        SAFETY_DISTANCE = 5

        self.target_position[2] = TARGET_ALTITUDE

        # TODO: read lat0, lon0 from colliders into floating point values
        with open('colliders.csv') as f:
            row1 = f.readline().strip().replace(',','').split(' ')
        home = {row1[0]: float(row1[1]), row1[2]: float(row1[3])}

        # TODO: set home position to (lon0, lat0, 0)
        self.set_home_position(home['lon0'],  home['lat0'],  0.0)

        # TODO: convert to current local position using global_to_local()
        local_north, local_east, local_down = global_to_local(self.global_position, self.global_home)

        print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
                                                                         self.local_position))
        # Read in obstacle map
        data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)

        if self.planning_option == 'GRID':
            print('creating grid...')
            # Define a grid for a particular altitude and safety margin around obstacles
            grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
        else:
            print('creating graph...')
            grid, edges, north_offset, east_offset = create_grid_and_edges(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
        print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))

        # Define starting point on the grid (this is just grid center)
        # TODO: convert start position to current position rather than map center
        grid_start = (int(local_north-north_offset), int(local_east-east_offset))

        # Set goal as some arbitrary position on the grid
        # TODO: adapt to set goal as latitude / longitude position and convert
        goal_global = [-122.40196856, 37.79673623, 0.0]
        goal_north, goal_east, goal_down = global_to_local(goal_global, self.global_home)
        grid_goal = (int(goal_north-north_offset), int(goal_east-east_offset))

        # Run A* to find a path from start to goal
        # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
        # or move to a different search space such as a graph (not done here)
        print('Local Start and Goal: ', grid_start, grid_goal)
        if self.planning_option == 'GRID':
            # Call A* based on grid search
            path, _ = a_star_grid(grid, heuristic, grid_start, grid_goal)
        else:
            # creating a graph first
            G = nx.Graph()
            for e in edges:
                p1 = e[0]
                p2 = e[1]
                dist = np.linalg.norm(np.array(p2) - np.array(p1))
                G.add_edge(p1, p2, weight=dist)
            # find nearest from the graph nodes
            start_ne_g = closest_point(G, grid_start)
            goal_ne_g = closest_point(G, grid_goal)
            # Call A* based on graph search
            path, _ = a_star_graph(G, heuristic, start_ne_g, goal_ne_g)
            # Append the actual start and goal states to path
            path = [grid_start] + path + [grid_goal]

        # TODO: prune path to minimize number of waypoints
        pruned_path = prune_path(path)

        # Convert path to waypoints(use integer for waypoints)
        waypoints = [[int(p[0] + north_offset), int(p[1] + east_offset), TARGET_ALTITUDE, 0] for p in pruned_path]

        # Set self.waypoints
        self.waypoints = waypoints

        # TODO: send waypoints to sim (this is just for visualization of waypoints)
        self.send_waypoints()
    def plan_path(self):
        self.flight_state = States.PLANNING
        print("Searching for a path ...")
        TARGET_ALTITUDE = 5
        SAFETY_DISTANCE = 5

        self.target_position[2] = TARGET_ALTITUDE

        # DONE: read lat0, lon0 from colliders into floating point values
        lat_lon = []
        with open('colliders.csv') as f:
            lat_lon = (f.readline()).split(",")
        lat0 = float(re.findall(r'-?\d+\.?\d*', lat_lon[0])[1])
        lon0 = float(re.findall(r'-?\d+\.?\d*', lat_lon[1])[1])
        print("Lat: {0}, Lon: {1}".format(lat0, lon0))

        # DONE: set home position to (lon0, lat0, 0)
        self.set_home_position = (lon0, lat0, 0)

        # DONE: retrieve current global position
        # DONE: convert to current local position using global_to_local()
        start_pos = global_to_local(self.global_position, self.global_home)

        print('global home {0}, position {1}, local position {2}'.format(
            self.global_home, self.global_position, self.local_position))
        # Read in obstacle map
        data = np.loadtxt('colliders.csv',
                          delimiter=',',
                          dtype='Float64',
                          skiprows=2)

        # Define a grid for a particular altitude and safety margin around obstacles
        # NOTE: Creating grid with edges to enable graph search below
        grid, north_offset, east_offset, edges = create_grid_and_edges(
            data, TARGET_ALTITUDE, SAFETY_DISTANCE)

        print("found {} edges. Creating graph... ".format(len(edges)))
        G = create_graph_from_edges(edges)
        print("Graph created")

        print("North offset = {0}, east offset = {1}".format(
            north_offset, east_offset))

        # DONE: convert start position to current position rather than map center
        grid_start = (int(start_pos[0]) - north_offset,
                      int(start_pos[1]) - east_offset)

        # DONE: adapt to set goal as latitude / longitude position and convert
        # Note use: self.poi.get_random() for a random POI
        goal = global_to_local(self.poi.get("Washington Street"),
                               self.global_home)
        grid_goal = (int(goal[0]) - north_offset, int(goal[1]) - east_offset)

        # Run A* to find a path from start to goal
        # DONE: add diagonal motions with a cost of sqrt(2) to your A* implementation
        # or move to a different search space such as a graph (not done here)
        # NOTE: I've implemented the diagonals but then implemented moved to graph search
        #       I left the diagnonal code in.

        # Figure out the closest node to the the grid start and goal positions
        nodes = np.array(G.nodes)
        node_start = np.linalg.norm(np.array(grid_start) - np.array(nodes),
                                    axis=1).argmin()
        node_goal = np.linalg.norm(np.array(grid_goal) - np.array(nodes),
                                   axis=1).argmin()
        g_start = tuple(nodes[node_start]) + (self.global_home[2], )
        g_goal = tuple(nodes[node_goal]) + (-1 * (int(goal[2])), )
        print('Local Start and Goal: ', g_start, g_goal)

        print('Starting A*')
        path, _ = a_star_graph(G, heuristic, g_start, g_goal)

        # Add gentle gradient for flying car (especially if goal is higher)
        path = add_altitude_gradient(path, g_start, g_goal)

        # The graph search ends at the clsoest node so I add the last point if there are no obstacles on the way
        if len(path) > 0 and (path[-1] != grid_goal) and not (collision_check(
                path[-1], g_goal, grid)):
            print("Adding end position")
            path.append(g_goal)

        # DONE: prune path to minimize number of waypoints
        # DONE (if you're feeling ambitious): Try a different approach altogether!
        # NOTE: I further prune the path by removing nodes not blocked by obstacles
        #       I take height into consideration so it does have paths that fly over
        #       low obstacles
        path = prune_path(path, grid)

        # Convert path to waypoints
        waypoints = [[
            int(p[0]) + north_offset,
            int(p[1]) + east_offset,
            int(p[2]), 0
        ] for p in path]

        #Asjust waypoint headings
        for i in range(1, len(waypoints)):
            wp1 = waypoints[i - 1]
            wp2 = waypoints[i]
            wp2[3] = np.arctan2((wp2[1] - wp1[1]), (wp2[0] - wp1[0]))

        print(waypoints)
        # Set self.waypoints
        self.waypoints = waypoints
        # DONE: send waypoints to sim (this is just for visualization of waypoints)
        self.send_waypoints()
Пример #5
0
    def plan_path(self):
        self.flight_state = States.PLANNING
        print("Searching for a path ...")
        TARGET_ALTITUDE = 5
        SAFETY_DISTANCE = 10

        self.target_position[2] = TARGET_ALTITUDE

        # TODO: read lat0, lon0 from colliders into floating point values
        first_line = next(open("colliders.csv"))
        gps = first_line.split(',')
        lat0 = np.float64(gps[0].lstrip().split(' ')[1])
        lon0 = np.float64(gps[1].lstrip().split(' ')[1])
        #print("The Lat and Long = ", lat0, lon0)

        # TODO: set home position to (lat0, lon0, 0)
        self.set_home_position(lon0, lat0, 0)

        # TODO: retrieve current global position
        # self.global_position = self.global_position
        #print("global_home ", self.global_home)


        # TODO: convert to current local position using global_to_local()
        current_local_pos = global_to_local(self.global_position, self.global_home)

        print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
                                                                         self.local_position))
        # Read in obstacle map
        data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
        
        # Define a grid for a particular altitude and safety margin around obstacles
        grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
        print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))


        # Define starting point on the grid (this is just grid center)
        #grid_start = (-north_offset, -east_offset)

        #(37.795345, -122.398013)? Mine is (633, 393).

        # TODO: convert start position to current position rather than map center
        grid_start = (-north_offset + int(current_local_pos[0]), -east_offset + int(current_local_pos[1]))
        #grid_start = (-north_offset-100, -east_offset -100)

        grid_goal = self.global_to_local(sys.argv[2],sys.argv[4],north_offset, east_offset)

        #start_ne = (203, 699)
        #goal_ne = (690, 300)

        """

        # This section is for Grid Based Implementation

        # Set goal as some arbitrary position on the grid
        #grid_goal = (-north_offset + 10, -east_offset + 10)


        # TODO: adapt to set goal as latitude / longitude position and convert
        # print("Arbitary Goal = ", self.get_arbitory_goal(data, 10))
        goal_latlon = self.get_arbitory_goal(grid, 10)

        goal_north = int(np.abs(goal_latlon[0]))
        goal_east = int(np.abs(goal_latlon[1]))
        #current_goal =  global_to_local(goal_latlon, self.global_home)



        # Run A* to find a path from start to goal
        # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
        # or move to a different search space such as a graph (not done here)
        print('Local Start and Goal: ', grid_start, grid_goal)
        path, _ = a_star(grid, heuristic, grid_start, grid_goal)

        print("path ", len(path))
        # TODO: prune path to minimize number of waypoints
        # TODO (if you're feeling ambitious): Try a different approach altogether!
        pruned_path = self.prune_path(path)

        print("pruned_path = ",pruned_path)
        # Convert path to waypoints
        waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]

        """





    
        # Graph Implementation
        # This is now the routine using Voronoi
        grid, edges = create_grid_and_edges(data, TARGET_ALTITUDE, SAFETY_DISTANCE)

        G = nx.Graph()
        for e in edges:
            p1 = e[0]
            p2 = e[1]
            dist = LA.norm(np.array(p2) - np.array(p1))
            G.add_edge(p1, p2, weight=dist)

        #start_ne = (815, 639)

        # Map to Grid location without any obstacles



        print('Local Start and Goal: ', grid_start, grid_goal)

        start_ne_g = self.closest_point(G, grid_start)
        goal_ne_g = self.closest_point(G, grid_goal)

        #print(start_ne_g,goal_ne_g)

        path1, cost = a_star_graph(G, heuristic, start_ne_g, goal_ne_g)
        print("path1 ",len(path1))
        pruned_path = self.prune_path(path1)
        waypoints = [[int(p[0] + north_offset), int(p[1] + east_offset), TARGET_ALTITUDE, 0] for p in pruned_path]
        




        # Set self.waypoints
        self.waypoints = waypoints
        # TODO: send waypoints to sim
        self.send_waypoints()
    def plan_path(self):
        self.flight_state = States.PLANNING
        print("Searching for a path ...")
        TARGET_ALTITUDE = 5
        SAFETY_DISTANCE = 3

        self.target_position[2] = TARGET_ALTITUDE

        # TODO: read lat0, lon0 from colliders into floating point values
        with open('colliders.csv', newline='') as f:
            reader = csv.reader(f)
            row1 = next(reader)  # gets the first line
            lat0, lon0 = float(row1[0][5:]), float(row1[1][5:])

        # TODO: set home position to (lat0, lon0, 0)
        self.set_home_position(
            lon0, lat0, 0)  # set the current location to be the home position

        # TODO: retrieve current global position
        current_global_pos = (self._longitude, self._latitude, self._altitude)

        # TODO: convert to current local position using global_to_local()
        current_local_pos = global_to_local(current_global_pos,
                                            self.global_home)

        print('global home {0}, position {1}, local position {2}'.format(
            self.global_home, self.global_position, self.local_position))
        # Read in obstacle map
        data = np.loadtxt('colliders.csv',
                          delimiter=',',
                          dtype='Float64',
                          skiprows=2)

        # Get the center of the grid
        north_offset = int(np.floor(np.min(data[:, 0] - data[:, 3])))
        east_offset = int(np.floor(np.min(data[:, 1] - data[:, 4])))

        # Define a grid for a particular altitude and safety margin around obstacles
        print("North offset = {0}, east offset = {1}".format(
            north_offset, east_offset))

        # This is now the routine using Voronoi
        grid_graph, edges = create_grid_and_edges(data, TARGET_ALTITUDE,
                                                  SAFETY_DISTANCE)
        print("GRAPH EDGES:", len(edges))

        # Create the graph with the weight of the edges
        # set to the Euclidean distance between the points
        G = nx.Graph()
        for e in edges:
            p1 = e[0]
            p2 = e[1]
            dist = LA.norm(np.array(p2) - np.array(p1))
            G.add_edge(p1, p2, weight=dist)

        # TODO: convert start position to current position rather than map center
        grid_start = (int(current_local_pos[0] - north_offset),
                      int(current_local_pos[1] - east_offset))

        # TODO: adapt to set goal as latitude / longitude position and convert
        goal_global_pos = (-122.393010, 37.790000, 0)
        goal_local_pos = global_to_local(goal_global_pos, self.global_home)
        grid_goal = (int(goal_local_pos[0] - north_offset),
                     int(goal_local_pos[1] - east_offset))

        #goal_ne = (840., 100.)
        start_ne_g = closest_point(G, grid_start)
        goal_ne_g = closest_point(G, grid_goal)
        print("START NE GOAL:", start_ne_g)
        print("GOAL NE GOAL", goal_ne_g)

        # Run A* to find a path from start to goal
        # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
        # or move to a different search space such as a graph (not done here)

        path, cost = a_star_graph(G, heuristic_graph, start_ne_g, goal_ne_g)
        print("Path Length:", len(path), "Path Cost:", cost)

        int_path = [[int(p[0]), int(p[1])] for p in path]

        # TODO: prune path to minimize number of waypoints
        pruned_path = prune_path(int_path)
        print("Length Pruned Path:", len(pruned_path))

        # Convert path to waypoints
        waypoints = [[
            p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
        ] for p in pruned_path]

        # Set self.waypoints
        self.waypoints = waypoints
        # TODO: send waypoints to sim
        self.send_waypoints()
    def plan_path(self):
        self.flight_state = States.PLANNING
        print("Searching for a path ...")
        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()
Пример #8
0
#
# plt.imshow(grid_narrow, cmap='Greys', origin='lower')
# 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], 'o', color='r')
# 	plt.plot(pp[:, 1], pp[:, 0], 'g')
# plt.xlabel('NORTH')
# plt.ylabel('EAST')
# plt.show()

###################### Voronoi ######################
# Create grid and voronoi edges from data
timer = time.time()
grid, offset, edges = create_grid_and_edges(data, TARGET_ALTITUDE,
                                            SAFETY_DISTANCE_VORONOI)
print('Voronoi {0} edges created in {1}s'.format(len(edges),
                                                 time.time() - timer))

# Construct graph from voronoi edges
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')

# Iterate several times for different start/goal locations
for iteration in range(1):
    print('> iteration #{0}'.format(iteration))
    def plan_path(self):
        self.flight_state = States.PLANNING
        print("Searching for a path ...")
        TARGET_ALTITUDE = 5
        SAFETY_DISTANCE = 5

        self.target_position[2] = TARGET_ALTITUDE

        print('global home {0}, position {1}, local position {2}'.format(
            self.global_home, self.global_position, self.local_position))
        # Read in obstacle map
        data = np.loadtxt('colliders.csv',
                          delimiter=',',
                          dtype='Float64',
                          skiprows=2)

        # Define a flying altitude (feel free to change this)
        drone_altitude = 5
        safety_distance = 3
        grid, north_offset, east_offset, edges = create_grid_and_edges(
            data, drone_altitude, safety_distance)
        print('Found %5d edges' % len(edges))
        plt.imshow(grid, origin='lower', cmap='Greys')
        # Stepping through each edge
        for e in edges:
            p1 = e[0]
            p2 = e[1]
            plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'b-')

        plt.xlabel('EAST')
        plt.ylabel('NORTH')
        plt.show()

        # Create networkx graph
        graph = nx.Graph()
        for e in edges:
            graph.add_edge(*e)

        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 = (
            640.7610999999999, 398.7684653999999
        )  # random point generated from: grid_start = edges[random.randint(0, len(edges))][0]

        # Set goal as some arbitrary position on the grid
        grid_goal = (
            250.761145, 190.76849833333335
        )  # random point generated from: grid_goal = edges[random.randint(0, len(edges))][0]

        # Run bfs to find a path from start to goal
        print('Local Start and Goal: ', grid_start, grid_goal)
        path, _ = bfs(graph, _, grid_start, grid_goal)
        # print(path)

        # Plot resulted path
        plt.imshow(grid, origin='lower', cmap='Greys')
        for e in edges:
            p1 = e[0]
            p2 = e[1]
            plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'b-')
        for p in path:
            plt.plot(p[1], p[0], 'ro-', markersize=2)

        plt.xlabel('EAST')
        plt.ylabel('NORTH')
        plt.show()

        # Convert path to waypoints
        waypoints = [[
            p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
        ] for p in path]
        # Set self.waypoints
        self.waypoints = waypoints
        self.send_waypoints()
Пример #10
0
    def find_path(self, data, TARGET_ALTITUDE, SAFETY_DISTANCE, start_v,
                  goal_v, nmin, emin):
        """
        Path search with different algorithms
        INPUT: search_alg = [VORONOI, MEDIAL_AXIS]
        OUTPUT: path
        """
        if self.search_alg == SearchAlg.VORONOI:
            # Define a grid for a particular altitude and safety margin around obstacles
            #grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
            #print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
            import time
            t0 = time.time()
            grid, edges = create_grid_and_edges(data, TARGET_ALTITUDE,
                                                SAFETY_DISTANCE)
            print('Found %5d edges' % len(edges))
            #print(flight_altitude, safety_distance)
            print('graph took {0} seconds to build'.format(time.time() - t0))

            # create the graph with the weight of the edges
            # set to the Euclidean distance between the points
            G = nx.Graph()
            for e in edges:
                p1 = tuple(e[0])
                p2 = tuple(e[1])
                dist = np.linalg.norm(np.array(p2) - np.array(p1))
                G.add_edge(p1, p2, weight=dist)

            graph_start, graph_goal = start_goal_graph(G, start_v, goal_v)

            # Run A* to find a path from start to goal
            # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
            # or move to a different search space such as a graph (not done here)
            print('Local Start and Goal on Voronoi graph: ', graph_start,
                  graph_goal)

            path_graph, cost = a_star_graph(G, eucl_heuristic, graph_start,
                                            graph_goal)
            print('Number of edges', len(path_graph), 'Cost', cost)
            # TODO: prune path to minimize number of waypoints
            # TODO (if you're feeling ambitious): Try a different approach altogether!
            waypoints = [[
                int(p[0] + nmin),
                int(p[1] + emin), TARGET_ALTITUDE, 0
            ] for p in path_graph]

            return waypoints

        elif self.search_alg == SearchAlg.MEDIAL_AXIS:

            # Define a grid for a particular altitude and safety margin around obstacles
            grid, north_offset, east_offset = create_grid(
                data, TARGET_ALTITUDE, SAFETY_DISTANCE)
            print("North offset = {0}, east offset = {1}".format(
                north_offset, east_offset))
            # Define starting point on the grid (this is just grid center)
            #grid_start = (-north_offset, -east_offset)
            #grid_start =(int(start_v[0][0]),int(start_v[0][1]))
            # TODO: convert start position to current position rather than map center

            # Set goal as some arbitrary position on the grid
            #grid_goal = (-north_offset + 10, -east_offset + 10)
            #grid_goal = (int(goal_v[0][0]),int(goal_v[0][1]))
            # TODO: adapt to set goal as latitude / longitude position and convert

            skeleton = medial_axis(invert(grid))
            #print(skeleton.shape)

            skel_start, skel_goal = find_start_goal_skeleton(
                skeleton, start_v[0:2], goal_v[0:2])
            # Run A* to find a path from start to goal
            # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
            # or move to a different search space such as a graph (not done here)
            print('Local Start and Goal on Skeleton: ', skel_start, skel_goal)
            #Manhattan heuristic
            #print(invert(skeleton).astype(np.int))
            path, cost = a_star(
                invert(skeleton).astype(np.int), manh_heuristic,
                tuple(skel_start), tuple(skel_goal))
            # TODO: prune path to minimize number of waypoints
            pruned_path = prune_path(path)
            # TODO (if you're feeling ambitious): Try a different approach altogether!
            waypoints = [[
                int(p[0] + nmin),
                int(p[1] + emin), TARGET_ALTITUDE, 0
            ] for p in pruned_path]

            return waypoints

        elif self.search_alg == SearchAlg.PROBABILISTIC:

            num_samp = 1000
            sampler = Sampler(data, TARGET_ALTITUDE, SAFETY_DISTANCE)

            start = (start_v[0] + nmin, start_v[1] + emin, start_v[2])
            goal = (goal_v[0] + nmin, goal_v[1] + emin, goal_v[2])
            polygons = sampler._polygons
            nodes = sampler.sample(num_samp)
            #for ind in range(len(nodes)):
            # nodes[ind] = (nodes[ind][0] - nmin, nodes[ind][1] - emin, nodes[ind][2])
            print('number of nodes', len(nodes), 'local NE', nodes[1])
            nearest_neighbors = 10
            import time
            t0 = time.time()
            g = create_graph(nodes, nearest_neighbors, polygons)
            print('graph took {0} seconds to build'.format(time.time() - t0))
            print("Number of edges", len(g.edges))
            grid, north_offset, east_offset = create_grid(
                data, TARGET_ALTITUDE, SAFETY_DISTANCE)
            print("North offset = {0}, east offset = {1}".format(
                north_offset, east_offset))

            prob_start, prob_goal = start_goal_graph(g, start, goal)

            print('Local Start and Goal on Probabilistic map: ', prob_start,
                  prob_goal)

            path, cost = a_star_graph(g, heuristic, prob_start, prob_goal)
            #path_pairs = zip(path[:-1], path[1:])
            """
            plt.imshow(grid, cmap='Greys', origin='lower')
            
            nmin = np.min(data[:, 0])
            emin = np.min(data[:, 1])
            
            # draw nodes
            for n1 in g.nodes:
                plt.scatter(n1[1] - emin, n1[0] - nmin, c='red')
                
            # draw edges
            for (n1, n2) in g.edges:
                plt.plot([n1[1] - emin, n2[1] - emin], [n1[0] - nmin, n2[0] - nmin], 'grey')
                
            # TODO: add code to visualize the path
            path_pairs = zip(path[:-1], path[1:])
            for (n1, n2) in path_pairs:
                plt.plot([n1[1] - emin, n2[1] - emin], [n1[0] - nmin, n2[0] - nmin], 'green')
            plt.plot(start[1]-emin, start[0]-nmin, 'bv')
            plt.plot(goal[1]-emin, goal[0]-nmin, 'bx')
            plt.xlabel('NORTH')
            plt.ylabel('EAST')
            
            plt.show()
            """
            waypoints = [[int(p[0]), int(p[1]), int(p[2]), 0] for p in path]

            return waypoints
Пример #11
0
    def plan_path(self):
        self.flight_state = States.PLANNING
        print("Searching for a path ...")
        TARGET_ALTITUDE = 4
        SAFETY_DISTANCE = 5

        self.target_position[2] = TARGET_ALTITUDE

        # TODO: read lat0, lon0 from colliders into floating point values

        with open('colliders.csv', newline='') as f:
            reader = csv.reader(f)
            row1 = next(reader)  # gets the first line
        lat0 = float(row1[0].split()[1])
        lon0 = float(row1[1].split()[1])
        print('lat0:', lat0, ', lon0:', lon0)

        # TODO: set home position to (lon0, lat0, 0)

        self.global_home[0] = lon0
        self.global_home[1] = lat0
        self.global_home[2] = 0

        #self.global_home[0]=self._longitude
        #self.global_home[1]=self._latitude
        #self.global_home[2]=0
        print('global home {0}'.format(self.global_home))

        # TODO: retrieve current global position
        global_position = self.global_position

        # TODO: convert to current local position using global_to_local()
        local_position = global_to_local(global_position, self.global_home)
        print('local position:', local_position)
        self.local_position[0] = local_position[0]
        self.local_position[1] = local_position[1]
        self.local_position[2] = local_position[2]

        print('global home {0}, position {1}, local position {2}'.format(
            self.global_home, self.global_position, self.local_position))
        # Read in obstacle map
        data = np.loadtxt('colliders.csv',
                          delimiter=',',
                          dtype='Float64',
                          skiprows=2)

        # Define a grid for a particular altitude and safety margin around obstacles
        grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
                                                      SAFETY_DISTANCE)
        print("North offset = {0}, east offset = {1}".format(
            north_offset, east_offset))

        # Define starting point on the grid (this is just grid center)
        # grid_start = (-north_offset, -east_offset)
        grid_start = (int(self.local_position[0] - north_offset),
                      int(self.local_position[1] - east_offset))
        # TODO: convert start position to current position rather than map center

        # Set goal as some arbitrary position on the grid

        # grid_goal = (-north_offset , -east_offset )
        # TODO: adapt to set goal as latitude / longitude position and convert
        goal_north = 100
        goal_east = -150
        grid_goal = (-north_offset + goal_north, -east_offset + goal_east)

        if self.choices_list[self.choice_idx] == 'astar':
            # Run A* to find a path from start to goal
            # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
            # or move to a different search space such as a graph (not done here)
            print('Local Start and Goal: ', grid_start, grid_goal)

            path, _ = a_star(grid, heuristic, grid_start, grid_goal)

        if self.choices_list[self.choice_idx] == 'medial_axis':
            skeleton = medial_axis(invert(grid))
            skel_start, skel_goal = find_start_goal(skeleton, grid_start,
                                                    grid_goal)
            print('Local Start and Goal: ', skel_start, skel_goal)
            path, _ = a_star(
                invert(skeleton).astype(np.int), heuristic, tuple(skel_start),
                tuple(skel_goal))

        if self.choices_list[self.choice_idx] == 'voronoi_graph_search':
            grid, edges, north_offset, east_offset = create_grid_and_edges(
                data, TARGET_ALTITUDE, SAFETY_DISTANCE)
            G = create_weighted_graph(edges)
            start_ne_g = closest_point(G, grid_start)
            goal_ne_g = closest_point(G, grid_goal)
            print('Local Start and Goal: ', start_ne_g, goal_ne_g)
            path, cost = a_star_graph(G, heuristic, start_ne_g, goal_ne_g)
        print('path found from a_star')

        if self.if_pruning:

            # TODO: prune path to minimize number of waypoints
            # TODO (if you're feeling ambitious): Try a different approach altogether!
            pruned_path = prune_path(path)
            print('path found from a_star is pruned')

            # Convert path to waypoints
            # waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
            waypoints = [[
                p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE,
                np.arctan2((q[1] - p[1]), (q[0] - p[0]))
            ] for p, q in zip(pruned_path[:-1], pruned_path[1:])]
        else:
            waypoints = [[
                p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
            ] for p in path]

        # Set self.waypoints
        self.waypoints = waypoints
        # TODO: send waypoints to sim (this is just for visualization of waypoints)
        self.send_waypoints()
Пример #12
0
    def plan_path(self):

        self.flight_state = States.PLANNING
        print("Searching for a path ...")
        TAKEOFF_ALTITUDE = 5
        SAFETY_DISTANCE = 3

        # Set the takeoff altitude
        self.target_position[2] = TAKEOFF_ALTITUDE

        # Read lon0, lat0 from colliders into floating point values
        with open('colliders.csv', newline='') as f:
            reader = csv.reader(f)
            b = next(reader)
        home_lat = float(b[0].split(" ")[1])
        home_long = float(b[1].split(" ")[2])
        print("home longitude = {0},  home latitude = {1}".format(
            home_long, home_lat))

        # Set home position to (lon0, lat0, 0)
        self.set_home_position(home_long, home_lat, 0)

        #  Retrieve current global position
        global_start = self.global_position

        # Convert to current local position using global_to_local()
        local_start = global_to_local(global_start, self.global_home)

        print('global home {0}, position {1}, local position {2}'.format(
            self.global_home, self.global_position, local_start))
        # Start position
        start_altitude = -1 * local_start[2] + TAKEOFF_ALTITUDE
        print("start altitude = ", start_altitude)
        start = [local_start[0], local_start[1], start_altitude]

        # Goal position
        goal = global_to_local(self.global_goal, self.global_home)

        # Read in obstacle map
        data = np.loadtxt('colliders.csv',
                          delimiter=',',
                          dtype='Float64',
                          skiprows=2)

        # Define a grid for a particular altitude and safety margin around obstacles
        grid, edges, polygons, north_offset, east_offset = create_grid_and_edges(
            data, min(start[2], goal[2]), SAFETY_DISTANCE)
        print("North offset = {0}, east offset = {1}".format(
            north_offset, east_offset))

        # Convert start position to current position on grid
        grid_start = (start[0] - north_offset, start[1] - east_offset)

        # Convert goal position to grid location
        grid_goal = (goal[0] - north_offset, goal[1] - east_offset)

        print('Local Start and Goal: ', grid_start, grid_goal)

        # Build a 2D graph
        G = nx.Graph()
        for e in edges:
            p1 = e[0]
            p2 = e[1]
            dist = np.linalg.norm(np.array(p2) - np.array(p1))
            G.add_edge(p1, p2, weight=dist)

        # Find closest nodes to start and goal on graph
        start_node = find_close_point(G, grid_start)
        goal_node = find_close_point(G, grid_goal)

        # Run A* to find a path from start to goal
        t0 = time.time()
        path, path_cost = a_star_graph(G, heuristic, start_node, goal_node)
        print("Time to find the path = ", time.time() - t0, "seconds")
        print("Length of path = ", len(path))
        print("Cost of path = ", path_cost)

        # Prune the path in 2D using Bresenham algorithm
        t0 = time.time()
        smooth_path = prune_path_2d(path, grid)
        print("Time to prune the path= ", time.time() - t0, "seconds")
        smooth_path.append(grid_goal)  # Add goal the list of path
        print("Length of pruned path = ", len(smooth_path))

        # Find the inc/dec for the required in altitude for each consecutive waypoint
        alt_inc = ((self.global_goal[2] + SAFETY_DISTANCE) -
                   start[2]) / (len(smooth_path) - 2)

        # Convert smooth_path to 3D waypoints
        temp_path = [[
            int(smooth_path[i][0] + north_offset),
            int(smooth_path[i][1] + east_offset),
            int(start_altitude + i * alt_inc)
        ] for i in range(len(smooth_path) - 1)]

        # Add the goal point to the 3D list with altitude equal to previous waypoint.
        # So that drone can move from final waypoint to goal by moving at same altitude.
        temp_path.append([
            int(smooth_path[-1][0] + north_offset),
            int(smooth_path[-1][1] + east_offset), temp_path[-1][2]
        ])

        smooth_path = temp_path

        # Prune the path in 3D using 2.5D Map representation
        t0 = time.time()
        final_path = prune_path_3d(smooth_path, polygons)
        print("Time to prune the 3D path= ", time.time() - t0, "seconds")
        print("Length of pruned 3D path = ", len(final_path))

        # Convert path to waypoints
        waypoints = [[final_path[i][0], final_path[i][1], final_path[i][2], 0]
                     for i in range(len(final_path))]

        # Set self.waypoints
        self.waypoints = waypoints

        # Sends waypoints to sim (this is just for visualization of waypoints)
        self.send_waypoints()
    def plan_path(self):
        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:
            lat_str, lon_str = f.readline().split(',')
            lat, lon = float(lat_str.split(' ')[-1]), float(
                lon_str.split(' ')[-1])
        print(lat, lon)
        # TODO: set home position to (lon0, lat0, 0)
        self.set_home_position(lon, lat, 0)
        # TODO: retrieve current global position
        current_global_position = np.array(
            [self._longitude, self._latitude, self._altitude])
        # TODO: convert to current local position using global_to_local()
        #current_local_pos = global_to_local([self._longitude, self._latitude, self._altitude], self.global_home)
        current_local_pos = 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))

        # Using graphs
        grid, edges, north_offset, east_offset = create_grid_and_edges(
            data, TARGET_ALTITUDE, SAFETY_DISTANCE)

        # create graph
        G = nx.Graph()
        for e in edges:
            p1 = e[0]
            p2 = e[1]
            dist = np.linalg.norm(np.array(p2) - np.array(p1))
            G.add_edge(p1, p2, weight=dist)

        # 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(-north_offset + current_local_pos[0]),
                      int(-east_offset + current_local_pos[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
        goal_latlon = [-122.39207985, 37.79685038, TARGET_ALTITUDE]
        grid_goal_ne = global_to_local(goal_latlon, self.global_home)
        grid_goal = (int(grid_goal_ne[0]), int(grid_goal_ne[1]))
        # Run A* to find a path from start to goal
        # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
        # or move to a different search space such as a graph (not done here)
        print('Local Start and Goal: ', grid_start, grid_goal)
        # path, _ = a_star(grid, heuristic, grid_start, grid_goal)

        start_ne_g = self.closest_point(G, grid_start)
        goal_ne_g = self.closest_point(G, grid_goal)
        home_g = self.closest_point(G, [-north_offset, -east_offset])
        path, cost = graph_a_star(G, heuristic, start_ne_g, goal_ne_g)

        # TODO: prune path to minimize number of waypoints
        # TODO (if you're feeling ambitious): Try a different approach altogether!
        pruned_path = self.prune_path(path)
        # Convert path to waypoints
        # waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]
        waypoints = [[
            int(p[0] - home_g[0]),
            int(p[1] - home_g[1]), 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()
Пример #14
0
    def plan_path(self):
        self.flight_state = States.PLANNING
        print("Searching for a path ...")
        TARGET_ALTITUDE = 100
        SAFETY_DISTANCE = 5

        self.target_position[2] = TARGET_ALTITUDE

        # TODO: read lat0, lon0 from colliders into floating point values
        parts = open('colliders.csv').readlines()[0].split(',')
        lat0 = float(parts[0].strip().split(' ')[1])
        lon0 = float(parts[1].strip().split(' ')[1])

        # TODO: set home position to (lon0, lat0, 0)
        self.set_home_position(lon0, lat0, 0)

        # TODO: retrieve current global position
        current_global_position = self.global_position

        # TODO: convert to current local position using global_to_local()
        current_local_position = global_to_local(current_global_position,
                                                 self.global_home)

        print('global home {0}, position {1}, local position {2}'.format(
            self.global_home, self.global_position, self.local_position))
        # Read in obstacle map
        data = np.loadtxt('colliders.csv',
                          delimiter=',',
                          dtype='Float64',
                          skiprows=2)

        # Define a grid for a particular altitude and safety margin around obstacles
        grid, edges, north_offset, east_offset = create_grid_and_edges(
            data, TARGET_ALTITUDE, SAFETY_DISTANCE)
        print("North offset = {0}, east offset = {1}".format(
            north_offset, east_offset))
        # Define starting point on the grid (this is just grid center)
        grid_start = (-north_offset, -east_offset)
        # TODO: convert start position to current position rather than map center
        # grid_start = global_to_local(self.global_position, self.global_home)
        grid_start = (int(current_local_position[1]) - north_offset,
                      int(current_local_position[0]) - east_offset)

        # Set goal as some arbitrary position on the grid
        grid_goal = (750, 370)

        # TODO: adapt to set goal as latitude / longitude position and convert
        lat_lon_grid_goal = local_to_global(
            (grid_goal[0], grid_goal[1], -TARGET_ALTITUDE), self.global_home)

        # Run A* to find a path from start to goal
        # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
        # or move to a different search space such as a graph (not done here)
        print('Local Start and Goal: ', grid_start, grid_goal)

        (graph_start, graph_end) = find_start_goal_2d(edges, grid_start,
                                                      grid_goal)
        print('Ajusted Start and Goal for a Graph', graph_start, graph_end)

        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)

        path, _ = a_star_2d_graph(G, heuristic, graph_start, graph_end)

        # TODO: prune path to minimize number of waypoints
        # TODO (if you're feeling ambitious): Try a different approach altogether!

        # Convert path to waypoints
        waypoints = [[
            p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
        ] for p in path]
        # Set self.waypoints
        self.waypoints = waypoints
        # TODO: send waypoints to sim (this is just for visualization of waypoints)
        self.send_waypoints()
Пример #15
0
    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()
Пример #16
0
    def plan_path(self):
        self.flight_state = States.PLANNING
        print("Searching for a path ...")
        TARGET_ALTITUDE = 2
        SAFETY_DISTANCE = 1

        self.target_position[2] = TARGET_ALTITUDE

        # TODO: read lat0, lon0 from colliders into floating point values
        lat0, lon0 = read_home_lat_lon("colliders.csv")
        
        # TODO: set home position to (lon0, lat0, 0)
        self.set_home_position(lon0, lat0, 0)

        # TODO: retrieve current global position
        current_global_position = self.global_position
 
        # TODO: convert to current local position using global_to_local()
        current_local_position = global_to_local(current_global_position, self.global_home)
        
        print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
                                                                         self.local_position))
        # Read in obstacle map
        data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
        
        # Define a grid for a particular altitude and safety margin around obstacles
        grid, edges, north_offset, east_offset = create_grid_and_edges(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
        print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
        # Define starting point on the grid (this is just grid center)
        grid_start = (-north_offset, -east_offset)
        # TODO: convert start position to current position rather than map center
        start = (int(np.ceil(current_local_position[0]) + grid_start[0]), int(np.ceil(current_local_position[1]) + grid_start[1]))
        # Set goal as some arbitrary position on the grid
        #goal = (-north_offset + 10, -east_offset + 10)
        # TODO: adapt to set goal as latitude / longitude position and convert
        goal_lat = 37.796793
        goal_lon = -122.397182
        goal_global = [goal_lon, goal_lat, 0.0]
        goal_local = global_to_local(goal_global, self.global_home)
        goal = (int(np.ceil(goal_local[0])) - north_offset, int(np.ceil(goal_local[1])) - east_offset)
        print("grid_goal", goal)

        # Run A* to find a path from start to goal
        # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
        # or move to a different search space such as a graph (not done here)
        print('Local Start and Goal: ', start, goal)

        G = create_nx_graph(edges)
        start_node = find_closest_point(G, start)
        goal_node = find_closest_point(G, goal)


        path = a_star_graph(G, heuristic, start_node, goal_node)
        # TODO: prune path to minimize number of waypoints
        pruned_path = prune_path_colinearity(grid, path)

        # Prune the path in 2D
        #smooth_path = prune_path_2d(path, grid)
               
        # TODO (if you're feeling ambitious): Try a different approach altogether!

        ## visualisation
        #plt.imshow(grid, cmap='Greys', origin='lower')
        #plt.plot(start[1], start[0], 'x')
        #plt.plot(goal[1], goal[0], 'x')
        #p = np.array(path)
        #pp = np.array(pruned_path)
        #ppc = np.array(pruned_path_col)
        #plt.plot(p[:,1], p[:,0], 'r')
        #plt.plot(pp[:,1], pp[:,0], 'g')
        #plt.plot(ppc[:,1], ppc[:,0], 'b')
        #plt.xlabel('EAST')
        #plt.ylabel('NORTH')
        #plt.show()


        # Convert path to waypoints
        waypoints = [[int(p[0]) + north_offset, int(p[1]) + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
        with open("wp.p", mode="wb") as f:
            pickle.dump(waypoints, f)

        with open("wp.p", mode="rb") as f:
            wp = pickle.load(f)

        # Set self.waypoints
        self.waypoints = wp
        # TODO: send waypoints to sim (this is just for visualization of waypoints)
        self.send_waypoints()
    def plan_path(self):
        self.flight_state = States.PLANNING
        print("Searching for a path ...")
        TARGET_ALTITUDE = 5
        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()
Пример #18
0
    def plan_path(self):
        self.flight_state = States.PLANNING
        print("Searching for a path ...")
        TARGET_ALTITUDE = 5
        SAFETY_DISTANCE = 5

        self.target_position[2] = TARGET_ALTITUDE

        # TODO: read lat0, lon0 from colliders into floating point values
        with open('colliders.csv') as colliders:
            position_line = colliders.readline().strip()

        position_line = position_line.split(',')
        position_lat = position_line[0].split(' ')[1]
        position_lon = position_line[1].strip().split(' ')[
            1]  # strip needed due to leading space.

        # TODO: set home position to (lon0, lat0, 0)
        self.set_home_position(float(position_lon), float(position_lat), 0)

        # TODO: retrieve current global position
        global_position = [self._longitude, self._latitude, self._altitude]

        # TODO: convert to current local position using global_to_local()
        local_position = global_to_local(global_position, self.global_home)

        print('global home {0}, position {1}, local position {2}'.format(
            self.global_home, self.global_position, self.local_position))
        # Read in obstacle map
        data = np.loadtxt('colliders.csv',
                          delimiter=',',
                          dtype='Float64',
                          skiprows=2)

        # Define a grid for a particular altitude and safety margin around obstacles
        grid, north_offset, east_offset, edges = create_grid_and_edges(
            data, TARGET_ALTITUDE, SAFETY_DISTANCE)
        graph = nx.Graph()
        for e in edges:
            p1 = e[0]
            p2 = e[1]
            dist = LA.norm(np.array(p2) - np.array(p1))
            graph.add_edge(p1, p2, weight=dist)

        print("These are the grid and edges", grid, edges)

        # Define starting point on the grid (this is just grid center)
        grid_start = (int(local_position[0] - north_offset),
                      int(local_position[1] - east_offset))
        graph_start = closest_point(graph, grid_start)
        print("Grid start and graph start", grid_start, graph_start)
        # TODO: convert start position to current position rather than map center
        # done above
        # Set goal as some arbitrary position on the grid
        grid_goal = (120, 610)
        graph_goal = closest_point(graph, grid_goal)
        print("Grid start and graph goal", grid_goal, graph_goal)
        # TODO: adapt to set goal as latitude / longitude position and convert

        # Run A* to find a path from start to goal
        # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
        # or move to a different search space such as a graph (not done here)
        # both done
        print('Local Start and Goal: ', grid_start, grid_goal)
        path, cost = a_star(graph, heuristic, graph_start, graph_goal)
        print('The path is', path, cost)
        # TODO: prune path to minimize number of waypoints
        # print('Path before pruning', len(path))
        # path = prune_path(path)
        # print('Path after pruning', len(path))
        # No need using a graph
        # TODO (if you're feeling ambitious): Try a different approach altogether!
        # done!

        # Convert path to waypoints
        waypoints = [[
            int(p[0] + north_offset),
            int(p[1] + east_offset), TARGET_ALTITUDE, 0
        ] for p in path]
        # Set self.waypoints
        self.waypoints = waypoints
        # TODO: send waypoints to sim (this is just for visualization of waypoints)
        self.send_waypoints()
Пример #19
0
    def plan_path(self):
        print("searching for a path ...")
        print("goal lon = {0}, lat = {1}".format(self.goal_lon, self.goal_lat))
        print("algorithm = {0}".format(self.algorithm))

        path = []
        north_offset = 0
        east_offset = 0

        print('global home: {0}, position: {1}, local position: {2}'.format(
            self.global_home, self.global_position, self.local_position))

        if self.algorithm == PathAlgorithm.Simple:
            grid, north_offset, east_offset = create_grid(
                self.data, self.default_altitude, self.safety_distance)
            print("north offset = {0}, east offset = {1}".format(
                north_offset, east_offset))

            # Define starting point on the grid (this is just grid center)
            # Set goal as some arbitrary position on the grid
            grid_start = self.to_grid_ne(self.start_lon, self.start_lat,
                                         north_offset, east_offset)
            grid_goal = self.to_grid_ne(self.goal_lon, self.goal_lat,
                                        north_offset, east_offset)
            print("grid start = {0}, goal = {1}".format(grid_start, grid_goal))

            path, _ = a_star(grid, heuristic, grid_start, grid_goal)
            print("count of waypoints: ", len(path))
            path = prune_waypoints(path)
            print("count of waypoints after prune: ", len(path))

            if self.plot_maps:
                plot_map(grid, path, grid_start, grid_goal)

        elif self.algorithm == PathAlgorithm.Skeleton:
            grid, north_offset, east_offset = create_grid(
                self.data, self.default_altitude, self.safety_distance)
            print("north offset = {0}, east offset = {1}".format(
                north_offset, east_offset))

            # Define starting point on the grid (this is just grid center)
            # Set goal as some arbitrary position on the grid
            grid_start = self.to_grid_ne(self.start_lon, self.start_lat,
                                         north_offset, east_offset)
            grid_goal = self.to_grid_ne(self.goal_lon, self.goal_lat,
                                        north_offset, east_offset)
            print("grid start = {0}, goal = {1}".format(grid_start, grid_goal))

            skeleton = medial_axis(invert(grid))
            skeleton_start, skeleton_goal = find_start_goal(
                skeleton, grid_start, grid_goal)

            print("skeleton start = {0}, goal = {1}".format(
                skeleton_start, skeleton_goal))

            path, _ = a_star_skeleton(
                invert(skeleton).astype(np.int), heuristic,
                tuple(skeleton_start), tuple(skeleton_goal))
            print("count of waypoints: ", len(path))
            path = prune_waypoints(path)
            print("count of waypoints after prune: ", len(path))

            if self.plot_maps:
                plot_map_skeleton(grid, skeleton, path, grid_start, grid_goal)

        elif self.algorithm == PathAlgorithm.Graphs:
            grid, edges, north_offset, east_offset = create_grid_and_edges(
                self.data, self.default_altitude, self.safety_distance)
            print("north offset = {0}, east offset = {1}".format(
                north_offset, east_offset))

            start_ne = self.to_grid_ne(self.start_lon, self.start_lat,
                                       north_offset, east_offset)
            goal_ne = self.to_grid_ne(self.goal_lon, self.goal_lat,
                                      north_offset, east_offset)
            print("grid start_ne = {0}, goal_ne = {1}".format(
                start_ne, goal_ne))

            g = nx.Graph()
            for e in edges:
                p1 = e[0]
                p2 = e[1]
                dist = la.norm(np.array(p2) - np.array(p1))
                g.add_edge(p1, p2, weight=dist)
            start_ne_g = closest_point(g, start_ne)
            goal_ne_g = closest_point(g, goal_ne)

            path, cost = a_star_graph(g, heuristic, start_ne_g, goal_ne_g)

            print("count of waypoints: ", len(path))
            path = prune_waypoints(path)
            print("count of waypoints after prune: ", len(path))

            if self.plot_maps:
                plot_map_graph(grid, edges, path, start_ne, goal_ne)

        # Convert path to waypoints
        waypoints = [[
            int(p[0]) + north_offset,
            int(p[1]) + east_offset, self.default_altitude, 0
        ] for p in path]
        self.waypoints = waypoints
        self.send_waypoints()
        self.flight_state = States.PLANNING