def get_path(start=(0, 0, 6), goal=(331, 116, 19)):
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
north_offset = -316
east_offset = -445
sampler = Sampler(data)
print("AAA")
polygons = sampler._polygons
print("BBB")
nodes = sampler.sample(3000)
# (339, 380) (441, 519)
# start = (0, 0, 6)
# goal = (331, 116, 19)
nodes += [start, goal]
print(nodes)
g = create_graph(nodes, k=10, polygons=polygons)
path, _ = a_star_graph(g, heuristic, start, goal)
waypoints = [[int(p[0]), int(p[1]), int(p[2]), 0] for p in path]
path_dict = {'path': waypoints}
print(waypoints)
with open('path.json', 'w') as json_file:
json.dump(path_dict, json_file)
with open('path.json', 'r') as json_file2:
dta = json.load(json_file2)
print(dta['path'])
return 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):
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
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.0
SAFETY_DISTANCE = 5.0
self.target_position[2] = -self.local_position[2] + TARGET_ALTITUDE
# Read lat0, lon0 from colliders into floating point values
with open('colliders.csv') as f:
first_line = f.readline()
lat0 = float(first_line.split(',')[0].strip().split(' ')[1])
lon0 = float(first_line.split(',')[1].strip().split(' ')[1])
# Set home position to (lat0, lon0, 0)
self.set_home_position(lon0, lat0, 0.0)
# Retrieve current global position
# global_position = (self._longitude, self._latitude, self._altitude)
# Convert current global position (lon, lat, up) to a local position (north, east, down)
# local_position = global_to_local(global_position, self.global_home)
print('global home (lat, lon, alt) {}'.format(self.global_home))
print('global position (lat, lon, alt) {}'.format(
self.global_position))
print('local position (nor, est, dwn) {}'.format(self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# # Create Voronoi Graph for a particular altitude and safety margin around obstacles
# # Only need to run once, save the graph to a file. Next time just load graph file
# t1 = time.time()
# edges= create_voronoi(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
# t_edges = time.time() - t1
# print("It takes {} seconds to create voronoi graph".format(np.round(t_edges,2)))
# t2 = time.time()
# 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)
# t_graph = time.time() - t2
# print("It takes {} seconds to create load edges to networkx graph".format(np.round(t_graph,2)))
# nx.write_gpickle(G, "voronoi.gpickle")
G = nx.read_gpickle("voronoi.gpickle")
# Convert start position to current position rather than map center
north_offset, east_offset, _, _ = calc_offset_gridsize(data)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
grid_start = (int(self.local_position[0]) - north_offset,
int(self.local_position[1]) - east_offset)
# Adapt to set goal as latitude / longitude position and convert
# goal_global = (-122.400835, 37.795407, 0.0)
# goal_global = (-122.4003, 37.7953, 34.0)
goal_local = global_to_local(goal_global, self.global_home)
grid_goal = (int(goal_local[0]) - north_offset,
int(goal_local[1]) - east_offset)
print('Local Start: ', grid_start)
print('Local Goal: ', grid_goal)
start_ne_g = closest_point(G, grid_start)
goal_ne_g = closest_point(G, grid_goal)
print("Start localtion on graph:", start_ne_g)
print("Goal localtion on graph:", goal_ne_g)
goal_g_local = (goal_ne_g[0] + north_offset,
goal_ne_g[1] + east_offset, 0)
goal_g_global = local_to_global(goal_g_local, self.global_home)
print("Goal global location:", goal_g_global)
# A* graph search for a path
t3 = time.time()
path, _ = a_star_graph(G, heuristic, start_ne_g, goal_ne_g)
t_search = time.time() - t3
print("It takes {} seconds to find a path".format(np.round(
t_search, 2)))
# prune path to minimize number of waypoints
print(len(path), "waypoints before pruning")
path = prune_path(path)
print(len(path), "waypoints after pruning")
# calculate waypoints altitude
path_tmp = np.array([path[0]] + path)
dist = np.linalg.norm(path_tmp[1:, :] - path_tmp[:-1, :], axis=1)
goal_alt = goal_global[2]
heights = np.cumsum(dist / sum(dist)) * goal_alt + TARGET_ALTITUDE
# calculate waypoints heading
# Set heading based on next waypoint position relative previous waypoint position
headings = np.arctan2(path_tmp[1:, 1] - path_tmp[:-1, 1],
path_tmp[1:, 0] - path_tmp[:-1, 0])
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, alt, heading]
for p, alt, heading in zip(path, heights, headings)]
takeoff_point = [
grid_start[0] + north_offset, grid_start[1] + east_offset,
int(-self.local_position[2]), waypoints[0][3]
]
waypoints.insert(0, takeoff_point)
landing_point = [
grid_goal[0] + north_offset, grid_goal[1] + east_offset,
goal_alt + TARGET_ALTITUDE, waypoints[-1][3]
]
waypoints.append(landing_point)
waypoints = [[
int(waypoint[0]),
int(waypoint[1]),
int(waypoint[2]), waypoint[3]
] for waypoint in waypoints]
print(waypoints)
# Set self.waypoints
self.waypoints = waypoints
# Send waypoints to sim
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# 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 = 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 ...")
self.target_position[2] = self.TARGET_ALTITUDE
lat0 = ''
lon0 = ''
with open('colliders.csv', newline='') as c:
reader = csv.reader(c)
lat0, lon0 = next(reader)
split = lat0.split()
split1 = lon0.split()
lat0, lon0 = float(split[1]), float(split1[1])
self.set_home_position(lon0, lat0, 0)
print('latitude {0}, longitude {1}'.format(self._latitude, self._longitude))
local_position = global_to_local(self.global_position, self.global_home)
print(local_position)
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, self.TARGET_ALTITUDE, self.SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
start = (int(local_position[0]), int(local_position[1]), self.TARGET_ALTITUDE)
#create graph representation
self.obstacles = extract_polygons(data, self.SAFETY_DISTANCE)
samples = create_samples(data, 400)
X = []
for p in self.obstacles:
X.append(p[0].bounds)
poly_tree = KDTree(X)
to_keep = []
for point in samples:
if not collides(self.obstacles, point, poly_tree):
to_keep.append(point)
start_adjusted = start
i = 0
while collides(self.obstacles, start_adjusted, poly_tree) and i < len(self.adjustments):
print('collision with start point!', i, len(self.adjustments) - 1)
start_adjusted = start
start_adjusted = (start_adjusted[0] + self.adjustments[i][0],start_adjusted[1] + self.adjustments[i][1], start_adjusted[2] + self.adjustments[i][2])
i += 1
to_keep.append(start_adjusted)
print('before collision check', len(samples), 'after collision check', len(to_keep))
print('number of obstacles', len(self.obstacles))
# for choosing a random point as the goal
# idx = np.random.randint(0,len(to_keep)-1)
# goal = to_keep[idx]
# for choosing the goal based on a random latitude and longitude
# goal_lon = np.random.randint(np.amin(data[:,0]), np.amax(data[:,0]))
# goal_lat = np.random.randint(np.amin(data[:,1]), np.amax(data[:,1]))
# goal = [goal_lon, goal_lat]
# for setting lat and lon based on terminal inputs
lon, lat = sys.argv[1], sys.argv[2]
goal = [lon, lat]
g = create_graph(to_keep, 8, poly_tree, self.obstacles)
# find the nodes closest to the start and goal positions
node_tree = KDTree(to_keep)
coords = [[start[0],start[1],0],[goal[0],goal[1],0]]
ind = node_tree.query(coords, k=1, return_distance=False)
start_approx, goal_approx = to_keep[ind[0][0]], to_keep[ind[1][0]]
# Run A* to find a path from start to goal
print('Local Start and Goal: ', start, goal)
print('Approximate Local Start and Goal: ', start_approx, goal_approx)
path, path_cost = a_star_graph(g, heuristic, start_approx, goal_approx)
print(path)
if len(path) == 0:
self.plan_path()
else:
# Convert path to waypoints
waypoints = [[int(p[0]), int(p[1]), int(p[2]), 0] for p in path]
print(waypoints)
# Set self.waypoints
self.waypoints = waypoints
# send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# Read lat0, lon0 from colliders.csv into floating point values
# I just use `open()` method and `f.readline()` to read the first line,
# which line it read depend on the times it calls, I just call it once so it's the first line,
# then slice out the number
with open('colliders.csv', 'r') as f:
first_line_data = f.readline().rstrip().split(', ')
lat0 = float(first_line_data[0][5:])
lon0 = float(first_line_data[1][5:])
# Set home position
self.set_home_position(lon0, lat0, 0)
# Retrieve current global position
gp = self.global_position # (lon, lat, up)
# Convert to current local position
lp = global_to_local(gp, self.global_home) # (north, east, down)
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
t0 = time.time()
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
print("Took {0} seconds to load csv file".format(time.time() - t0))
# Convert start position to current position
start = (lp[0], lp[1], TARGET_ALTITUDE)
t0 = time.time()
sampler = Sampler(data, self.global_home)
print(
"Took {0} seconds to initialize Sampler class".format(time.time() -
t0))
t0 = time.time()
nodes = sampler.sample(30)
# Set goal as some arbitrary position on the map
goal = sampler.sample_goal(nodes, 37.793, -122.4)
print('Local Start and Goal: ', start, goal)
nodes.append(start)
nodes.append(goal)
print("Took {0} seconds to create {1} sample nodes.".format(
time.time() - t0, len(nodes)))
t0 = time.time()
g = Graph(sampler.polygons, sampler.polygon_center_tree,
sampler.max_poly_xy)
g.create_graph(nodes, 10)
print("Took {0} seconds to build graph".format(time.time() - t0))
print("Number of edges:", len(g.edges))
t0 = time.time()
# Convert grid search to graph
path, _ = a_star_graph(g.graph, heuristic, start, goal)
print("Took {0} seconds to search path".format(time.time() - t0))
# Prune path to minimize number of waypoints
t0 = time.time()
path = prune_path(path)
print("Took {0} seconds to prune path".format(time.time() - t0))
# Convert path to waypoints
waypoints = [[int(p[0]), int(p[1]), TARGET_ALTITUDE, 0] for p in path]
self.waypoints = waypoints
# Send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
print("Searching for a path ...")
TARGET_ALTITUDE = 5.0
# read lat0, lon0 from colliders into floating point values
with open('colliders.csv') as f:
first_line = f.readline().split(',')
lat0, lon0 = float(first_line[0].split(' ')[-1]), float(
first_line[1].split(' ')[-1])
# set home position to (lon0, lat0, 0)
self.set_home_position(
lon0, lat0,
0) # set the current location to be the home position
# Convert drone global position to local position (relative to global home) 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))
# Load precomputed graph
print('Loading graph {0}'.format(len(self.graph.nodes)))
# Set grid start position at our current drone global position
drone_location = (self.local_position[0] - self.north_offset,
self.local_position[1] - self.east_offset,
self.local_position[2])
print('Drone location local: {0}, Drone location map: {1}'.format(
self.local_position, drone_location))
# Select the nearest node closest to the drone location
nearest_start = None
closest_distance = sys.float_info.max
for n in self.graph.nodes:
# heuristic is the Euclidean distance:
distance = heuristic(drone_location, n)
if distance < closest_distance:
closest_distance = distance
nearest_start = n
if nearest_start == None:
print('Error while getting closest starting node')
return
print('Found starting node = {0}'.format(nearest_start))
# Select a random goal node
rnd_goal = np.random.randint(len(self.graph.nodes))
goal = list(self.graph.nodes)[rnd_goal]
print('Selecting random goal[{0}]: {1}'.format(rnd_goal, goal))
print('****')
path, cost = a_star_graph(self.graph, heuristic, nearest_start, goal)
print('A* from {0} to {1} with a length of: {2}'.format(
nearest_start, goal, len(path)))
print('A* path: ', path)
# First waypoint of path
curr = path[0]
# Save height for Takeoff
self.target_position[2] = curr[2]
# Add first waypoint
waypoints = [[curr[0], curr[1], curr[2], 0]]
idx = 1
while idx < len(path):
prev_wp = waypoints[len(waypoints) - 1]
# idx indicate a good waypoint candidate to be inserted
# path[idx] is supposed to be a good candidate already for the A* construction
# Check following waypoints
while idx + 1 < len(path):
candidate_wp = path[idx + 1]
hit = False
cells = list(
bresenham(int(prev_wp[0]), int(prev_wp[1]),
int(candidate_wp[0]), int(candidate_wp[1])))
for c in cells:
# Check if we're in collision
if self.collision_grid[
c[0], c[1]] >= FLYING_ALTITUDE + SAFETY_DISTANCE:
hit = True
break
# If the path does not hit on obstacle, add it to the list
if not hit:
# It's a good candidate. Update idx
idx = idx + 1
else:
break
# Add the candidate using idx
good_candidate = path[idx]
curr_wp = [
good_candidate[0], good_candidate[1], good_candidate[2], 0
]
# Set heading of curr_wp based on relative position to prev_wp
heading = np.arctan2((curr_wp[1] - prev_wp[1]),
(curr_wp[0] - prev_wp[0]))
curr_wp[3] = heading
# Append it to waypoints
waypoints.append(curr_wp)
idx = idx + 1
# Set self.waypoints
self.waypoints = waypoints
#self.send_waypoints()
self.flight_state = States.PLANNING
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
self.target_position[2] = TARGET_ALTITUDE
with open('colliders.csv') as f:
lat_lon_data = f.readline().split()
lat0, lon0 = float(lat_lon_data[1].strip(',')), float(
lat_lon_data[3])
self.set_home_position(lon0, lat0, 0)
north_home, east_home, down_home = global_to_local(
self.global_position, self.global_home)
# TODO: read lat0, lon0 from colliders into floating point values
# TODO: set home position to (lon0, lat0, 0)
# TODO: retrieve current global position
# TODO: convert to current local position using global_to_local()
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)
graph, xmin, ymin, xmax, ymax, zmax, obstacle_tree, obstacle_dict, max_radius, valid_nodes = create_graph(
data)
graph_start = closest_node(graph, self.local_position)
graph_goal = closest_node(graph, [
np.random.uniform(0, xmax),
np.random.uniform(0, ymax),
np.random.uniform(0, zmax)
])
#visualize_graph(data, graph_start, graph_goal, graph, valid_nodes)
#graph_goal = closest_node(graph, global_to_local([np.random.randint(xmin, xmax),
# np.random.randint(ymin, ymax),
# np.random.randint(0, zmax)], self.global_home))
# Define a grid for a particular altitude and safety margin around obstacles
#grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
#print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
#grid_start = (-north_offset, -east_offset)
# TODO: convert start position to current position rather than map center
# Set goal as some arbitrary position on the grid
# TODO: adapt to set goal as latitude / longitude position and convert
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', graph_start, graph_goal)
path, _ = a_star_graph(graph, heuristic, graph_start, graph_goal)
pruned_path = prune_path(path, obstacle_tree, obstacle_dict,
max_radius)
#visualize_path(data, graph, pruned_path, graph_start, graph_goal)
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [[int(p[0]), int(p[1]), int(p[2]), 0] for p in pruned_path]
print(f'waypoints {waypoints}')
#waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
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, g_goal):
"""
Plan a path from current position to specified global position (lat, lon, alt)
:param g_goal: goal, geodetic coordinates
:return:
"""
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# read lat0, lon0, coordinates of the map center
filename = 'colliders.csv'
with open(filename) as f:
for line in f:
break
(_, lat0, _, lon0) = line.split()
lat0 = float(lat0.strip(','))
lon0 = float(lon0.strip(','))
# set home position to center of the map
self.set_home_position(lon0, lat0, 0)
# retrieve current global position of the Drone. (lon, lat, alt) as np.array
# convert current global coordinates to NED frame (centered at home)
l_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
map_data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define 2D grid representation of the map at specified altitude, with specified safety margin around obstacles
# create_grid discretizes the map at 1 meter resolution.
grid, north_offset, east_offset = create_grid(map_data,
TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Convert local position to grid coordinates
def int_round(i):
return int(round(i))
def local_to_grid(l_pos, north_offset, east_offset):
return (-north_offset + int_round(l_pos[0]),
-east_offset + int_round(l_pos[1]))
grid_start = local_to_grid(l_pos, north_offset, east_offset)
# Set goal in grid coordinates
l_goal = global_to_local(g_goal, self.global_home)
grid_goal = local_to_grid(l_goal, north_offset, east_offset)
# Run A* to find a path from start to goal on a graph using Voronoi regions
print('Local Start and Goal: ', grid_start, grid_goal)
starttime = time.time()
path, _ = a_star_graph(map_data, TARGET_ALTITUDE, SAFETY_DISTANCE,
grid_start, grid_goal)
print('planned using A* on full graph in {} secs. path length: {}'.
format(time.time() - starttime, len(path)))
path = prune_path(path)
print('pruned path length: {}'.format(len(path)))
# Convert path to waypoints
heading = 0
waypoints = []
wp1 = None
total_distance = 0.0
for p in path:
wp2 = [
int_round(p[0]) + north_offset,
int_round(p[1]) + east_offset, TARGET_ALTITUDE, heading
]
if wp1 is not None:
heading = np.arctan2(wp2[1] - wp1[1], wp2[0] - wp1[0])
wp2[3] = heading
total_distance += np.linalg.norm(np.array(wp2) - np.array(wp1))
waypoints.append(wp2)
wp1 = wp2
print("total planned 3D path length: {:.2f} meters".format(
total_distance))
# Set self.waypoints to follow
self.waypoints = waypoints
# send waypoints to sim for visualization
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 7
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])))
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
sampler = Sampler(data)
polygons = sampler._polygons
# Example: sampling 1200 points and removing
# ones conflicting with obstacles.
nodes = sampler.sample(1200)
print("Non collider nodes:", len(nodes))
t0 = time.time()
# Uncomment line 162 to generate the graph from the sampled points, and comment out line 163.
# Increase the Mavlink timer to avoid disconnecting from the simulator.
#G = create_graph(nodes, 10, polygons)
G = nx.read_gpickle(
"graph_1200_SD_nodes.gpickle"
) # Comment out this line if generating the graph instead of using the saved graph
print('graph took {0} seconds to build'.format(time.time() - t0))
print("Number of edges", len(G.edges))
# 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), TARGET_ALTITUDE)
# TODO: adapt to set goal as latitude / longitude position and convert
goal_global_pos = (-122.394700, 37.789825, 13)
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_global_pos[2])
print("goal_local_N:", goal_local_pos[0], "goal_local_E:",
goal_local_pos[1], "goal_local_alt:", goal_local_pos[2])
#goal_ne = (455., 635., 20.)
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)
# Run A* to find a path from start to goal
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]), int(p[2])] 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))
print("PRUNED PATH:", pruned_path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, p[2], 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 = 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 = 5
RANDOM_START = False
GRAPH_SEARCH = False
self.target_position[2] = TARGET_ALTITUDE
# Read lat0, lon0 from colliders into floating point values
with open('colliders.csv') as f:
line = f.readline()
latLonArray = map(lambda x: x.strip(), line.split(','))
lat0, lon0 = map(lambda x: float(x.split(' ')[1]), latLonArray)
# Set the home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# Retrieve current global position
current_global_position = np.array(
[self._longitude, self._latitude, self._altitude])
# Convert to current local position using global_to_local()
current_local_position = global_to_local(current_global_position,
self.global_home)
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
if (RANDOM_START):
random_start_pos = self.randomPoint(grid, north_offset,
east_offset, 0,
self.global_home)
self.set_home_position(random_start_pos[1], random_start_pos[0], 0)
current_global_position = np.array(
[self._longitude, self._latitude, self._altitude])
current_local_position = global_to_local(current_global_position,
self.global_home)
# Convert start position to current position
grid_start = (int(current_local_position[0] - north_offset),
int(current_local_position[1]) - east_offset)
# Random Start
if (RANDOM_START):
local_start = global_to_local(random_start_pos, self.global_home)
grid_start = (int(local_start[0]) - north_offset,
int(local_start[1]) - east_offset)
# Set goal as latitude / longitude position: np.array([-122.39747, 37.79275, TARGET_ALTITUDE])
goal_pos = self.randomPoint(grid, north_offset, east_offset,
TARGET_ALTITUDE, self.global_home)
# Convert lat/lon to local grid: (325, 455)
local_goal = global_to_local(goal_pos, self.global_home)
grid_goal = (int(local_goal[0]) - north_offset,
int(local_goal[1]) - east_offset)
print('Local Start and Goal: ', grid_start, grid_goal)
if not GRAPH_SEARCH:
# Run A* to find a path from start to goal
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# Prune path to minimize number of waypoints
pruned_path = [p for p in path]
i = 1
while i < len(pruned_path) - 1:
pt1 = pruned_path[i - 1]
pt2 = pruned_path[i]
pt3 = pruned_path[i + 1]
if self.collinearity_check(pt1, pt2, pt3):
del pruned_path[i:i + 1]
else:
i = i + 1
# Try a different approach altogether! - Probabilistic Graph
if (GRAPH_SEARCH):
# Get samples and polygons
samples = random_samples(data, 200)
polygons = extract_polygons(data)
to_keep = []
for point in samples:
if not collides(polygons, point):
to_keep.append(point)
# # Create the graph and determine start and goal points on graph
graph = create_graph(to_keep, 10, polygons)
print(graph)
graph_start = closest_point(graph, current_local_position)
graph_goal = closest_point(graph, goal_pos)
# Run A*
path, _ = a_star_graph(graph, heuristic, graph_start, graph_goal)
# Add original start and goal
path.append(local_goal)
path.insert(0, current_local_position)
# If no path found, just takeoff and land
if len(path) == 2:
path.pop()
# Adjust for offset - should refactor
pruned_path = [[int(p[0]) - north_offset,
int(p[1]) - east_offset] for p in path]
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# Send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
latitude, longitude = self.get_home_position()
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(longitude, latitude, 0.0)
# TODO: retrieve current global position
# TODO: convert to current local position using global_to_local()
current_global_position = np.array(
[self._longitude, self._latitude, self._altitude])
self._north, self._east, self._down = global_to_local(
current_global_position, self.global_home)
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Get a graph representation with edges for a particular altitude and safety margin around obstacles
graph, north_offset, east_offset = create_graph(
data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# TODO: convert start position to current position rather than map center
current_position = self.local_position
start = (-north_offset + int(current_position[0]),
-east_offset + int(current_position[1]))
# Set goal as some arbitrary position on the grid
# TODO: adapt to set goal as latitude / longitude position and convert
goal_longitude = -122.397745
goal_latitude = 37.793837
#goal_longitude = -122.394324
#goal_latitude = 37.791684
#goal_longitude = -122.401216
#goal_latitude = 37.796713
target_location = global_to_local([goal_longitude, goal_latitude, 0],
self.global_home)
goal = (int(-north_offset + target_location[0]),
int(-east_offset + target_location[1]))
# Compute the closest points to the start and goal positions
closest_start = closest_point(
graph, (-north_offset + int(current_position[0]),
-east_offset + int(current_position[1])))
closest_goal = closest_point(graph,
(-north_offset + int(target_location[0]),
-east_offset + int(target_location[1])))
print("Local Start and Goal : ", start, goal)
# Run A* to find a path from start to goal
path, _ = a_star_graph(graph, closest_start, closest_goal)
print("Found a path of length : ", len(path))
# Check for collinearity between points and prune the obtained path
path = prune_path(path)
# Convert path to waypoints
self.waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
# TODO: send waypoints to simulator
print("Sending waypoints to the simulator")
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 4
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
with open('colliders.csv', newline='') as f:
reader = csv.reader(f)
row1 = next(reader) # gets the first line
lat0 = float(row1[0].split()[1])
lon0 = float(row1[1].split()[1])
print('lat0:', lat0, ', lon0:', lon0)
# TODO: set home position to (lon0, lat0, 0)
self.global_home[0] = lon0
self.global_home[1] = lat0
self.global_home[2] = 0
#self.global_home[0]=self._longitude
#self.global_home[1]=self._latitude
#self.global_home[2]=0
print('global home {0}'.format(self.global_home))
# TODO: retrieve current global position
global_position = self.global_position
# TODO: convert to current local position using global_to_local()
local_position = global_to_local(global_position, self.global_home)
print('local position:', local_position)
self.local_position[0] = local_position[0]
self.local_position[1] = local_position[1]
self.local_position[2] = local_position[2]
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
# grid_start = (-north_offset, -east_offset)
grid_start = (int(self.local_position[0] - north_offset),
int(self.local_position[1] - east_offset))
# TODO: convert start position to current position rather than map center
# Set goal as some arbitrary position on the grid
# grid_goal = (-north_offset , -east_offset )
# TODO: adapt to set goal as latitude / longitude position and convert
goal_north = 100
goal_east = -150
grid_goal = (-north_offset + goal_north, -east_offset + goal_east)
if self.choices_list[self.choice_idx] == 'astar':
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
if self.choices_list[self.choice_idx] == 'medial_axis':
skeleton = medial_axis(invert(grid))
skel_start, skel_goal = find_start_goal(skeleton, grid_start,
grid_goal)
print('Local Start and Goal: ', skel_start, skel_goal)
path, _ = a_star(
invert(skeleton).astype(np.int), heuristic, tuple(skel_start),
tuple(skel_goal))
if self.choices_list[self.choice_idx] == 'voronoi_graph_search':
grid, edges, north_offset, east_offset = create_grid_and_edges(
data, TARGET_ALTITUDE, SAFETY_DISTANCE)
G = create_weighted_graph(edges)
start_ne_g = closest_point(G, grid_start)
goal_ne_g = closest_point(G, grid_goal)
print('Local Start and Goal: ', start_ne_g, goal_ne_g)
path, cost = a_star_graph(G, heuristic, start_ne_g, goal_ne_g)
print('path found from a_star')
if self.if_pruning:
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
pruned_path = prune_path(path)
print('path found from a_star is pruned')
# Convert path to waypoints
# waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE,
np.arctan2((q[1] - p[1]), (q[0] - p[0]))
] for p, q in zip(pruned_path[:-1], pruned_path[1:])]
else:
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
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 = 1
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
home_pos = open('colliders.csv').readline()
lat, lon = home_pos.replace(' ', '').replace('lat0',
'').replace('lon0',
'').split(',')
lat, lon = float(lat), float(lon)
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon, lat, 0)
# TODO: retrieve current global position
# Why? cant I just use self.global_position?
global_position = np.asarray(
[self._longitude, self._latitude, self._altitude])
local_pos = global_to_local(self.global_position, self.global_home)
self._north = local_pos[0]
self._east = local_pos[1]
self._down = local_pos[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))
#goal_pos = global_to_local((-122.397967, 37.792010, 0), self.global_home)
#goal_pos = global_to_local((-122.399177, 37.791030, 0), self.global_home)
#goal_pos = global_to_local((-122.399646, 37.791320, 0), self.global_home)
#goal_pos = global_to_local((-122.399646, 37.791320, 0), self.global_home)
goal_pos = global_to_local((-122.392980, 37.792826, 0),
self.global_home)
# this one is out of bounds
#goal_pos = global_to_local((-122.402583, 37.791741, 0), self.global_home)
use_grid = False
if use_grid:
# Set goal as some arbitrary position on the grid
grid_goal = (-north_offset + int(goal_pos[0]),
-east_offset + int(goal_pos[1]))
# Define starting point on the grid (this is just grid center)
grid_start = (int(self._north) - north_offset,
int(self._east) - east_offset)
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
path = prune_path_bresenham(path, grid)
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
else:
start_graph = (self._north, self._east, 0.0)
goal_graph = tuple(goal_pos)
print(start_graph)
print(goal_graph)
# num samples needs to be low because otherwise simulator does not react to commands after some timout I believe?
# how can I speed up the connect edge code?
graph, nodes = prm(data,
num_samples=400,
extra_points=[start_graph, goal_graph])
tree = KDTree(nodes)
indicies = tree.query([start_graph], 10, return_distance=False)[0]
start_graph = nodes[int(indicies[0])]
indicies = tree.query([goal_graph], 10, return_distance=False)[0]
goal_graph = nodes[int(indicies[0])]
path, _ = a_star_graph(graph, heuristic, start_graph, goal_graph)
# Convert path to waypoints
# Why do I need to cast this to int?
# does the simulator / API not support exact foat valuesfor coordinates?
# When I do not change to int this just hangs at takeoff transition
waypoints = [[int(p[0]), int(p[1]), TARGET_ALTITUDE, 0]
for p in path]
print(waypoints)
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
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_graph(self):
"""
Construct a configuration space using a graph representation, set destination GPS position, find a path from start (current) position to destination, and minimize and set waypoints.
"""
self.flight_state = States.PLANNING
print("Searching for a path ...")
data = np.loadtxt('map_obstacle_course.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
TARGET_ALTITUDE = 0.3 / 0.0254
SAFETY_DISTANCE = 0.25 / 0.0254
st = time.time()
# grid, G = create_graph_and_edges_crazyflie(
# data, 130, 86, TARGET_ALTITUDE, SAFETY_DISTANCE)
GRID_NORTH_SIZE = 130
GRID_EAST_SIZE = 86
NUMBER_OF_NODES = 200
OUTDEGREE = 4
grid, G = construct_road_map_crazyflie(data, GRID_NORTH_SIZE,
GRID_EAST_SIZE, TARGET_ALTITUDE,
SAFETY_DISTANCE,
NUMBER_OF_NODES, OUTDEGREE)
# visualize_prob_road_map_crazyflie(data, grid, G)
time_taken = time.time() - st
print(f'create_graph_and_edges() took: {time_taken} seconds')
grid_start = (32, 40, 0)
grid_goal = (110, 40, 0)
# Find closest node on the graph
g_start, g_goal = find_start_goal(G, grid_start, grid_goal)
print("Start and Goal location:", grid_start, grid_goal)
print("Start and Goal location on graph:", g_start, g_goal)
path, _ = a_star_graph(G, heuristic, g_start, g_goal)
path.append(grid_goal)
new_path = []
# new_path.append(grid_start)
new_path.append(g_start)
new_path.extend(path)
print(new_path)
unique_path = []
for p in new_path:
if p not in unique_path:
unique_path.append(p)
print(unique_path)
# Reduce waypoints
reduced_path = condense_waypoints_crazyflie(grid, unique_path)
print(f'reduced_path: {reduced_path}')
waypoints = []
# modify path coordinates as offset from the takeoff position
# i.e., we treat the firt position as origin
for p in reduced_path:
print(np.array(p) - np.array(grid_start))
offset = (np.array(p) - np.array(grid_start)) * 0.0254
offset[2] = TARGET_ALTITUDE * 0.0254
waypoints.append(list(offset))
print(f'All waypoints: {waypoints}')
self.all_waypoints = waypoints
visualize_prob_road_map_crazyflie(data,
grid,
G,
grid_start,
grid_goal,
unique_path,
all_nodes=False)
visualize_prob_road_map_crazyflie(data,
grid,
G,
grid_start,
grid_goal,
reduced_path,
all_nodes=False)
return self.all_waypoints
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 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 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 prob_road_map(self):
"""Probablistic Road Map implementation to create configuration space, set start and goal positions, find path from start to goal, and condense and set waypoints for navigation.
"""
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
lat0, lon0 = self.read_home_lat_lon('colliders.csv')
# TODO: set home position to (lon0, lat0, 0)
print(f'Setting home to (lon0, lat0, 0): ({lon0, lat0, 0})')
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
global_position = self.global_position
# TODO: convert to current local position using global_to_local()
local_home = global_to_local(global_position, self.global_home)
print(f'Home coordinates in local form: {local_home}')
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
print(f'Constructing Probablistic Road Map...')
st = time.time()
grid, G, north_offset, east_offset = construct_road_map(
data, TARGET_ALTITUDE, SAFETY_DISTANCE, 500, 4)
time_taken = time.time() - st
print(f'construct_road_map() took: {time_taken} seconds')
# TODO: convert start position to current position rather than map center
grid_start = (int(self.local_position[0] - north_offset),
int(self.local_position[1] - east_offset))
# goal_lon_lat = [-122.398525, 37.793510, 0]
# goal_lon_lat = [-122.398250, 37.793875, 0]
# goal_lon_lat = [-122.398275, 37.796875, 0] # (point3) reachable if SAFETY_DISTANCE = 5
# goal_lon_lat = [-122.399970, 37.795947, 0] # reachable if SAFETY_DISTANCE = 5
# goal_lon_lat = [-122.393284, 37.790545, 0] # (point2)
goal_lon_lat = [-122.401173, 37.795514, 0] # (point1)
# goal_lon_lat = [-122.393805, 37.795825, 0] # not reachable if TARGET_ALTITUDE = 5
# (point1) -> (point2) -> (point3) this sequence results in some
# trees getting in the way of the drone. This is a good example
# of imprecise mapping and we may have to use receding horizon
# planning which utilizes a 3D state space for a limited region
# around the drone.
grid_goal = global_to_local(goal_lon_lat, self.global_home)
grid_goal = (int(grid_goal[0] - north_offset),
int(grid_goal[1] - east_offset))
# Find closest node on probablistic road map graph
g_start, g_goal = find_start_goal(G, grid_start, grid_goal)
print("Start and Goal location:", grid_start, grid_goal)
print("Start and Goal location on graph:", g_start, g_goal)
path, _ = a_star_graph(G, heuristic, g_start, g_goal)
# Add the final goal state (instead of the approximated
# goal on the graph)
path.append(grid_goal)
print(path)
# Reduce waypoints
path = condense_waypoints(grid, path)
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
dist = distance(p2, p1)
G.add_edge(p1,p2,weight=dist)
plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'b-')
dist_s = distance(start_ne, p1)
if dist_s < close_start_dist :
close_start_dist = dist_s
close_start_pt = p1
dist_g = distance(goal_ne, p1)
if dist_g < close_goal_dist :
close_goal_dist = dist_g
close_goal_pt = p1
print("graph digested", time.clock())
path = a_star_graph(G, heuristic, close_start_pt, close_goal_pt)
print("A* graph found", time.clock())
shortestpath = nx.dijkstra_path(G, close_start_pt, close_goal_pt, 'weight')
print("dijkstra graph found", time.clock())
plt.plot(start_ne[1], start_ne[0], 'rx')
plt.plot(goal_ne[1], goal_ne[0], 'rx')
plt.plot(close_start_pt[1], close_start_pt[0], 'yo')
plt.plot(close_goal_pt[1], close_goal_pt[0], 'yo')
# plot dijkstra path in cyan
for pt in shortestpath :
plt.plot(pt[1], pt[0], 'g^' )
# plot path in yellow
for p in range(0, len(path[0])-1) :
i, j = random.randint(0, grid.shape[0] - 1), random.randint(
0, grid.shape[1] - 1)
if not grid[i, j]: p = (i, j)
return p
# grid_start = findRandomFreePosition()
# grid_goal = findRandomFreePosition()
# Find closest points in voronoi graph
graph_start = closest_point(G, grid_start)
graph_goal = closest_point(G, grid_goal)
print('Graph path: {0} -> {1}'.format(graph_start, graph_goal))
# Find path on graph
timer = time.time()
path, cost = a_star_graph(G, graph_start, graph_goal)
if path is None: continue
print('Found path on graph of {0} waypoints in {1}s'.format(
len(path),
time.time() - timer))
# Add to path 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 path on grid
timer = time.time()
pruned_path = prune_path(path, grid_narrow)
pruned_path = prune_path(pruned_path, grid_narrow)
print('Pruned path from {0} to {1} waypoints in {2}s'.format(
def perform_astar(Gr, Cg, nmin=0, emin=0):
#drone_location = (-emin, -nmin, 5.0) # map coordinates
drone_location = (445.04762260615826, 315.94609723985195, 5.0)
print('Find Start node from {0}'.format(drone_location))
nearest_start = None
closest_distance = sys.float_info.max
for n in Gr.nodes:
# heuristic is the Euclidean distance:
distance = heuristic(drone_location, n)
if distance < closest_distance:
closest_distance = distance
nearest_start = n
if nearest_start == None:
print('Error while getting closest starting node')
return
print('Found starting node = {0}'.format(nearest_start))
##########
goal_location = (240.7685, 360.76114, 5.0) # map coordinates
print('Find Goal node from {0}'.format(goal_location))
nearest_goal = None
closest_distance = sys.float_info.max
for n in Gr.nodes:
# heuristic is the Euclidean distance:
distance = heuristic(goal_location, n)
if distance < closest_distance:
closest_distance = distance
nearest_goal = n
################
start = nearest_start
print('Start: ', start)
goal = nearest_goal
print('Goal: ', goal)
path, cost = a_star_graph(Gr, heuristic, start, goal)
print(len(path), path)
if len(path) == 0:
return
waypoints = [[p[0], p[1], p[2], 0] for p in path]
print("start")
fig = plt.figure(figsize=(10,10))
plt.imshow(Cg, cmap='Greys', origin='lower')
path_pairs = zip(waypoints[:-1], waypoints[1:])
for (n1, n2) in path_pairs:
plt.plot([n1[1], n2[1]], [n1[0], n2[0]], 'green')
plt.scatter(drone_location[0], drone_location[1], c='blue') # (0,0)
plt.scatter(emin - emin, nmin - nmin , c='green') # Lowest point
plt.scatter(100, 0, c='purple') # (0,0)
plt.xlabel('EAST')
plt.ylabel('NORTH')
plt.show()
