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 grid_plan(self, data,local_start, local_goal, TARGET_ALTITUDE, SAFETY_DISTANCE):
print('Simple Grid-based Planning selected')
# 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))
# convert start position to current position rather than map center
grid_start = ( int(local_start[0]) - north_offset, int(local_start[1]) - east_offset)
grid_goal = ( int(local_goal[0]) - north_offset, int(local_goal[1]) - east_offset )
# make sure goal point is not inside an obstacle or safety margin
if (grid[grid_goal[0]][grid_goal[1]] == 1):
print("Invalid goal point: Aborting")
self.landing_transition()
return
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# Prune path to minimize number of waypoints
if (args.prune == 'collinearity'):
print('Pruning path with collinearity check')
path = collinearity_prune(path)
elif (args.prune == 'bresenham'):
print('Pruning path with bresenham')
path = bresenham_prune(path,grid)
else:
print('Unrecognized prune option returning full path')
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]
return waypoints
def plan_medial_axis(self, data,local_start, local_goal, TARGET_ALTITUDE, SAFETY_DISTANCE):
print('Medial-Axis planning')
# create grid and skeleton
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
skeleton = medial_axis(invert(grid))
print('Skeleton generated')
# calculate the closest start/goal points on the "graph"
start_ne = ( local_start[0] - north_offset, local_start[1] - east_offset)
goal_ne = ( local_goal[0] - north_offset, local_goal[1] - east_offset)
skel_start, skel_goal = find_start_goal(skeleton, start_ne, goal_ne)
# run A* to search for the goal along the road network
path, cost = a_star(invert(skeleton).astype(np.int), heuristic, tuple(skel_start), tuple(skel_goal))
# Prune path to minimize number of waypoints
if (args.prune == 'collinearity'):
print('Pruning path with collinearity check')
path = collinearity_prune(path)
elif (args.prune == 'bresenham'):
print('Pruning path with bresenham')
path = bresenham_prune(path,grid)
else:
print('Unrecognized prune option returning full path')
# Convert path to waypoints
waypoints = [[int(p[0]) + north_offset,int(p[1]) + east_offset, TARGET_ALTITUDE, 0] for p in path]
return waypoints
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 8
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
with open('colliders.csv') as f:
home_pos_data = f.readline().split(",")
lat0 = float(home_pos_data[0].strip().split(" ")[1])
lon0 = float(home_pos_data[1].strip().split(" ")[1])
print(lat0, lon0)
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
global_pos = [self._longitude, self._latitude, self._altitude]
# TODO: convert to current local position using global_to_local()
local_pos = global_to_local(global_pos, self.global_home)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
grid_start = (-north_offset, -east_offset)
# TODO: convert start position to current position rather than map center
grid_start = (int(local_pos[0]-north_offset), int(local_pos[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_pos = global_to_local([-122.400886, 37.795174, self._altitude], self.global_home)
grid_goal = (int(goal_pos[0]-north_offset), int(goal_pos[1]-east_offset))
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
path = prune_path(path)
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self, start, goal):
if not self._graph:
h = max(self._plib.get_height_at_point(start),
self._plib.get_height_at_point(goal))
self.create_voronoi(h)
start = (start[0], start[1])
goal = (goal[0], goal[1])
# Find the nearest node on the graph to the start position & add it
# to the graph:
near_start = self._find_nearest_point(start)
print('Adding starting edge to graph: {} <-> {}'.format(
start, near_start))
self._graph.add_edge(
start,
near_start,
weight=LA.norm(np.array(start) - np.array(near_start)))
# Do the same for the goal position:
near_goal = self._find_nearest_point(goal)
print('Adding goal edge to graph: {} <-> {}'.format(near_goal, goal))
self._graph.add_edge(
near_goal,
goal,
weight=LA.norm(np.array(near_goal) - np.array(goal)))
path, _ = a_star(lambda node: graph_get_children(self._graph, node),
heuristic, start, goal)
print(len(path), path)
return path
def main():
print('main')
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
grid, north_offset, east_offset = planning_utils.create_grid(data, 5, 5)
print('data: {}'.format(grid.shape))
#grid_start = (-north_offset, -east_offset)
# grid_goal = (-north_offset + 50, -east_offset + 50)
grid_start = planning_utils.random_free_location_in_grid(grid)
grid_goal = planning_utils.random_free_location_in_grid(grid)
print('grid_start: {} grid_goal: {}'.format(grid_start, grid_goal))
path, path_cost = planning_utils.a_star(grid, planning_utils.heuristic,
grid_start, grid_goal)
print('path: {} path_cost: {}'.format(len(path), path_cost))
# path = planning_utils.prune_path(path)
# print("pruned_path: {}".format(len(path)))
smoothed_path = planning_utils.smooth_path(grid, path)
print("smoothed_path: {}".format(len(smoothed_path)))
greedy_path = planning_utils.greedy_smooth_path(grid, smoothed_path)
print('greedy_path: {}'.format(len(greedy_path)))
if True:
plt.imshow(grid, cmap='Greys', origin='lower')
for i in range(len(path) - 1):
p1 = path[i]
p2 = path[i + 1]
plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'b-')
for i in range(len(smoothed_path) - 1):
p1 = smoothed_path[i]
p2 = smoothed_path[i + 1]
plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'g-')
for i in range(len(greedy_path) - 1):
p1 = greedy_path[i]
p2 = greedy_path[i + 1]
plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'r-')
# plot in x and y
plt.plot(grid_start[1], grid_start[0], 'rx')
plt.plot(grid_goal[1], grid_goal[0], 'gx')
# plt.plot([grid_start[1], grid_goal[1]], [grid_start[0], grid_goal[0]], 'g-')
# zoom up to the area
# plt.xlim((grid_start[1] - 10, grid_goal[1] + 10))
# plt.ylim((grid_start[0] - 10, grid_goal[0] + 10))
plt.xlabel('EAST')
plt.ylabel('NORTH')
#plt.show()
plt.rcParams['figure.figsize'] = 12, 12
plt.savefig('misc/path2.png')
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 10
SAFETY_DISTANCE = 5
GOAL_LAT = 37.797638
GOAL_LON = -122.394808
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
data = np.loadtxt('colliders.csv', delimiter=',', dtype='U16', usecols = (0, 1))
lat0 = np.float64(data[0, 0].split(' ')[1])
lon0 = np.float64(data[0, 1].split(' ')[2])
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
global_pos = self.global_position
# TODO: convert to current local position using global_to_local()
local_pos = global_to_local(global_pos, self.global_home)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows = 2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
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(-north_offset + local_pos[0]), int(-east_offset + local_pos[1]))
# Set goal as some arbitrary position on the grid
# TODO: adapt to set goal as latitude / longitude position and convert
local_goal = global_to_local([GOAL_LON, GOAL_LAT, 0], self.global_home)
grid_goal = (int(-north_offset + local_goal[0]), int(-east_offset + local_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('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
pruned_path = copy.deepcopy(path)
for i in range(1, len(path) - 1):
p1 = path[i-1]
p2 = path[i]
p3 = path[i+1]
det = p1[0]*(p2[1] - p3[1]) + p2[0]*(p3[1] - p1[1]) + p3[0]*(p1[1] - p2[1])
if det == 0:
pruned_path.remove(p2)
path = pruned_path
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 7
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
fileptr = open('colliders.csv')
latitudeLongitudeArr = fileptr.readline().rstrip().replace('lat0','').replace('lon0','').split(',')
lat0 = float(latitudeLongitudeArr[0])
lon0 = float(latitudeLongitudeArr[1])
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
global_position = self.global_position
# TODO: convert to current local position using global_to_local()
north, east, down = global_to_local(global_position,self.global_home)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
grid_start = (int(np.ceil(north - north_offset)), int(np.ceil(east - east_offset)))
# TODO: convert start position to current position rather than map center
if len(self.target_global_position) == 0:
lon0t = (lon0 + self.targetlongitudeDelta)
lat0t = (lat0 + self.targetlatitudeDelta)
print('Target Longitude and latitude: ', lon0t, lat0t)
northg, eastg, downg = global_to_local([lon0t,lat0t,self.global_home[2]],self.global_home)
else:
self.target_global_position[2] = self.global_home[2]
northg, eastg, downg = global_to_local(self.target_global_position,self.global_home)
# Set goal as some arbitrary position on the grid
grid_goal = (int(np.ceil(northg - north_offset)),int(np.ceil(eastg - east_offset)))
# 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: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
path = collinearity_prune(path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# Read lat0, lon0 from colliders into floating point values
lat0, lon0 = read_home('colliders.csv')
# Set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# Convert to current local position using global_to_local()
current_position = [self._longitude, self._latitude, self._altitude]
self._north, self._east, self._down = global_to_local(
current_position, self.global_home)
print('global home: {0}, global pos: {1}, local pos: {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
grid_start = (int(np.ceil(-north_offset + self.local_position[0])),
int(np.ceil(-east_offset + self.local_position[1])))
# Set goal as some arbitrary position on the grid
local_target_position = global_to_local(self.global_target_position,
self.global_home)
grid_goal = (int(np.ceil(-north_offset + local_target_position[0])),
int(np.ceil(-east_offset + local_target_position[1])))
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# Prune path to minimize number of waypoints
pruned_path = prune_path(path)
print('Length of pruned path: {}'.format(len(pruned_path)))
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# Send waypoints to sim (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
lat_lon_data = np.loadtxt('colliders.csv', usecols=(0, 1), delimiter=',', dtype='str')
# read lat0, lon0 from colliders into floating point values
lat0 = float(lat_lon_data[0,0][5:])
lon0 = float(lat_lon_data[0,1][5:])
# set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# retrieve current global position
current_global_position = self.global_position
# convert to current local position using global_to_local()
current_local_position = global_to_local(current_global_position, self.global_home)
print("current local position: ", current_local_position[0], " ", current_local_position[1])
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# convert start position to current position rather than map center
grid_start = (int(current_local_position[0])-north_offset, int(current_local_position[1])-east_offset)
# set goal as latitude / longitude position and convert
lon_goal = -122.3961
lat_goal = 37.7940
goal_global = np.array([lon_goal, lat_goal, 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)
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# prune path to minimize number of waypoints
pruned_path = prune_path(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
colliders_file = 'colliders.csv'
# TODO: read lat0, lon0 from colliders into floating point values
lat0, lon0 = read_home(colliders_file)
print(f'Home lat : {lat0}, lon : {lon0}')
# # # TODO: set home position to (lat0, lon0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
local_north, local_east, local_down = global_to_local(self.global_position, self.global_home)
print(f'Local => north : {local_north}, east : {local_east}, down : {local_down}')
# 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_file, delimiter=',', dtype='Float64', skiprows=3)
# 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 = int(np.ceil(local_north - north_offset))
grid_start_east = int(np.ceil(local_east - east_offset))
grid_start = (grid_start_north, grid_start_east)
# TODO: convert start position to current position rather than map center
# Set goal as some arbitrary position on the grid
goal_north, goal_east, goal_alt = global_to_local(self.goal_global_position, self.global_home)
grid_goal = (int(np.ceil(goal_north - north_offset)), int(np.ceil(goal_east - east_offset)))
# 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: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
path = collinearity_prune(path)
# 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 path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 3
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
# TODO: set home position to (lat0, lon0, 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=3)
# Determine offsets between grid and map
north_offset = int(np.abs(np.min(data[:, 0])))
east_offset = int(np.abs(np.min(data[:, 1])))
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define a grid for a particular altitude and safety margin around obstacles
grid = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
# 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(current_local_pos[0]+north_offset), int(current_local_pos[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
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [(p[0] - north_offset, p[1] - east_offset,
TARGET_ALTITUDE + 1) for p in 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
# TODO: read lat0, lon0 from colliders into floating point values
file = open('colliders.csv', 'r')
reader = csv.reader(file)
header = next(reader)
lat0, lon0 = [float(header[i].split()[1]) for i, j in enumerate(header)]
# TODO: set home position to (lon0, lat0, 0)
global_home = (lon0, lat0, 0)
print("global home is ", global_home)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
global_position = (self._longitude, self._latitude, self._altitude)
# print(global_position)
# TODO: convert to current local position using global_to_local()
local_position = global_to_local(global_position, global_home)
local_north, local_east, local_down = local_position
print(local_position)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
# TODO: convert start position to current position rather than map center
grid_start = (-north_offset + int(local_north), -east_offset + int(local_east))
# Set goal as some arbitrary position on the grid
# TODO: adapt to set goal as latitude / longitude position and convert
goal_north, goal_east, goal_alt = global_to_local([-122.400150, 37.796005, TARGET_ALTITUDE], self.global_home)
grid_goal = (int(np.ceil((-north_offset + goal_north))), int(np.ceil(-east_offset + goal_east)))
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
pruned_path = prune_path(path)
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
print("Searching for a path ...")
self.flight_state = States.PLANNING
self.target_position[2] = self.TARGET_ALTITUDE
# Read lat0, lon0 from colliders into floating point values, and set home position
with open('colliders.csv', 'r') as f:
latlon_string = f.readline()
f.close()
m = re.search('lat0 (?P<lat>-*\d+.\d+), lon0 (?P<lon>-*\d+.\d+)\n',
latlon_string)
lat0, lon0 = [m.group('lat'), m.group('lon')]
self.set_home_position(lon0, lat0, 0)
# Convert current posiiton to local frame
local_position = global_to_local(self.global_position,
self.global_home)
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data,
self.TARGET_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)
grid_start = local_to_grid(local_position, north_offset, east_offset)
# Set goal as some arbitrary position on the grid
local_goal = global_to_local(self.GOAL_LATLON, self.global_home)
grid_goal = local_to_grid(local_goal, north_offset, 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)
pruned_path = prune(path)
# Convert path to waypoints
print("Path computed, publishing waypoints...")
waypoints = [[
p[0] + north_offset, p[1] + east_offset, self.TARGET_ALTITUDE, 0
] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
self.send_waypoints()
def button_callback(event):
global pp
path, cost = a_star(grid=grid, start=home_grid, goal=goal_grid)
pp = np.array(path)
# ax.imshow(grid, cmap='Greys')
ax.plot(pp[:, 1], pp[:, 0], linewidth=3)
pruned_path = np.array(prune_path(path))
ax.plot(pruned_path[:, 1], pruned_path[:, 0], 'g')
ax.scatter(pruned_path[:, 1], pruned_path[:, 0])
plt.draw()
def get_medial_axis_path(grid, grid_goal, grid_start):
skeleton = medial_axis(invert(grid))
skel_start, skel_goal = find_start_goal(skeleton, np.array(grid_start),
np.array(grid_goal))
# path, _ = a_star(grid, heuristic, skel_start, skel_goal)
path, _ = a_star(
invert(skeleton).astype(np.int), euclidean_distance, skel_start,
skel_goal)
path = path + [grid_goal]
return path, skeleton
def plan_path(self):
def local_to_grid(local_coord):
"""
Convert a local frame to a grid frame location
"""
return (int(np.ceil(local_coord[0] - north_offset)),
int(np.ceil(local_coord[1] - east_offset)))
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
self.load_home('colliders.csv')
local = global_to_local(self.global_position, self.global_home)
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define starting point on the grid
grid_start = local_to_grid(local)
# Set goal as some arbitrary position on the grid
grid_goal = (grid_start[0] + 10, grid_start[1] - 10
) #(-north_offset + 10, -east_offset + 10)
# 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: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 6
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
#posX,posY,posZ,halfSizeX,halfSizeY,halfSizeZ
# TODO: read lat0, lon0 from colliders into floating point values
lat_lon = pd.read_csv('colliders.csv')
lat = float(lat_lon.columns[0][5:])
lon = float(lat_lon.columns[1][6:])
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position = (lon, lat, 0)
print("home position set to {0}, {1}".format(lon,lat))
# TODO: retrieve current global position
local_north, local_east, local_down = global_to_local(self.global_position, self.global_home)
print("local position set to {0}, {1}, {2}".format(local_north,local_east,local_down))
# TODO: convert to current local position using global_to_local()
local_coordinates_NED = global_to_local( self.global_position, self.global_home)
print('local_coordinates_NED {0}'.format(local_coordinates_NED))
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64',skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
#grid_start = (-north_offset, -east_offset)
# TODO: convert start position to current position rather than map center
grid_start = (int(np.ceil(local_north - north_offset)), int(np.ceil(local_east - east_offset)))
# Set goal as some arbitrary position on the grid
goal = global_to_local(self.goal_position,self.global_home)
grid_goal = ( int(np.ceil(goal[0]-north_offset)), int(np.ceil(goal[1]-east_offset)))
# 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: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
print('path: ',path)
pruned_path = prune_path(path)
print('pruned path: ',pruned_path)
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# read lat0, lon0 from colliders into floating point values
with open('colliders.csv') as f:
first_line = f.readline()
items = first_line.split(',')
lat0 = float(items[0].split()[1])
lon0 = float(items[1].split()[1])
# set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# retrieve current global position
print ("current global position:", self.global_position)
local_pos = global_to_local(self.global_position, self.global_home)
print("current local position", local_pos)
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))
# Set goal: latitude / longitude
goal_lon = -122.398470
goal_lat = 37.793445
local_goal = global_to_local((goal_lon, goal_lat, 0), self.global_home)
grid_start = (int(local_pos[0])-north_offset, int(local_pos[1])-east_offset)
grid_goal = (int(local_goal[0]-north_offset), int(local_goal[1]-east_offset))
# Adapt to set goal as position and convert
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
if grid_start[0] != grid_goal[0] or grid_start[1] != grid_goal[1]:
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# prune path to minimize number of waypoints
pruned_path = self.prune_path(path)
print ("prunned path is :", pruned_path)
if 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
# send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 20
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# Setting the home position based on the latitude and longitude given
# on the first line of colliders.csv
file = 'colliders.csv'
latlon = []
latlon = open(file).readline().split()
lat_h, lon_h = float(latlon[1].replace(',', '')), float(latlon[3])
self.set_home_position(lon_h, lat_h, 0.0)
global_position = self.global_position
# creating a 2D grid from the colliders.csv
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)
# Acquiring start and goal position
grid_start_latlon = (self._longitude, self._latitude, self._altitude)
grid_start_l = global_to_local(grid_start_latlon, self.global_home)
grid_start = (int(grid_start_l[0]) - north_offset,
int(grid_start_l[1]) - east_offset)
goal_lon = input('Goal longitude: ') #goal_lon = -122.396071
goal_lat = input('Goal latitude: ') #goal_lat = 37.793077
goal_position = (float(goal_lon), float(goal_lat), 0.0)
grid_goal_l = global_to_local(goal_position, self.global_home)
grid_goal = (int(grid_goal_l[0]) - north_offset,
int(grid_goal_l[1]) - east_offset)
print('Local Start: ', grid_start)
print('Local Goal: ', grid_goal)
# path planning and pruning using collinearity check and bresenham
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
pruned_path = prune_path(path, grid)
print('Path length: ', len(pruned_path))
print(pruned_path)
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
lat0, lon0 = self.get_global_home_from_file()
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
# TODO: convert to current local position using global_to_local()
local_position = global_to_local(self.global_position, self.global_home)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
# TODO: convert start position to current position rather than map center
grid_start = (int(local_position[0])-north_offset, int(local_position[1])-east_offset)
# Set goal as some arbitrary position on the grid
# grid_goal = (grid_start[0] + 10, grid_start[1] + 10)
# TODO: adapt to set goal as latitude / longitude position and convert
goal_geo = np.array([-122.401763, 37.795809, 0])
local_goal = global_to_local(goal_geo, self.global_home)
grid_goal = (int(local_goal[0])-north_offset, int(local_goal[1])-east_offset)
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
print('Path length: {0}'.format(len(path)))
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
"""
I have implemented both Collinearity and Bresenham methods to prune the path.
You can call either below to test
"""
# pruned_path = prune_coll(path)
pruned_path = prune_bres(path, grid)
print('Pruned-path length: {0}'.format(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 (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 20
SAFETY_DISTANCE = 10
self.target_position[2] = TARGET_ALTITUDE
#initial localtion in world coordinate frame
global_home =(-122.397450,37.792480)
lat0 ,lon0 = global_home[1],global_home[0]
#intialize the drone position to the global home then that is going to referance the local frame from there local_home (0,0,0)
self.set_home_position(lon0,lat0,0)
#self.set_home_as_current_position()
global_loc = self.global_position
local_position = global_to_local(global_loc , 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)
grid_start=(int(local_position[0] - north_offset) ,int(local_position[1] -east_offset) )
# Set goal as some arbitrary position on the grid
grid_goal = (-north_offset + 750, -east_offset + 370)
grid_goal =(int(750) ,int( 370) )
# 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)
cleared_path = prune_path(path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in cleared_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 = 4
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
pos = open("colliders.csv").readline().split(",")
lat0 = float(pos[0].replace("lat0 ",""))
lon0 = float(pos[1].replace("lon0 ",""))
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0.0)
# TODO: retrieve current global position
#print('global position {}'.format(self.global_position))
# TODO: convert to current local position using global_to_local()
local_pos = global_to_local(self.global_position, self.global_home)
print('global home {0}, \n position {1}, \n 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))
# convert start position to current position rather than map center
grid_start = [int(np.ceil(local_pos[0] - north_offset)), int(np.ceil(local_pos[1] - east_offset))]
# Set goal as some arbitrary position on the grid
#grid_goal = (-north_offset + 300, -east_offset + 300)
# adapt to set goal as latitude / longitude position and convert
local_goal_pos = global_to_local(self.global_goal_position, self.global_home)
grid_goal = (int(np.ceil(local_goal_pos[0] - north_offset)), int(np.ceil(local_goal_pos[1] - east_offset)))
print(grid_goal)
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# prune path to minimize number of waypoints
reduced_path = prune_path(path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in reduced_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
lat0, lon0 = open('colliders.csv').readline().replace("lat0", "").replace("lon0", "").strip().split(",")
lat0 = float(lat0)
lon0 = float(lon0)
self.set_home_position(lon0, lat0, 0)
current_local_position = global_to_local(self.global_position, self.global_home)
goal_local_position = global_to_local(self.global_goal, 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 = (int(current_local_position[0]-north_offset), int(current_local_position[1]-east_offset))
# Set goal as some arbitrary position on the grid
grid_goal = (int(goal_local_position[0]-north_offset), int(goal_local_position[1]-east_offset))
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
path = prune_path(path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
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
grid_start = (-north_offset, -east_offset)
# Set goal as some arbitrary position on the grid
grid_goal = (-north_offset + 10, -east_offset + 10)
# 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)
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
# Set self.waypoints
self.waypoints = waypoints
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = args.altitude
SAFETY_DISTANCE = args.safe_dist
self.target_position[2] = TARGET_ALTITUDE
origin = open('colliders.csv')
line = origin.readline().rstrip().split(', ')
origin.close()
lat0 = float(line[0].split()[1])
lon0 = float(line[1].split()[1])
self.set_home_position(lon0, lat0, 0)
local_origin = 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))
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
grid_start = (int(local_origin[0] - north_offset),
int(local_origin[1] - east_offset))
grid_goal = global_to_local((args.longitude, args.latitude, 0),
self.global_home)
grid_goal = (int(grid_goal[0] - north_offset),
int(grid_goal[1] - east_offset))
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
path = prune_path(path)
self.waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
self.send_waypoints()
def proj_min_req(self, data, global_goal, TARGET_ALTITUDE,
SAFETY_DISTANCE):
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
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)
local_goal = global_to_local(global_goal, 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)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
pruned_path = prune_path_bres(path, grid)
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in pruned_path]
for i in range(1, len(waypoints)):
heading = np.arctan2(
waypoints[i][0] - waypoints[i - 1][0],
waypoints[i][1] - waypoints[i - 1][1]) - pi / 2
waypoints[i][3] = -heading
return waypoints
def plan_simple_path(self, target_global, altitude, safety):
print("Departing from:", self.local_position)
# Use grid to determine path from start -> goal
grid_start = (int(self.local_position[0] - self.north_offset),
int(self.local_position[1] - self.east_offset))
# Set the target
self.landing_position = global_to_local(np.array(target_global),
self.global_home)
print("Landing at:", self.landing_position)
grid_goal = (int(self.landing_position[0] - self.north_offset),
int(self.landing_position[1] - self.east_offset))
path, cost = a_star(self.grid._grid, heuristic, grid_start, grid_goal,
-self.local_position[2])
print('cur->goal: ', grid_start, grid_goal, cost)
# prune path
if self.cull: path = prune(path, self.grid, -self.local_position[2])
# and convert to waypoints, climbing if necessary
waypoints = []
last_alt = int(-self.local_position[2]) + safety # current altitude
prevpoint = None
for p in path:
climb = (self.grid.get_altitude(p[0], p[1]) + safety) - last_alt
if climb < 0: climb = 0
altitude = last_alt + climb
last_alt = altitude
if prevpoint is not None:
heading = self.compute_heading(prevpoint, (p[0], p[1]))
else:
heading = 0.0
prevpoint = (p[0], p[1])
waypoints.append([
p[0] + self.north_offset, p[1] + self.east_offset,
int(altitude), heading
])
return waypoints
def plan_path_grid(self, TARGET_ALTITUDE, SAFETY_DISTANCE):
self.read_in()
# convert to current local position using global_to_local()
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))
# Make grid given a particular altitude and margin around obstacles
grid, north_offset, east_offset = create_grid(self.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)
# convert start position to current position rather than map center
grid_start = (int(local_pos[0]) + grid_start[0],
int(local_pos[1]) + grid_start[1])
# Set goal as lat lon and convert to local coordinates (from bottom left)
grid_goal_global = (-122.397450, 37.79380, 0)
grid_goal = global_to_local(grid_goal_global, self.global_home)
grid_goal = (int(grid_goal[0]) - north_offset,
int(grid_goal[1]) - east_offset)
# Run A* and prune path
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
path = prune_path_grid(path, grid)
# Convert path to waypoints and send to simulator
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
self.waypoints = waypoints
def media_axis_method(grid, grid_start, grid_goal, check_linear=True):
#check_linear specify how we do path pruning, if True, we choose colinearity
#if False, we use bresenham, usually bresenham yield a much better result
skeleton = medial_axis(invert(grid))
skel_start, skel_goal = find_start_goal(skeleton, grid_start, grid_goal)
path, _ = a_star(
invert(skeleton).astype(np.int), heuristic, tuple(skel_start),
tuple(skel_goal))
if len(path) == 0:
# print("warning, no path is found, please select another point")
return path
#insert the start and end point if necessary
if tuple(skel_start) != grid_start:
path.insert(0, grid_start)
if tuple(skel_goal) != grid_goal:
path.append(grid_goal)
#prune the path
print("path point num = {}, path={}".format(len(path), path))
path = prune_path(path, grid=grid, check_linear=check_linear)
print("pruned path point num = {}, path={}".format(len(path), path))
path = np.array(path).astype(int).tolist()
return path, skeleton, grid_start, grid_goal
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()
