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
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
firstline = open('colliders.csv').readline().split(',')
lat0 = float(firstline[0].strip("lat0 "))
lon0 = float(firstline[1].strip("lon0 "))
print('lat0 = {}, lon0 = {}'.format(lat0, lon0))
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
local_position = global_to_local(self.global_position,
self.global_home)
# TODO: convert to current local position using global_to_local()
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
grid, edges, north_offset, east_offset = create_grid_and_edges(
data, TARGET_ALTITUDE, SAFETY_DISTANCE)
offset = np.array([north_offset, east_offset, 0])
#create the graph with the weight of the edges using euclidean distance between the points
G = nx.Graph()
for e in edges:
p1 = e[0]
p2 = e[1]
dist = LA.norm(np.array(p2) - np.array(p1))
G.add_edge(p1, p2, weight=dist)
start_pos = closest_point(G, local_position - offset)
local_goal = global_to_local(
[float(self.goal_lon),
float(self.goal_lat),
float(self.goal_alt)], self.global_home) - offset
goal_pos = closest_point(G, local_goal)
print('Local Start and Goal: ', start_pos, goal_pos)
path, _ = a_star_graph(G, start_pos, goal_pos)
path = prune_path(path, 0.5)
# Convert path to waypoints
waypoints = [[
int(p[0] + north_offset),
int(p[1] + east_offset), TARGET_ALTITUDE, 0
] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def 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()
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()
#
# 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()
def find_path(self, data, TARGET_ALTITUDE, SAFETY_DISTANCE, start_v,
goal_v, nmin, emin):
"""
Path search with different algorithms
INPUT: search_alg = [VORONOI, MEDIAL_AXIS]
OUTPUT: path
"""
if self.search_alg == SearchAlg.VORONOI:
# Define a grid for a particular altitude and safety margin around obstacles
#grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
#print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
import time
t0 = time.time()
grid, edges = create_grid_and_edges(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print('Found %5d edges' % len(edges))
#print(flight_altitude, safety_distance)
print('graph took {0} seconds to build'.format(time.time() - t0))
# create the graph with the weight of the edges
# set to the Euclidean distance between the points
G = nx.Graph()
for e in edges:
p1 = tuple(e[0])
p2 = tuple(e[1])
dist = np.linalg.norm(np.array(p2) - np.array(p1))
G.add_edge(p1, p2, weight=dist)
graph_start, graph_goal = start_goal_graph(G, start_v, goal_v)
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal on Voronoi graph: ', graph_start,
graph_goal)
path_graph, cost = a_star_graph(G, eucl_heuristic, graph_start,
graph_goal)
print('Number of edges', len(path_graph), 'Cost', cost)
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
waypoints = [[
int(p[0] + nmin),
int(p[1] + emin), TARGET_ALTITUDE, 0
] for p in path_graph]
return waypoints
elif self.search_alg == SearchAlg.MEDIAL_AXIS:
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(
data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
#grid_start = (-north_offset, -east_offset)
#grid_start =(int(start_v[0][0]),int(start_v[0][1]))
# TODO: convert start position to current position rather than map center
# Set goal as some arbitrary position on the grid
#grid_goal = (-north_offset + 10, -east_offset + 10)
#grid_goal = (int(goal_v[0][0]),int(goal_v[0][1]))
# TODO: adapt to set goal as latitude / longitude position and convert
skeleton = medial_axis(invert(grid))
#print(skeleton.shape)
skel_start, skel_goal = find_start_goal_skeleton(
skeleton, start_v[0:2], goal_v[0:2])
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal on Skeleton: ', skel_start, skel_goal)
#Manhattan heuristic
#print(invert(skeleton).astype(np.int))
path, cost = a_star(
invert(skeleton).astype(np.int), manh_heuristic,
tuple(skel_start), tuple(skel_goal))
# TODO: prune path to minimize number of waypoints
pruned_path = prune_path(path)
# TODO (if you're feeling ambitious): Try a different approach altogether!
waypoints = [[
int(p[0] + nmin),
int(p[1] + emin), TARGET_ALTITUDE, 0
] for p in pruned_path]
return waypoints
elif self.search_alg == SearchAlg.PROBABILISTIC:
num_samp = 1000
sampler = Sampler(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
start = (start_v[0] + nmin, start_v[1] + emin, start_v[2])
goal = (goal_v[0] + nmin, goal_v[1] + emin, goal_v[2])
polygons = sampler._polygons
nodes = sampler.sample(num_samp)
#for ind in range(len(nodes)):
# nodes[ind] = (nodes[ind][0] - nmin, nodes[ind][1] - emin, nodes[ind][2])
print('number of nodes', len(nodes), 'local NE', nodes[1])
nearest_neighbors = 10
import time
t0 = time.time()
g = create_graph(nodes, nearest_neighbors, polygons)
print('graph took {0} seconds to build'.format(time.time() - t0))
print("Number of edges", len(g.edges))
grid, north_offset, east_offset = create_grid(
data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
prob_start, prob_goal = start_goal_graph(g, start, goal)
print('Local Start and Goal on Probabilistic map: ', prob_start,
prob_goal)
path, cost = a_star_graph(g, heuristic, prob_start, prob_goal)
#path_pairs = zip(path[:-1], path[1:])
"""
plt.imshow(grid, cmap='Greys', origin='lower')
nmin = np.min(data[:, 0])
emin = np.min(data[:, 1])
# draw nodes
for n1 in g.nodes:
plt.scatter(n1[1] - emin, n1[0] - nmin, c='red')
# draw edges
for (n1, n2) in g.edges:
plt.plot([n1[1] - emin, n2[1] - emin], [n1[0] - nmin, n2[0] - nmin], 'grey')
# TODO: add code to visualize the path
path_pairs = zip(path[:-1], path[1:])
for (n1, n2) in path_pairs:
plt.plot([n1[1] - emin, n2[1] - emin], [n1[0] - nmin, n2[0] - nmin], 'green')
plt.plot(start[1]-emin, start[0]-nmin, 'bv')
plt.plot(goal[1]-emin, goal[0]-nmin, 'bx')
plt.xlabel('NORTH')
plt.ylabel('EAST')
plt.show()
"""
waypoints = [[int(p[0]), int(p[1]), int(p[2]), 0] for p in path]
return waypoints
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 4
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
with open('colliders.csv', newline='') as f:
reader = csv.reader(f)
row1 = next(reader) # gets the first line
lat0 = float(row1[0].split()[1])
lon0 = float(row1[1].split()[1])
print('lat0:', lat0, ', lon0:', lon0)
# TODO: set home position to (lon0, lat0, 0)
self.global_home[0] = lon0
self.global_home[1] = lat0
self.global_home[2] = 0
#self.global_home[0]=self._longitude
#self.global_home[1]=self._latitude
#self.global_home[2]=0
print('global home {0}'.format(self.global_home))
# TODO: retrieve current global position
global_position = self.global_position
# TODO: convert to current local position using global_to_local()
local_position = global_to_local(global_position, self.global_home)
print('local position:', local_position)
self.local_position[0] = local_position[0]
self.local_position[1] = local_position[1]
self.local_position[2] = local_position[2]
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
# grid_start = (-north_offset, -east_offset)
grid_start = (int(self.local_position[0] - north_offset),
int(self.local_position[1] - east_offset))
# TODO: convert start position to current position rather than map center
# Set goal as some arbitrary position on the grid
# grid_goal = (-north_offset , -east_offset )
# TODO: adapt to set goal as latitude / longitude position and convert
goal_north = 100
goal_east = -150
grid_goal = (-north_offset + goal_north, -east_offset + goal_east)
if self.choices_list[self.choice_idx] == 'astar':
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
if self.choices_list[self.choice_idx] == 'medial_axis':
skeleton = medial_axis(invert(grid))
skel_start, skel_goal = find_start_goal(skeleton, grid_start,
grid_goal)
print('Local Start and Goal: ', skel_start, skel_goal)
path, _ = a_star(
invert(skeleton).astype(np.int), heuristic, tuple(skel_start),
tuple(skel_goal))
if self.choices_list[self.choice_idx] == 'voronoi_graph_search':
grid, edges, north_offset, east_offset = create_grid_and_edges(
data, TARGET_ALTITUDE, SAFETY_DISTANCE)
G = create_weighted_graph(edges)
start_ne_g = closest_point(G, grid_start)
goal_ne_g = closest_point(G, grid_goal)
print('Local Start and Goal: ', start_ne_g, goal_ne_g)
path, cost = a_star_graph(G, heuristic, start_ne_g, goal_ne_g)
print('path found from a_star')
if self.if_pruning:
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
pruned_path = prune_path(path)
print('path found from a_star is pruned')
# Convert path to waypoints
# waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE,
np.arctan2((q[1] - p[1]), (q[0] - p[0]))
] for p, q in zip(pruned_path[:-1], pruned_path[1:])]
else:
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
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()
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()
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 = 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()
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()
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