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 In lat0, lon0 From CSV Into Floating Point Values -- First Line
with open('colliders.csv') as f:
first_line = f.readline()
lat_lon=[]
for col in [x.strip() for x in first_line.split(',')]:
lat_lon.append(float(col.split(' ')[1]))
# Set Home Position To (lon0, lat0, 0)
# Debugging: Print Home Position
self.set_home_position(lat_lon[1], lat_lon[0], 0)
print("Home Position - Longitude, Latitude, Altitude: ", self.global_home)
# Retrieve Current Global Position
# Debugging: Print Global Position
global_pos = self.global_position
print("Global Position- Longitude, Latitude, Altitude: ", global_pos)
# Convert To Current Local Position Using global_to_local()
# Debugging: Print Local Position
local_pos = global_to_local(global_pos, self.global_home)
# Print Global Home, Global Position, Local Position
print('Global Home: {0} \nGlobal Position: {1} \nLocal 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 Grid For A Particular Altitude And Safety Distance 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 Grid (Grid Center)
grid_start = (-north_offset, -east_offset)
# Tmp Goal
goal_lat = 37.795049
goal_lon = -122.396362
# 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 Grid
grid_goal = (-north_offset+50, -east_offset+50)
# Adapt To Set Goal As Lat/Lon Position And Convert
global_goal = [-122.396362, 37.795049, self._altitude]
local_goal = global_to_local(global_goal, 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
print('Local Start And Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# Prune Path
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
# Send Waypoints To Sim (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', 'r') as f:
first_line = f.readline().strip('\n')
fl_list = first_line.split()
lat0 = float(fl_list[1].strip(','))
lon0 = float(fl_list[3])
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
self.global_position[0] = self._longitude
self.global_position[1] = self._latitude
self.global_position[2] = self._altitude
# TODO: convert to current local position using global_to_local()
local_position = global_to_local(self.global_position,
self.global_home)
self._north = local_position[0]
self._east = local_position[1]
self._down = 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)
# TODO: convert start position to current position rather than map center
start_north = int(self._north) - north_offset
start_east = int(self._east) - east_offset
grid_start = (start_north, start_east)
# Set goal as some arbitrary position on the grid, last used for diag tests
# grid_goal = (start_north - 5, start_east + 5)
# TODO: adapt to set goal as latitude / longitude position and convert
# updated to include parsed args (for srource see line 2)
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)))
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation - done
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
print('local._north and local._east: ', self._north, self._east)
ori_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!
# added prune_path to planning_utlis
pruned_path = prune_path(ori_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 = 10
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
# The first line of colliders.csv provides the latitude and longitude that we will call home
f = open('colliders.csv')
line_list = f.readline().rstrip().split(', ')
line_list_lat = line_list[0].split()
line_list_lon = line_list[1].split()
lat0 = float(line_list_lat[1])
lon0 = float(line_list_lon[1])
# TODO: set home position to (lon0, lat0, 0)
# After extracting those latitude and longitude values from the file, we actually set it as the home.
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
# Let's gather where (latitude and longitude) we are on the map (e.g. using GPS)
# current_position_global = self.global_position
current_position_global = [
self._longitude, self._latitude, self._altitude
]
# TODO: convert to current local position using global_to_local()
# Now, let's determine how far (north and east) we are from the home position
current_position_local = global_to_local(current_position_global,
self.global_home)
# current_position_local = self.local_position
current_position_local_north = int(np.round(current_position_local[0]))
current_position_local_east = int(np.round(current_position_local[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))
# 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
# Subtract offets from current local position, rather than the local home position
grid_start = (current_position_local_north - north_offset,
current_position_local_east - 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
# grid_goal = (current_position_local_north + grid_goal[0], current_position_local_east + grid_goal[1])
# Define a goal latitude and longitude, convert to the local frame, and then adjust by the offsets
goal_position_global = (-122.396855, 37.794185, 0)
goal_position_local = global_to_local(goal_position_global,
self.global_home)
goal_position_local_north = int(np.round(goal_position_local[0]))
goal_position_local_east = int(np.round(goal_position_local[1]))
grid_goal = (goal_position_local_north - north_offset,
goal_position_local_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)
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!
# We send the path generated by A* to the path pruner.
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
# TODO: read lat0, lon0 from colliders into floating point values
with open('colliders.csv', newline='') as f:
reader = csv.reader(f)
row1 = next(reader)
lat = re.findall("\d+\.\d+", row1[0])
lon = re.findall("[-+]\d+\.\d+", row1[1])
lat0 = [float(s) for s in lat]
lon0 = [float(s) for s in lon]
# TODO: set home position to (lon0, lat0, 0)
global_home = [lon0, lat0, 0]
# TODO: retrieve current global position
# TODO: convert to current local position using global_to_local()
current_local_position = global_to_local(self.global_position,
self.global_home)
# print(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_start1 = (int(current_local_position[0] - north_offset),
int(current_local_position[1] - east_offset))
# printf("{}".format(self.grid_start))
# geodetic_current_coordinates = local_to_global(grid_goal, geodetic_home_coordinates)
# Set goal as some arbitrary position on the grid
grid_goal = (-north_offset + 50, -east_offset + 50)
# TODO: adapt to set goal as latitude / longitude position and convert
geodetic_goal = (-122.39632967, 37.79755765, 0.)
# getting error
# geodetic_current_coordinates = local_to_global(self.grid_goal,self.global_home )
# print(geodetic_current_coordinates)
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# DONE IN PLANNING_UTILS.PY
# 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
p1 = prune_path(path)
print(len(p1))
print(p1)
# 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 p1]
# 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):
"""
path_planing function
"""
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:
origin_pos_data = f.readline().split(',')
lat0 = float(origin_pos_data[0].strip().split(' ')[1])
lon0 = float(origin_pos_data[1].strip().split(' ')[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
print(global_position)
print(lon0, lat0)
# TODO: convert to current local position using global_to_local()
local_position = global_to_local(global_position, (lon0, lat0, 0))
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
self.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(self.data,
TARGET_ALTITUDE,
SAFETY_DISTANCE)
print(grid.shape)
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_position[0]) - north_offset,
int(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
#global_goal = (-122.397850, 37.792580, 0.0) # (37.792480, -122.397450)
global_goal = self.global_goal
local_goal = global_to_local(global_goal, self.global_home)
print(local_goal)
grid_goal = (int(local_position[0] - north_offset + local_goal[0]),
int(local_position[1] - east_offset + local_goal[1]))
#grid_goal = (290, 445)
# 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):
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 = read_home('colliders.csv')
# 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_north, local_east, _ = global_to_local(self.global_position,
self.global_home)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset,
east_offset))
# Define starting point on the grid (this is just grid center)
grid_start = (-north_offset, -east_offset)
# TODO: convert start position to current position rather than map center
grid_start = (int(np.ceil(grid_start[0] + local_north)),
int(np.ceil(grid_start[1] + local_east)))
# 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
global_goal_position = np.array([-122.395148, 37.794142, 5])
local_goal_north, local_goal_east, _ = global_to_local(global_goal_position,
self.global_home)
print("Local Grid Goal : " + str(local_goal_north) + "-" + str(local_goal_east))
grid_goal = (int(np.ceil(local_goal_north + grid_goal[0])),
int(np.ceil(local_goal_east + grid_goal[1])))
print("Grid Goal : " + str(grid_goal[0]) + "-" + str(grid_goal[1]))
plt.imshow(grid, origin='lower')
plt.plot(grid_start[0], grid_start[1], 'rx')
plt.plot(grid_goal[0], grid_goal[1], 'bx')
#plt.plot(grid_goal[0], grid_goal[1], 'bx')
plt.show()
# 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):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 6
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
with open('colliders.csv') as f:
s = f.readline()
s = s.replace(',', ' ')
data = s.split()
lat0 = float(data[1])
lon0 = float(data[3])
print(lat0, lon0)
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
self._global_position = [self._latitude, self._longitude, self._altitude]
# TODO: convert to current local position using global_to_local()
self._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, 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(self.local_position[0])-north_offset, int(self.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
#LAT=37.797337 lON = -122.401122
goal_gps = [-122.401122, 37.797337, 0]
#goal_gps = [-122.397629, 37.793520, 0]
goal_local = global_to_local(goal_gps, self.global_home)
grid_goal = (int(goal_local[0])-north_offset, int(goal_local[1])-east_offset)
"""
#Show goal as location seems weird
plt.imshow(grid, cmap='Greys', origin='lower')
plt.plot(grid_start[1], grid_start[0], 'x')
plt.plot(grid_goal[1], grid_goal[0], 'x')
plt.xlabel('EAST')
plt.ylabel('NORTH')
plt.show()
"""
# 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)
print('Start A*')
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
print('Pruning')
pruned_path = prune_path(path)
plt.imshow(grid, cmap='Greys', origin='lower')
# For the purposes of the visual the east coordinate lay along
# the x-axis and the north coordinates long the y-axis.
plt.plot(grid_start[1], grid_start[0], 'x')
plt.plot(grid_goal[1], grid_goal[0], 'x')
if path is not None:
pp = np.array(path)
plt.plot(pp[:, 1], pp[:, 0], 'g')
plt.xlabel('EAST')
plt.ylabel('NORTH')
plt.show()
# 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]
for i in range(1, len(waypoints)):
print(i)
wp1 = waypoints[i-1]
wp2 = waypoints[i]
# Set heading of wp2 based on relative position to wp1
waypoints[i][3] = np.arctan2((wp2[1]-wp1[1]), (wp2[0]-wp1[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
lat0 = 0.0
lon0 = 0.0
with open('colliders.csv', 'r') as f:
lines = f.readlines()
head = lines[0]
half_h = head.split(',')
lat0 = float(half_h[0].strip().split(' ')[1])
lon0 = float(half_h[1].strip().split(' ')[1])
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
my_global_position = self.global_position
# TODO: convert to current local position using global_to_local()
my_local_position = global_to_local(my_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)
# TODO: convert start position to current position rather than map center
grid_start = (int(my_local_position[0] - north_offset),
int(my_local_position[1] - east_offset))
# Set goal as some arbitrary position on the grid
# TODO: adapt to set goal as latitude / longitude position and convert
global_goal = (-122.397450 - 0.0017, 37.792480 + 0.0038, 0)
local_goal = global_to_local(global_goal, 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)
# TODO: prune path to minimize number of waypoints
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
print('Initial path ', len(path), ' points')
path = prune_path(path)
print('After pruning path: ', len(path), ' points')
l = len(path)
dl = 10
while dl != 0:
path = pick_uncrossed(grid, path)
dl = l - len(path)
l = len(path)
print('After removing uncrossed points: ', len(path), ' points')
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = []
for p in path:
waypoints.append(
[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0])
if len(waypoints) > 1:
waypoints[-1][3] = np.arctan2(
(waypoints[-1][1] - waypoints[-2][1]),
(waypoints[-1][0] - waypoints[-2][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
with open('colliders.csv', newline='') as f:
reader = csv.reader(f)
for row in reader:
lhs, rhs = row[0], row[1]
break
_, lat = lhs.split()
_, lon = rhs.split()
lat0, lon0 = float(lat), float(lon)
# 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()
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 = 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 curren0t position rather than map center
grid_start = (abs(int(local_position[1])) - north_offset,
abs(int(local_position[0])) - east_offset)
# Set goal as some arbitrary position on the grid
# grid_goal = (-north_offset + 10, -east_offset + 10)
#grid_goal = (200, 200)
# TODO: adapt to set goal as latitude / longitude position and convert
goal_y, goal_x = lat_lon_goal(37.796391, -122.398300, self.global_home)
grid_goal = (int(goal_y - north_offset), int(goal_x - east_offset))
#Need to cehck if start and goal are in free space
skeleton = medial_axis(invert(grid))
skel_start, skel_goal = find_start_goal(skeleton, grid_start,
grid_goal)
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
grid_start = int(skel_start[0]), int(skel_start[1])
grid_goal = int(skel_goal[0]), int(skel_goal[1])
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)
print(len(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 path]
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
# -------------------------------------
# YW [1]: read lat0, lon0 from colliders into floating point values
# -------------------------------------
with open('colliders.csv') as f:
first_line = f.readline()
geo_lat_lon = []
for col in [x.strip() for x in first_line.split(',')]:
geo_lat_lon.append(float(col.split(' ')[1]))
print("Global home coordinate in Lat-Long: ", geo_lat_lon)
# YW: set home position to (lon0, lat0, 0). Beware of the lat0, lon0 in collider.csv!
self.set_home_position(geo_lat_lon[1], geo_lat_lon[0], 0)
# -------------------------------------
# YW [2]: retrieve current global position
# -------------------------------------
global_pose = self.global_position
print("Current global pose (lon, lat, alt): ", global_pose)
print("Global home coord (lon, lat, alt): ", self.global_home)
print("\n")
# YW: convert to current local position using global_to_local()
cur_pose_local = global_to_local(global_pose, self.global_home)
print("------------------------")
print("local pose: ", cur_pose_local)
print('global home {0} \nglobal position {1} \nlocal position {2}'.
format(self.global_home, self.global_position,
self.local_position))
print("------------------------\n")
# -------------------------------------
# YW [3]: Read in obstacle map
# -------------------------------------
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
int_cur_pose_local = cur_pose_local.astype(int)
print("local int_cur_pose_local: ", int_cur_pose_local)
global_goal = [-122.396580, 37.796085, self._altitude]
print("global_goal (long-lat-alt): ", global_goal)
local_goal = global_to_local(global_goal, self.global_home)
int_local_goal = local_goal.astype(int)
print("local_goal : ", int_local_goal)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_min, east_min = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North min = {0}, east min = {1}".format(north_min, east_min))
# plt.imshow(grid, origin='lower')
# plt.xlabel('EAST')
# plt.ylabel('NORTH')
# plt.show()
grid_start = (int_cur_pose_local[0] - north_min,
int_cur_pose_local[1] - east_min)
# YW: convert start position to current position rather than map center
# Don't add tuple and numpy array
print("grid_start: ", grid_start)
# Set goal as some arbitrary position on the grid
# -------------------------------------
# YW: adapt to set goal as latitude / longitude position and convert
# use the manual mode to get the appropriate goal coordinate
# -------------------------------------
# convert the local NORTH EAST direction to the grid map
grid_goal = (int_local_goal[0] - north_min,
int_local_goal[1] - east_min)
#grid_goal = (-north_min + 20, - east_min )
print("grid_goal: ", grid_goal)
print("Grid size: ", grid.shape)
# Run A* to find a path from start to goal
# YW: 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) -> planning_utils
print('Local Start and Goal: ', grid_start, " -> ", grid_goal)
print("------------------------\n")
if self.mode == 'grid':
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
path = prune_path(path)
print("Path: ", path)
# Convert path to waypoints
waypoints = [[
p[0] + north_min, p[1] + east_min, TARGET_ALTITUDE, 0
] for p in path]
print("Waypoints: ", waypoints)
elif self.mode == "graph":
# Use KdSampler instead of create grid
graph_start = tuple(int_cur_pose_local.tolist())
graph_goal = tuple(int_local_goal.tolist())
print("Start: {0}, goal: {1}".format(graph_start, graph_goal))
kdtree_sampler = prmap.KdSampler(data, 140, graph_start,
graph_goal, SAFETY_DISTANCE)
print("Number of samples: ", len(kdtree_sampler._kept_samples))
print("Number of rejected samples: ",
len(kdtree_sampler._removed_samples))
t0 = time.time()
# YW NOTE: given a large amount of k allows the algorithm to connect
# the nodes that has a long distance
g = prmap.create_graph(kdtree_sampler, 10)
print('graph took {0} seconds to build'.format(time.time() - t0))
path, cost = prmap.a_star_graph(g, heuristic, graph_start,
graph_goal)
print("A* graph length {0}, path: {1} \ncost: {2}".format(
len(path), path, cost))
prmap.visualize_graph(grid, data, g, kdtree_sampler, path)
# Convert path to waypoints
waypoints = [[p[0], p[1], TARGET_ALTITUDE, 0] for p in path]
else:
print("Error: Unknown planning mode!")
print("Waypoints: ", waypoints)
# Set self.waypoints
self.waypoints = waypoints
# send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 10
SAFETY_DISTANCE = 2
self.target_position[2] = TARGET_ALTITUDE
# read lat0, lon0 from colliders into floating point values
first_line = open('colliders.csv').readline()
lat_lon = dict(val.strip().split(' ') for val in first_line.split(','))
# set home position to (lon0, lat0, 0)
self.set_home_position(float(lat_lon['lon0']), float(lat_lon['lat0']),
0)
# retrieve current global position
# convert to current local position using global_to_local()
local_position = global_to_local(self.global_position,
self.global_home)
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
# Create Voronoi graph from obstacles
grid, edges, north_offset, east_offset = create_grid_and_edges(
data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define starting point on the grid
# convert start position to current position rather than map center
grid_start = (-north_offset + int(local_position[0]),
-east_offset + int(local_position[1]))
# Set goal as some arbitrary position on the grid
# adapt to set goal as latitude / longitude position and convert
# some possible destinations on the map
destinations = [[-122.39324423, 37.79607305],
[-122.39489652, 37.79124152],
[-122.40059743, 37.79272177],
[-122.39625334, 37.79478161]]
# randomly choose one of destinations
destination = destinations[np.random.randint(0, len(destinations))]
destination_local = global_to_local(
[destination[0], destination[1], TARGET_ALTITUDE],
self.global_home)
# adjust destination coordinates on the grid
grid_goal = (-north_offset + int(destination_local[0]),
-east_offset + int(destination_local[1]))
print('Local Start and Goal: ', grid_start, grid_goal)
# create Voronoi graph with the weight of the edges
# set to the Euclidean distance between the points
voronoi_graph = nx.Graph()
for e in edges:
p1 = e[0]
p2 = e[1]
dist = np.linalg.norm(np.array(p2) - np.array(p1))
voronoi_graph.add_edge(p1, p2, weight=dist)
# find the nearest points on the graph for start and the goal
voronoi_start = nearest_point(grid_start, voronoi_graph)
voronoi_finish = nearest_point(grid_goal, voronoi_graph)
# run A-star on the graph
voronoi_path, voronoi_cost = a_star_graph(voronoi_graph, heuristic,
voronoi_start,
voronoi_finish)
voronoi_path.append(grid_goal)
# prune path - from Lesson 6.5
print('Original path len: ', len(voronoi_path))
voronoi_path = prune_path(voronoi_path)
print('Pruned path len: ', len(voronoi_path))
# Convert path to waypoints
waypoints = [[
int(p[0]) + north_offset,
int(p[1]) + east_offset, TARGET_ALTITUDE, 0
] for p in voronoi_path]
# Set self.waypoints
self.waypoints = waypoints
# send waypoints to sim
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 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:
line = np.array(f.readline().replace(',', ' ').split())
line = line[[1,3]]
lat0, lon0 = np.asarray(line, dtype=np.float64)
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0.0)
# TODO: retrieve current global position
current_global = self.global_position
# TODO: convert to current local position using global_to_local()
current_local = global_to_local(current_global, 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))
print("grid shape", grid.shape)
# 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(current_local[0])-north_offset, int(current_local[1])-east_offset)
print("grid_start", grid_start)
# 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
lat_goal = 37.794766 #37.796077
lon_goal = -122.399456 #-122.398163
goal_global = [lon_goal, lat_goal, 0.]
goal_local = global_to_local(goal_global, self.global_home)
#grid_goal = (goal_local[0], goal_global[1])
grid_goal = (int(goal_local[0])-north_offset, int(goal_local[1])-east_offset)
print("grid_goal", grid_goal)
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', 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 = prune_path(path)
path = bresenham_prune(grid, path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
#NearBy
# TARGET_LAT = 37.792653
# TARGET_LON = -122.397075
#around the corner
TARGET_LAT = 37.792504
TARGET_LON = -122.397980
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# TODO: read lat0, lon0 from colliders into floating point values
print(
"# TODO: read lat0, lon0 from colliders into floating point values"
)
homeGPSLat, homeGPSLon = self.getFirstRowOfCSV()
print("homeGPSLon = " + str(homeGPSLon))
print("homeGPSLat = " + str(homeGPSLat))
# TODO: set home position to (lon0, lat0, 0)
print("# TODO: set home position to (lon0, lat0, 0)")
self.set_home_position(homeGPSLon, homeGPSLat, 0)
print("# home position set")
# TODO: retrieve current global position
print("# TODO: retrieve current global position")
currentGPSLat = self._latitude
currentGPSLon = self._longitude
currentGPSAlt = self._altitude
print("currentGPSLon = " + str(currentGPSLon))
print("currentGPSLat = " + str(currentGPSLat))
print("currentGPSAlt = " + str(currentGPSAlt))
# TODO: convert to current local position using global_to_local()
print(
"# TODO: convert to current local position using global_to_local()"
)
geodetic_current_coordinates = [
currentGPSLon, currentGPSLat, currentGPSAlt
]
geodetic_home_coordinates = [homeGPSLon, homeGPSLat, 0]
print("geodetic_current_coordinates " +
str(geodetic_current_coordinates))
print("geodetic_home_coordinates " + str(geodetic_home_coordinates))
local_coordinates_NED = global_to_local(geodetic_current_coordinates,
geodetic_home_coordinates)
print("local_coordinates_NED " + str(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)
# print("original grid_start " + str(grid_start))
# TODO: convert start position to current position rather than map center
print(
"# TODO: convert start position to current position rather than map center"
)
grid_start = (int(-north_offset + local_coordinates_NED[0]),
int(-east_offset + local_coordinates_NED[1]))
print("Overriding start location to " + str(grid_start))
# Set goal as some arbitrary position on the grid
# grid_goal = (-north_offset + 10, -east_offset + 20)
# TODO: adapt to set goal as latitude / longitude position and convert
geodetic_goal_cordinates = [TARGET_LON, TARGET_LAT,
TARGET_ALTITUDE] # center of MAP
print("geodetic_goal_cordinates " + str(geodetic_goal_cordinates))
goal_coordinates_NED = global_to_local(geodetic_goal_cordinates,
geodetic_home_coordinates)
print("goal_coordinates_NED " + str(goal_coordinates_NED))
grid_goal = (int(-north_offset + goal_coordinates_NED[0]),
int(-east_offset + goal_coordinates_NED[1]))
print("grid_goal" + str(grid_goal))
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
print("path len before pruning " + str(len(path)))
path = prune_path(path)
print("path len after pruning " + str(len(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):
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
filename = 'colliders.csv'
f = open(filename)
line1 = f.readline()
f.close()
print(line1)
line1 = line1.split(" ")
lat0 = np.float(line1[1][:-1])
lon0 = np.float(line1[3])
print("lat0", lat0)
print("lon0", lon0)
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0.0)
# TODO: retrieve current global position
current_global_position = self.global_position
# TODO: convert to current local position using global_to_local()
current_local_position = global_to_local(self.global_position,
self.global_home)
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
grid_start = (-north_offset, -east_offset)
# TODO: 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 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
# Corner of Front St & Market st
goal_lon = -122.398494
goal_lat = 37.791558
goal_home_position = (goal_lon, goal_lat, 0)
goal_local_position = global_to_local(goal_home_position,
self.global_home)
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
# 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 = prune_path(path)
print('Pruned Path:', pruned_path)
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in pruned_path]
# 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
filename = 'colliders.csv'
with open(filename) as f:
origin_pos_data = f.readline().split(',')
lat0 = float(origin_pos_data[0].strip().split(' ')[1])
lon0 = float(origin_pos_data[1].strip().split(' ')[1])
# set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# retrieve current global position
current_global_position = [
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))
# 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.round(current_local_position[0])) - north_offset,
int(np.round(current_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
# Path not found
# goal_lon = -122.396342
# goal_lat = 37.796520
# OK
# goal_lon = -122.397532
# goal_lat = 37.792973
goal_lon = -122.399219
goal_lat = 37.794262
goal_global = (goal_lon, goal_lat, 0)
goal_local = global_to_local(goal_global, self.global_home)
grid_goal = (int(np.round(goal_local[0])) - north_offset,
int(np.round(goal_local[1])) - east_offset)
# Run A* to find a path from start to goal
# 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, grid_start, grid_goal)
# prune path to minimize number of waypoints
print('prunning path', path)
pruned_path = prune_path(path)
print('prunned path', pruned_path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, 5, 0]
for p in pruned_path]
print(waypoints)
# Set self.waypoints
self.waypoints = waypoints
# send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
#read lat0, lon0 from colliders into floating point values
lat0, lon0 = self.get_home()
#set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: convert to current local position using global_to_local()
local_north, local_east, _ = 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
# 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
#set goal as latitude / longitude position and convert
goal_lat = 37.79456
goal_lon = -122.397437
local_goal = global_to_local((goal_lon, goal_lat, 0), self.global_home)
# convert to grid coordinates
grid_goal = (int(local_goal[0]) - north_offset,
int(local_goal[1]) - east_offset)
# Run A* to find a path from start to goal
# I added diagonal motions with a cost of sqrt(2) to your A* implementation
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
print('Path:', path)
# prune path to minimize number of waypoints
prunned_path = prune_path(path)
print('Prunned path:', prunned_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
# 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
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
""" probably the np.genfromtxt fuction could have done this, but I was not able
to find the way, used csv.reader to get it"""
latlondata = csv.reader(open('colliders.csv', newline=''),
delimiter=',')
for row in latlondata:
lat0, lon0 = row[:2]
break
lat0 = lat0.replace("lat0 ", "")
lon0 = lon0.replace(" lon0 ", "")
lat0 = float(lat0)
lon0 = float(lon0)
# # TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
Globalcurrent = np.array(
(self.global_position[0], self.global_position[1],
self.global_position[2]))
# TODO: convert to current local position using global_to_local()
Localcurrent = global_to_local(Globalcurrent, self.global_home)
#self.local_position = [Localcurrent[0],Localcurrent[1],Localcurrent[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
""" Create grid comes from the planning_utils.py so we can call it directly here"""
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
""" Using Medial-Axis solution for motion planning 3"""
skeleton = medial_axis(invert(grid))
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
grid_center = (north_offset, east_offset)
# TODO: convert start position to current position rather than map center
""" Get local grid position and determine the new grid start from global home then
add it to grid center """
Localcurrent = global_to_local(self.global_position, self.global_home)
grid_start = (int(-grid_center[0] + Localcurrent[0]),
int(-grid_center[1] + Localcurrent[1]))
# Set goal as some arbitrary position on the grid
#grid_goal = (-north_offset + 10, -east_offset + 10)
# TODO: adapt to set goal as latitude / longitude position and convert
""" code to get the grid goal based on LAT LONG position, assume altitude is 0 for goal"""
grid_goal_lat_lon = (-122.39966, 37.793593, 0)
#grid_goal_lat_lon = (-122.397185, 37.792857, 0)
grid_goal = global_to_local(grid_goal_lat_lon, self.global_home)
grid_goal = tuple((int(grid_goal[0] - north_offset),
int(grid_goal[1] - east_offset)))
print(grid_start, grid_goal)
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
""" Done on Planning_utils.py """
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
""" Using Medial-Axis solution to find the skeleton start and goal"""
skel_start, skel_goal = find_start_goal(skeleton, grid_start,
grid_goal)
print(grid_start, grid_goal)
print(skel_start, skel_goal)
""" a_star, heuristic come from the planning_utils.py so we can call it directly"""
#path, _ = a_star(grid, heuristic, grid_start, grid_goal)
path, _ = a_star(
invert(skeleton).astype(np.int), heuristic, tuple(skel_start),
tuple(skel_goal))
# TODO: prune path to minimize number of waypoints
pruned_path = prune_path(path)
""" Add the grid goal so that it can fly to the exact location"""
pruned_path.append([grid_goal[0], grid_goal[1]])
# TODO (if you're feeling ambitious): Try a different approach altogether!
""" USE RRT to replan """
# num_vertices = 300
# dt = 1
# x_init = (50, 50)
# rrt = generate_RRT(grid, x_init, num_vertices, dt)
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in pruned_path]
#print (waypoints)
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
""" This function sends the waypoints to the sim for visualization"""
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
""" probably the np.genfromtxt fuction could have done this, but I was not able
to find the way, used csv.reader to get it"""
latlondata = csv.reader(open('colliders.csv', newline=''),
delimiter=',')
for row in latlondata:
lat0, lon0 = row[:2]
break
lat0 = lat0.replace("lat0 ", "")
lon0 = lon0.replace(" lon0 ", "")
lat0 = float(lat0)
lon0 = float(lon0)
# # TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
Globalcurrent = np.array(
(self.global_position[0], self.global_position[1],
self.global_position[2]))
# TODO: convert to current local position using global_to_local()
Localcurrent = global_to_local(Globalcurrent, self.global_home)
#self.local_position = [Localcurrent[0],Localcurrent[1],Localcurrent[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)
Zmax = np.max(data[:, 2] + data[:, 5])
# Define a grid for a particular altitude and safety margin around obstacles
""" Create grid comes from the planning_utils.py so we can call it directly here"""
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
""" Using Medial-Axis solution for motion planning 3"""
skeleton = medial_axis(invert(grid))
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
grid_center = (north_offset, east_offset)
# TODO: convert start position to current position rather than map center
""" Get local grid position and determine the new grid start from global home then
add it to grid center """
Localcurrent = global_to_local(self.global_position, self.global_home)
grid_start = (int(-grid_center[0] + Localcurrent[0]),
int(-grid_center[1] + Localcurrent[1]))
#if local position is not at altitude 0, update the altitute for takeoff
if -Localcurrent[2] > TARGET_ALTITUDE:
TARGET_ALTITUDE = int(TARGET_ALTITUDE - Localcurrent[2])
self.target_position[2] = TARGET_ALTITUDE
# Set goal as some arbitrary position on the grid
if UseLatLonAlt == False:
grid_goal = (-north_offset + Localcurrent[0] + 10,
-east_offset + Localcurrent[1] + 10)
print("using grid position for the goal")
else:
print("using Lat, Lon, Alt position for the goal")
# TODO: adapt to set goal as latitude / longitude position and convert
""" See global variable grid_lat_lon at the top of the file"""
""" code to get the grid goal based on LAT LONG position, assume altitude is 0 for goal"""
grid_goal = global_to_local(grid_goal_lat_lon_alt,
self.global_home)
grid_goal = tuple((int(grid_goal[0] - north_offset),
int(grid_goal[1] - east_offset)))
print(grid_start, grid_goal)
# if Altitude is not ground level adapt to new altitude
if grid_goal_lat_lon_alt[2] > TARGET_ALTITUDE:
TARGET_ALTITUDE = grid_goal_lat_lon_alt[2] + SAFETY_DISTANCE
self.target_position[2] = TARGET_ALTITUDE
#self.global_home[2] = grid_goal_lat_lon_alt[2]
grid, north_offset, east_offset = create_grid(
data, TARGET_ALTITUDE, SAFETY_DISTANCE)
""" Using Medial-Axis solution for motion planning 3"""
skeleton = medial_axis(invert(grid))
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
""" Done on Planning_utils.py """
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
""" Using Medial-Axis solution to find the skeleton start and goal"""
skel_start, skel_goal = find_start_goal(skeleton, grid_start,
grid_goal)
print(grid_start, grid_goal)
print(skel_start, skel_goal)
""" a_star, heuristic come from the planning_utils.py so we can call it directly"""
#path, _ = a_star(grid, heuristic, grid_start, grid_goal)
path, _, FoundPath = a_star(
invert(skeleton).astype(np.int), heuristic, tuple(skel_start),
tuple(skel_goal))
while FoundPath == False:
# increase target altitude by 5 ft, get new skeeton and try again
TARGET_ALTITUDE += 20
print("TRY AGAIN, NEW CRUISE ALTITUDE")
print(TARGET_ALTITUDE)
self.target_position[2] = TARGET_ALTITUDE
grid, north_offset, east_offset = create_grid(
data, TARGET_ALTITUDE, SAFETY_DISTANCE)
skeleton = medial_axis(invert(grid))
skel_start, skel_goal = find_start_goal(skeleton, grid_start,
grid_goal)
path, _, FoundPath = a_star(
invert(skeleton).astype(np.int), heuristic, tuple(skel_start),
tuple(skel_goal))
if TARGET_ALTITUDE > Zmax:
break
# TODO: prune path to minimize number of waypoints
pruned_path = prune_path(path)
""" Add the grid goal so that it can fly to the exact location"""
pruned_path.append([grid_goal[0], grid_goal[1]])
sampler = Sampler(data, SAFETY_DISTANCE)
polygons = sampler._polygons
# Second Prunning to try to minimize waypoints by doing direct to
# Also add Altitude
Path3D, TARGET_ALTITUDE = get3DPath(pruned_path,TARGET_ALTITUDE,self.local_position[2],\
grid_goal_lat_lon_alt[2],SAFETY_DISTANCE, polygons)
# TODO (if you're feeling ambitious): Try a different approach altogether!
#add heading to all the waypoints
Path_with_heading = []
Path_with_heading.append([Path3D[0][0], Path3D[0][1], 0])
for i in range(0, len(Path3D) - 1):
heading = np.arctan2((Path3D[i + 1][1] - Path3D[i][1]),
(Path3D[i + 1][0] - Path3D[i][0]))
Path_with_heading.append(
[Path3D[i + 1][0], Path3D[i + 1][1],
int(heading)])
if FoundPath == False:
# if path not found then takeoff and land on the spot
localpos = global_to_local(self.global_position, self.global_home)
waypoints = [[int(localpos[0]),int(localpos[1]),5,0],\
[int(localpos[0]),int(localpos[1]),0,0]]
else:
# Convert path to waypoints
waypoints = [[
int(p[0] + north_offset),
int(p[1] + east_offset), TARGET_ALTITUDE,
int(p[2])
] for p in Path_with_heading]
print(waypoints)
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
""" This function sends the waypoints to the sim for visualization"""
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 8 # extra margin of safety around obstacles
self.target_position[2] = TARGET_ALTITUDE
# debug flag
debug = True
# Requirement #1:
# * Read lat0, lon0 from colliders into floating point values
# * set home position to (lon0, lat0, 0)
lat0, lon0 = get_latlon_fromfile('colliders.csv')
self.set_home_position(lon0, lat0, 0)
# Requirement #2:
# * retrieve current global position
# * convert to current local position using global_to_local()
curr_global_pos = (self._longitude, self._latitude, self._altitude)
curr_global_home = self.global_home
curr_local_pos = global_to_local(curr_global_pos, self.global_home)
if debug:
print("curr_global_pos: ", curr_global_pos)
print("curr_global_home: ", curr_global_home)
print("curr_local_pos: ", curr_local_pos)
# 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)
# save offsets as tuple
offsets = (north_offset, east_offset)
# Requirement #3: convert start position to current position rather than map center
grid_start = get_gridrelative_position(curr_local_pos[0:2], offsets)
# Requirement #4: set goal as latitude/longitude
# specified as command line args
custom_goal_pos = (self.goal_longitude, self.goal_latitude, 0)
goal_local = global_to_local(custom_goal_pos, self.global_home)[0:2]
grid_goal = get_gridrelative_position(goal_local, offsets)
# Run A* to find a path from start to goal
print('Grid Start and Goal: ', grid_start, grid_goal)
print('START: Path planning....')
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
print('Path planning complete!')
# Requirement 6: 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]
if debug:
print("waypoints:")
for i, waypoint in enumerate(waypoints):
print("{}: {}".format(i, waypoint))
# Set self.waypoints
self.waypoints = waypoints
# Send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# Read lat0, lon0 from colliders into floating point values
lat0 = lon0 = None
with open('colliders.csv') as f:
ns = re.findall("-*\d+\.\d+", f.readline())
assert len(
ns
) == 2, "Could not parse lat0 & lon0 from 'colliders.csv'. The file might be broken."
lat0, lon0 = float(ns[0]), float(ns[1])
self.set_home_position(
lon0, lat0, 0) # set home position as stated in colliders.csv
# curLocal - is local position of drone relative home)
curLocal = global_to_local(
(self._longitude, self._latitude, self._altitude),
self.global_home)
print('global home {0}'.format(self.global_home.tolist()))
print('position {0}'.format(self.global_position.tolist()))
print('local position {0}'.format(self.local_position.tolist()))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
timer = time.time()
grid, offset, edges = create_grid_and_edges(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print('Voronoi {0} edges created in {1}s'.format(
len(edges),
time.time() - timer))
print('Offsets: north = {0} east = {1}'.format(offset[0], offset[1]))
G = nx.Graph()
for e in edges:
G.add_edge(tuple(e[0]),
tuple(e[1]),
weight=np.linalg.norm(np.array(e[1]) - np.array(e[0])))
print('Graph created')
# Define starting point on the grid as current location
grid_start = (-offset[0] + int(curLocal[0]),
-offset[1] + int(curLocal[1]))
# Set goal as some arbitrary position on the grid
# grid_goal = (-offset[0] + 64, -offset[1] + 85)
# grid_goal = (290, 720)
if self.goal is None:
grid_goal = None
while not grid_goal:
i, j = random.randint(0, grid.shape[0] - 1), random.randint(
0, grid.shape[1] - 1)
if not grid[i, j]: grid_goal = (i, j)
else:
target_global = global_to_local(
(self.goal[1], self.goal[0], TARGET_ALTITUDE),
self.global_home)
grid_goal = (-offset[0] + int(target_global[0]),
-offset[1] + int(target_global[1]))
print('Path: {0} -> {1}'.format(grid_start, grid_goal))
graph_start = closest_point(G, grid_start)
graph_goal = closest_point(G, grid_goal)
print('Graph path: {0} -> {1}'.format(
(graph_start[0] - offset[0], graph_start[1] - offset[1]),
(graph_goal[0] - offset[0], graph_goal[1] - offset[1])))
timer = time.time()
path, cost = a_star_graph(G, graph_start, graph_goal)
if path is None:
print('Could not find path')
return
print('Found path on graph of {0} waypoints in {1}s'.format(
len(path),
time.time() - timer))
# Add to path exact (non-voronoi) start&goal waypoints
path = [(int(p[0]), int(p[1])) for p in path]
path.insert(0, grid_start)
path.insert(len(path), grid_goal)
# Prune the path on grid twice
timer = time.time()
pruned_path = prune_path(path, grid)
pruned_path = prune_path(pruned_path, grid)
print('Pruned path from {0} to {1} waypoints in {2}s'.format(
len(path), len(pruned_path),
time.time() - timer))
path = pruned_path
# Convert path to waypoints
waypoints = [[p[0] + offset[0], p[1] + offset[1], TARGET_ALTITUDE, 0]
for p in path]
# Set self.waypoints
self.waypoints = waypoints
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
with open('colliders.csv', newline='') as f:
reader = csv.reader(f)
row1 = next(reader)
for item in row1:
if 'lat0' in item:
lat0 = np.float(item.strip().split(' ')[1])
elif 'lon0' in item:
lon0 = np.float(item.strip().split(' ')[1])
# 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, 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_north = int(np.ceil(local_position[0] + grid_start[0]))
grid_start_east = int(np.ceil(local_position[1] + grid_start[1]))
grid_start = (grid_start_north, grid_start_east)
# 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
goal_lon = -122.40199327
goal_lat = 37.79245808
goal_global_pos = global_to_local((goal_lon, goal_lat, 0),
self.global_home)
grid_goal_north = int(np.ceil(goal_global_pos[0] + grid_start[0]))
grid_goal_east = int(np.ceil(goal_global_pos[1] + grid_start[1]))
grid_goal = (grid_goal_north, grid_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
# TODO (if you're feeling ambitious): Try a different approach altogether!
pruned_path = prune_path(path)
plot_path(grid, pruned_path, north_offset, east_offset, grid_start,
grid_goal)
# 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
with open('colliders.csv', newline='') as f:
r = csv.reader(f)
r1 = next(r)
# TODO: set home position to (lon0, lat0, 0)
lat0 = float((r1[0].strip('lat0')))
lon0 = float((r1[1].strip(' lon0')))
print(lat0,lon0)
self.set_home_position(lon0,lat0,0.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 = 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
n_start = int(local_position[0])
e_start = int(local_position[1])
grid_start = ((n_start + -north_offset),(e_start + -east_offset))
# Set goal as some arbitrary position on the grid
grid_goal = (-north_offset + 10, -east_offset + 10)
print("north_start:",n_start,"easth_start:",e_start)
print ("Grid_Start:",grid_start)
# TODO: adapt to set goal as latitude / longitude position and conver
goal_lon = -122.396640
goal_lat = 37.796232
goal_global = [goal_lon ,goal_lat , 0]
goal_local = global_to_local (goal_global,self.global_home)
north_goal = int(goal_local[0])
east_goal = int(goal_local[1])
grid_goal = ((north_goal+-north_offset) ,(east_goal+-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
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):
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
home = np.genfromtxt('colliders.csv',
delimiter=',',
dtype=None,
encoding=None,
max_rows=1)
home_lat = home[0].split()[1]
home_long = home[1].split()[1]
# TODO: set home position to (lat0, lon0, 0)
self.set_home_position(float(home_long), float(home_lat), 0)
# TODO: retrieve current global position
# TODO: convert to current local position using global_to_local()
current_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 (this is just grid center)
# TODO: convert start position to current position rather than map center
grid_start = (int(current_local[0]) - north_offset,
int(current_local[1]) - east_offset)
# Set goal as some arbitrary position on the grid
global_goal = (-122.396504, 37.795887, 0)
# TODO: adapt to set goal as latitude / longitude position and convert
local_goal = global_to_local(global_goal, 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)
# 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
print('Found waypoints: {}'.format(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
# Done: read lat0, lon0 from colliders into floating point values
f = open('colliders.csv')
header = f.readline().strip('\n').split(',')
long_lat = {}
for h in header:
line = h.strip().split(' ')
long_lat[line[0]] = np.float(line[1])
print("HOME Position %s", long_lat)
# DONE: set home position to (lon0, lat0, 0)
self.set_home_position(long_lat['lon0'], long_lat['lat0'], 0)
# retrieve current global position
# DONE: convert to current local position using global_to_local()
local_north, local_east, _ = global_to_local(self.global_position,
self.global_home)
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
# grid_start = (-north_offset, -east_offset)
print("LOCAL POS %s %s" % (local_north, local_east))
# DONE: 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
# DONE: Adapt to set goal as latitude / longitude position and convert
local_goal_north, local_goal_east, _ = global_to_local(
self._global_goal_position, self.global_home)
grid_goal = (int(local_goal_north - north_offset),
int(local_goal_east - east_offset))
# Run A* to find a path from start to goal
# DONE: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# DONE: prune path to minimize number of waypoints
path = prune_path(path, grid)
print("No. of points %s" % len(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 = 20
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# Read lat0, lon0 from colliders into floating point values
with open('colliders.csv', newline='') as f:
reader = csv.reader(f)
row1 = next(reader)
lat0 = float(row1[0].split()[1])
lon0 = float(row1[1].split()[1])
# Set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# Retrieve current global position
global_pos = self.global_position
# 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)
# Create a voxmap (2.5D map) centered at TARGET_ALTITUDE
# and with a resolution of:
VOXMAP_RES = 10
voxmap, north_offset, east_offset = create_voxmap(data, TARGET_ALTITUDE, SAFETY_DISTANCE, VOXMAP_RES)
# Create a 1m3 resolution voxmap (for local planning)
self.voxmap, self.north_offset, self.east_offset = create_voxmap(data, TARGET_ALTITUDE, SAFETY_DISTANCE, 1)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid
grid_start = (int(local_pos[0]-north_offset), int(local_pos[1]-east_offset), TARGET_ALTITUDE)
# Define starting point on the voxmap
voxmap_start = (grid_start[0] // VOXMAP_RES, grid_start[1] // VOXMAP_RES, grid_start[2] // VOXMAP_RES)
# Set goal as some arbitrary position on the grid/voxmap
while True:
n_goal = random.randint(0, voxmap.shape[0] - 1)
e_goal = random.randint(0, voxmap.shape[1] - 1)
alt_goal = random.randint(0, voxmap.shape[2] - 1)
if voxmap[n_goal, e_goal, alt_goal] == 0:
break
# Voxmap goal
voxmap_goal = (n_goal, e_goal, alt_goal)
# Run A* to find a path from start to goal
print('Voxmap Start and Goal: ', voxmap_start, voxmap_goal)
# Grid Search
#path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# Graph Search
#path, _ = graph_a_star(G, heuristic, graph_start, graph_goal)
# 3D A* Search
path, _ = a_star_3D(voxmap, heuristic, voxmap_start, voxmap_goal)
# Prune path to minimize number of waypoints
path = prune_path(path)
print("Path: ", path)
# Convert path to waypoints
waypoints = []
for i in range(len(path)):
p = path[i]
if i:
last_p = path[i-1]
# Set heading based on relative position to last wp
heading = np.arctan2((p[1]-last_p[1]), (p[0]-last_p[0]))
else:
heading = 0
# Append waypoint
waypoints.append([p[0] * VOXMAP_RES + north_offset, p[1] * VOXMAP_RES + east_offset, p[2] * VOXMAP_RES, heading])
print("Waypoints: ", waypoints)
# Set self.global_waypoints
self.global_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 = 3
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
with open('colliders.csv', newline='') as f:
reader = csv.reader(f)
row1 = next(reader) # gets the first line
lat0, lon0 = float(row1[0][5:]), float(row1[1][5:])
# TODO: set home position to (lat0, lon0, 0)
self.set_home_position(
lon0, lat0, 0) # set the current location to be the home position
# TODO: retrieve current global position
current_global_pos = (self._longitude, self._latitude, self._altitude)
# TODO: convert to current local position using global_to_local()
current_local_pos = global_to_local(current_global_pos,
self.global_home)
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Get the center of the grid
north_offset = int(np.floor(np.min(data[:, 0] - data[:, 3])))
east_offset = int(np.floor(np.min(data[:, 1] - data[:, 4])))
# Define a grid for a particular altitude and safety margin around obstacles
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# This is now the routine using Voronoi
grid_graph, edges = create_grid_and_edges(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("GRAPH EDGES:", len(edges))
# Create the graph with the weight of the edges
# set to the Euclidean distance between the points
G = nx.Graph()
for e in edges:
p1 = e[0]
p2 = e[1]
dist = LA.norm(np.array(p2) - np.array(p1))
G.add_edge(p1, p2, weight=dist)
# TODO: convert start position to current position rather than map center
grid_start = (int(current_local_pos[0] - north_offset),
int(current_local_pos[1] - east_offset))
# TODO: adapt to set goal as latitude / longitude position and convert
goal_global_pos = (-122.393010, 37.790000, 0)
goal_local_pos = global_to_local(goal_global_pos, self.global_home)
grid_goal = (int(goal_local_pos[0] - north_offset),
int(goal_local_pos[1] - east_offset))
#goal_ne = (840., 100.)
start_ne_g = closest_point(G, grid_start)
goal_ne_g = closest_point(G, grid_goal)
print("START NE GOAL:", start_ne_g)
print("GOAL NE GOAL", goal_ne_g)
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
path, cost = a_star_graph(G, heuristic_graph, start_ne_g, goal_ne_g)
print("Path Length:", len(path), "Path Cost:", cost)
int_path = [[int(p[0]), int(p[1])] for p in path]
# TODO: prune path to minimize number of waypoints
pruned_path = prune_path(int_path)
print("Length Pruned Path:", len(pruned_path))
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim
self.send_waypoints()
def plan_local_path(self):
"Plans a local path inside a local volume around the current location"
# Define current position in the (global) voxmap
if len(self.local_waypoints):
last_wp = self.local_waypoints[-1]
current_voxmap = (int(last_wp[0]-self.north_offset),
int(last_wp[1]-self.east_offset),
int(last_wp[2]))
else:
current_voxmap = (int(self.local_position[0]-self.north_offset),
int(self.local_position[1]-self.east_offset),
int(-self.local_position[2]))
# Define size of local map (40m x 40m x 10 m)
d_north = 20
d_east = 20
d_alt = 5
vox_shape = self.voxmap.shape
north_min = int(np.clip(current_voxmap[0] - d_north, 0, vox_shape[0]))
north_max = int(np.clip(current_voxmap[0] + d_north, 0, vox_shape[0]))
east_min = int(np.clip(current_voxmap[1] - d_east, 0, vox_shape[1]))
east_max = int(np.clip(current_voxmap[1] + d_east, 0, vox_shape[1]))
alt_min = int(np.clip(current_voxmap[2] - d_alt, 0, vox_shape[2]))
alt_max = int(np.clip(current_voxmap[2] + d_alt, 0, vox_shape[2]))
local_voxmap = self.voxmap[north_min:north_max, east_min:east_max, alt_min:alt_max]
# Define next global wp
next_wp = self.global_waypoints[0]
# Define start in the center of voxmap
local_start = (d_north, d_east, d_alt)
# Define goal to a node in the direction of the next waypoint
local_goal = (np.clip(next_wp[0]-self.north_offset-north_min, 0, 2*d_north-1),
np.clip(next_wp[1]-self.east_offset-east_min, 0, 2*d_east-1),
np.clip(next_wp[2]-alt_min, 0, 2*d_alt-1))
# 3D A* Search
path, _ = a_star_3D(local_voxmap, heuristic, local_start, local_goal)
if (len(path) == 0):
# Go to the next waypoint if failed to find a path
del self.global_waypoints[0]
if len(self.global_waypoints):
self.plan_local_path()
else:
return
# Prune path to minimize number of waypoints
path = prune_path(path)
# Convert path to waypoints
for i in range(1, len(path)):
p = path[i]
# Append waypoint to local waypoints
self.local_waypoints.append([p[0] + self.north_offset + north_min,
p[1] + self.east_offset + east_min,
p[2] + alt_min, next_wp[3]])
# Delete next global wp if already reached
if np.linalg.norm(np.array(self.local_waypoints[-1]) - np.array(self.global_waypoints[0])) < 1.0:
del self.global_waypoints[0]
print("Waypoints: ", self.local_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 = 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()
