def GetPath(self, north_offset, east_offset, grid, graph, start, goal):
if (grid[goal[0], goal[1]] == 1):
print("Goal is unreachable")
return None
start_g = pu.closest_point(graph, start)
goal_g = pu.closest_point(graph, goal)
print(goal_g)
path, _ = pu.a_starGraph(graph, start_g, goal_g)
# If it is not possible to find a path in tha graph, let's find a path in the grid
if (path is None):
print('Searching in grid')
newpath, _ = pu.a_star(grid, start, goal)
newpath = pu.prune_path(newpath)
else:
start_g = (int(start_g[0]), int(start_g[1]))
goal_g = (int(goal_g[0]), int(goal_g[1]))
print('start={0}, start_g={1}'.format(start, start_g))
pathToBegin, _ = pu.a_star(grid, start, start_g)
pathFromEnd, _ = pu.a_star(grid, goal_g, goal)
if (pathToBegin is None or pathFromEnd is None):
return None
newpath = pathToBegin + path + pathFromEnd
newpath = pu.prune_path(newpath)
return [[
int(p[0]) + north_offset,
int(p[1]) + east_offset, pu.TARGET_ALTITUDE, 0
] for p in newpath]
def 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()
for iteration in range(1):
print('> iteration #{0}'.format(iteration))
def findRandomFreePosition():
p = None
while not p:
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)
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
# 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
lat0, lon0 = np.loadtxt('colliders.csv',
delimiter=',',
dtype='str',
usecols=(0, 1))[0]
lat0 = float(lat0.split()[1])
lon0 = float(lon0.split()[1])
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
global_home = self.global_home
# TODO: retrieve current global position
global_position = self.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))
local_position = global_to_local(global_position, 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, north_offset, east_offset = create_grid_graph(
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 = (local_position[0] - north_offset,
local_position[1] - east_offset)
# Set goal as some arbitrary position on the grid
# grid_goal = (-north_offset + 10, -east_offset + 10)
# TODO: adapt to set goal as latitude / longitude position and convert
goal_global_position = (-122.39827005, 37.79639587, 0)
goal_local_position = global_to_local(goal_global_position,
global_home)
grid_goal = (int(goal_local_position[0] - north_offset),
int(goal_local_position[1] - east_offset))
print('Local Start and Goal: ', grid_start, grid_goal)
# Add weight to graph
G = nx.Graph()
for e in edges:
p1 = tuple(e[0])
p2 = tuple(e[1])
dist = LA.norm(np.array(p2) - np.array(p1))
G.add_edge(p1, p2, weight=dist)
# Find the closest point in the graph to our current location, same thing for the goal location.
start_g = closest_point(G, grid_start)
goal_g = closest_point(G, grid_goal)
print('Local Start and Goal in graph: ', start_g, goal_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(G, heuristic, start_g, goal_g)
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
pruned_path = prune_path(path)
pruned_path.append(grid_goal)
pruned_path.insert(0, grid_start)
# Convert path to waypoints
waypoints = [[
int(p[0] + north_offset),
int(p[1] + east_offset), TARGET_ALTITUDE, 0
] for p in pruned_path]
# waypoints = [[0, 0, 5, 0], [0, 1, 5, 0], [1, 1, 5, 0], [1, 2, 5, 0], [2, 2, 5, 0], [2, 3, 5, 0], [3, 3, 5, 0], [3, 4, 5, 0], [4, 4, 5, 0], [4, 5, 5, 0], [5, 5, 5, 0], [5, 6, 5, 0], [6, 6, 5, 0], [6, 7, 5, 0], [7, 7, 5, 0], [7, 8, 5, 0], [8, 8, 5, 0], [8, 9, 5, 0], [9, 9, 5, 0], [9, 10, 5, 0], [10, 10, 5, 0]]
# waypoints = [[10, 10, 5, 0], [7, 0, 5, 0], [26, 14, 5, 0], [32, 17, 5, 0], [34, 15, 5, 0], [44, 5, 5, 0], [54, -1, 5, 0], [64, -6, 5, 0], [84, -11, 5, 0], [93, -15, 5, 0], [94, -16, 5, 0], [98, -17, 5, 0], [104, -24, 5, 0], [129, -39, 5, 0], [149, -19, 5, 0], [154, -16, 5, 0], [174, -26, 5, 0], [184, -29, 5, 0], [204, -29, 5, 0], [204, -29, 5, 0], [212, -29, 5, 0], [214, -30, 5, 0], [224, -34, 5, 0], [244, -34, 5, 0], [254, -37, 5, 0], [258, -40, 5, 0], [274, -44, 5, 0], [279, -41, 5, 0], [294, -44, 5, 0], [314, -44, 5, 0], [324, -46, 5, 0], [327, -45, 5, 0], [344, -49, 5, 0], [362, -49, 5, 0], [364, -50, 5, 0], [374, -54, 5, 0], [397, -54, 5, 0], [404, -62, 5, 0], [407, -67, 5, 0], [409, -74, 5, 0], [412, -81, 5, 0], [428, -82, 5, 0], [432, -80, 5, 0], [434, -75, 5, 0]]
# Set self.waypoints
self.waypoints = waypoints
print(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 = 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 = 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
latitude, longitude = self.get_home_position()
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(longitude, latitude, 0.0)
# TODO: retrieve current global position
# TODO: convert to current local position using global_to_local()
current_global_position = np.array(
[self._longitude, self._latitude, self._altitude])
self._north, self._east, self._down = global_to_local(
current_global_position, self.global_home)
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Get a graph representation with edges for a particular altitude and safety margin around obstacles
graph, north_offset, east_offset = create_graph(
data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# TODO: convert start position to current position rather than map center
current_position = self.local_position
start = (-north_offset + int(current_position[0]),
-east_offset + int(current_position[1]))
# Set goal as some arbitrary position on the grid
# TODO: adapt to set goal as latitude / longitude position and convert
goal_longitude = -122.397745
goal_latitude = 37.793837
#goal_longitude = -122.394324
#goal_latitude = 37.791684
#goal_longitude = -122.401216
#goal_latitude = 37.796713
target_location = global_to_local([goal_longitude, goal_latitude, 0],
self.global_home)
goal = (int(-north_offset + target_location[0]),
int(-east_offset + target_location[1]))
# Compute the closest points to the start and goal positions
closest_start = closest_point(
graph, (-north_offset + int(current_position[0]),
-east_offset + int(current_position[1])))
closest_goal = closest_point(graph,
(-north_offset + int(target_location[0]),
-east_offset + int(target_location[1])))
print("Local Start and Goal : ", start, goal)
# Run A* to find a path from start to goal
path, _ = a_star_graph(graph, closest_start, closest_goal)
print("Found a path of length : ", len(path))
# Check for collinearity between points and prune the obtained path
path = prune_path(path)
# Convert path to waypoints
self.waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
# TODO: send waypoints to simulator
print("Sending waypoints to the simulator")
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
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 = 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()
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 = 10
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
filename = 'colliders.csv'
data = np.genfromtxt(filename, delimiter=',', dtype='str', max_rows=1)
lat0 = float(data[0].split(' ')[-1])
lon0 = float(data[1].split(' ')[-1])
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0.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(filename, 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, 0, 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 = self.global_position
# TODO: convert start position to current position rather than map center
grid_start = self.global_to_map(
grid_start, grid_start, grid.shape, north_offset, east_offset,
int(np.ceil(TARGET_ALTITUDE - self.local_position[2])))
# Set goal as some arbitrary position on the grid
# generate a random goal
x_min = 0
x_max = grid.shape[0]
y_min = 0
y_max = grid.shape[1]
grid_goal = random_goal(grid, x_min, x_max, y_min, y_max)
# If a goal is input manually, use the global variable GOAL_COORD in (long, lat)
# grid_goal = GOAL_COORD
# grid_goal = self.global_to_map(grid_goal, self.global_position, grid.shape, north_offset, east_offset, 60)
# 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)
# Use Voronoi graph
# load a graph
g = load_graph('map.gpickle')
print("Number of edges", len(g.edges))
# find closest points for start and goal
start_closest = closest_point(g, grid_start)
goal_closest = closest_point(g, grid_goal)
# a-star. If a path cannot be found, add graph points and paths around start/goal.
path_found = False
for i in range(3):
path, cost, path_found = a_star(g, heuristic, start_closest,
goal_closest)
if path_found:
break
g = random_graph_pts(grid, g, grid_start, samples=100)
g = random_graph_pts(grid, g, grid_goal, samples=100)
start_closest = closest_point(g, grid_start)
goal_closest = closest_point(g, grid_goal)
# TODO: prune path to minimize number of waypoints
pruned_path = prune_path(path, grid)
print('unpruned_path: ', path, type(path))
print('pruned_path: ', pruned_path, type(pruned_path))
# append destination to path, if destination is more than 1 meter away from last waypoint
if LA.norm(np.array(pruned_path[-1]) - np.array(grid_goal)) > 1.0:
pruned_path.append((grid_goal[0], grid_goal[1], grid_goal[2]))
# assign a heading at each waypoint based on direction of next waypoint
path_with_heading = get_heading(pruned_path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, p[2], p[3]]
for p in path_with_heading]
# 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 = int(self.goal_position[2] * 1.1) + 5
SAFETY_DISTANCE = 5
# Assuming home is always set at 0
self.target_position[2] = self.goal_position[2]
init_pos = self.get_home_coordinates()
self.set_home_position(init_pos[1], init_pos[0], 0)
local_position = global_to_local(self.global_position, self.global_home)
goal_position = global_to_local(self.goal_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)
graph = None
north_offset = None
east_offset = None
alt_dim = False
if(self.probabilistic_mode):
alt_dim = True
print('********************************************')
print(" Using probabalistic mode")
print('********************************************')
t0 = time.time()
graph = create_graph_probabilistic(data, TARGET_ALTITUDE, int(SAFETY_DISTANCE / 2))
print('graph took {0} seconds to build'.format(time.time() - t0))
north_offset, east_offset = get_min_north_east(data)
else:
print('********************************************')
print(" Using graph with voronoi")
print('********************************************')
graph, north_offset, east_offset = create_graph_voronoi(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print(north_offset, east_offset)
graph_start = self.get_relative_coords(local_position, north_offset, east_offset, alt_dim)
graph_goal = self.get_relative_coords(goal_position, north_offset, east_offset, alt_dim)
# Get closest points on the graph
graph_start_c = closest_point(graph, graph_start)
graph_goal_c = closest_point(graph, graph_goal)
# Create Graph
# Run A* to find a path from start to goal
print('Local Start and Goal and closest points: ', graph_start, graph_start_c, graph_goal, graph_goal_c)
path, _ = a_star(graph, heuristic, graph_start_c, graph_goal_c)
# Convert path to waypoints
waypoints = [
[
graph_start[0] + north_offset,
graph_start[1] + east_offset,
TARGET_ALTITUDE,
0
]
]
waypoints = waypoints + [[
int(np.ceil(p[0] + north_offset)),
int(np.ceil(p[1] + east_offset)),
TARGET_ALTITUDE,
0] for p in path]
waypoints = waypoints + [
[
int(np.ceil(graph_goal[0] + north_offset)),
int(np.ceil(graph_goal[1] + east_offset)),
TARGET_ALTITUDE,
0
]
]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
# read the first line and extract only the latitude and longitude vals
with open('colliders.csv', 'r') as f:
temp = f.readline()
lat, lon = temp.replace('lat0 ', '').replace('lon0', '').split(', ')
lat, lon = float(lat), float(lon)
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon, lat, 0)
# TODO: retrieve current global position
curr_global_pos = self.global_position
# TODO: convert to current local position using global_to_local()
curr_local_pos = global_to_local(curr_global_pos, self.global_home)
print("Current Local position {}".format(curr_local_pos))
# start_local = int(curr_local_pos[0]), int(curr_local_pos[1]) #int(start_local[0]), int(start_local[1])
# print(start_local, end_local)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# # Define starting point on the grid (this is just grid center)
# grid_start = start_local
# # TODO: convert start position to current position rather than map center
grid_start = (int(curr_local_pos[0]-north_offset), int(curr_local_pos[1]-east_offset))
# # Set goal as some arbitrary position on the grid
# grid_goal = end_local
# TODO: adapt to set goal as latitude / longitude position and convert
end_geo = self.goal
end_local = global_to_local(end_geo, self.global_home)
grid_goal = end_local[0]-north_offset, end_local[1]-east_offset
grid_goal = int(grid_goal[0]), int(grid_goal[1])
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Grid Start and Goal: ', grid_start, grid_goal)
# TODO (if you're feeling ambitious): Try a different approach altogether!
if self.planner == 1:
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
path = prune_path(path)
else:
if self.planner == 2:
sampler = Sampler(data)
polygons = sampler._polygons
# Example: sampling 100 points and removing
# ones conflicting with obstacles.
nodes = sampler.sample(300)
print(nodes[0])
elif self.planner == 3:
pass
elif self.planner == 4:
pass
#create the graph and calculate the a_star
t0 = time.time()
print('building graph ... ', )
g = create_graph(nodes, 10, polygons)
print('graph took {0} seconds to build'.format(time.time()-t0))
start = closest_point(g, (grid_start[0], grid_start[1], TARGET_ALTITUDE))
goal = closest_point(g, (grid_goal[0], grid_goal[1], TARGET_ALTITUDE))
print(start, goal)
# print(start, start_ne)
print('finding path ... ', )
path, cost = a_star_graph(g, heuristic, start, goal)
print('done. path size and cost: {}'.format((len(path), cost)))
# print(len(path), path)
# path_pairs = zip(path[:-1], path[1:])
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]
print(waypoints)
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
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()
