def plan_medial_axis(self, data,local_start, local_goal, TARGET_ALTITUDE, SAFETY_DISTANCE):
print('Medial-Axis planning')
# create grid and skeleton
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
skeleton = medial_axis(invert(grid))
print('Skeleton generated')
# calculate the closest start/goal points on the "graph"
start_ne = ( local_start[0] - north_offset, local_start[1] - east_offset)
goal_ne = ( local_goal[0] - north_offset, local_goal[1] - east_offset)
skel_start, skel_goal = find_start_goal(skeleton, start_ne, goal_ne)
# run A* to search for the goal along the road network
path, cost = a_star(invert(skeleton).astype(np.int), heuristic, tuple(skel_start), tuple(skel_goal))
# Prune path to minimize number of waypoints
if (args.prune == 'collinearity'):
print('Pruning path with collinearity check')
path = collinearity_prune(path)
elif (args.prune == 'bresenham'):
print('Pruning path with bresenham')
path = bresenham_prune(path,grid)
else:
print('Unrecognized prune option returning full path')
# Convert path to waypoints
waypoints = [[int(p[0]) + north_offset,int(p[1]) + east_offset, TARGET_ALTITUDE, 0] for p in path]
return waypoints
def medial_axis_Astar(self, data, global_goal, TARGET_ALTITUDE,
SAFETY_DISTANCE):
grid, north_offset, east_offset = create_grid_bfs(
data, TARGET_ALTITUDE, SAFETY_DISTANCE)
skeleton = medial_axis(invert(grid))
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
grid_start = (int(self.local_position[0]) - north_offset,
int(self.local_position[1]) - east_offset)
local_goal = global_to_local(global_goal, self.global_home)
grid_goal = (int(local_goal[0]) - north_offset,
int(local_goal[1]) - east_offset)
skel_start, skel_goal = find_start_goal(skeleton, grid_start,
grid_goal)
mapa = breadth_first(
invert(skeleton).astype(np.int), tuple(skel_goal),
tuple(skel_start))
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star_bfs(
invert(skeleton).astype(np.int), mapa, tuple(skel_start),
tuple(skel_goal))
path.append(grid_goal)
pruned_path = prune_path_bres(path, grid)
waypoints = [[
int(p[0] + north_offset),
int(p[1] + east_offset), TARGET_ALTITUDE, 0
] for p in pruned_path]
for i in range(1, len(waypoints)):
heading = np.arctan2(
waypoints[i][0] - waypoints[i - 1][0],
waypoints[i][1] - waypoints[i - 1][1]) - pi / 2
waypoints[i][3] = -heading
return waypoints
def plan_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
lat0, lon0 = read_home_location()
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
current_global_position = [
self._longitude, self._latitude, self._altitude
]
print("\nself.global_position: {0}".format(self.global_position))
print("current_global_position: {0}".format(current_global_position))
# TODO: convert to current local position using global_to_local()
current_local_position = global_to_local(current_global_position,
self.global_home)
print("current_local_position: {0}".format(current_local_position))
print('\nglobal home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
self.collider_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(self.collider_map_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) #hardcoded to middle of the grid, replacing it with below:
# TODO: convert start position to current position rather than map center
grid_start = (int(np.ceil(current_local_position[0] - north_offset)),
int(np.ceil(current_local_position[1] - east_offset)))
print("grid_start: {0}".format(grid_start))
# Set goal as some arbitrary position on the grid
#grid_goal = (-north_offset + 10, -east_offset + 10) #hardcoded as some location 10 m north and 10 m east of map center as dfault, replacing it with below:
# TODO: adapt to set goal as latitude / longitude position and convert
#goal_global_position = new_global_position(self.global_home, 15, 25) #adding 20 x 10 meters to the global_home position.
goal_global_position = [-122.401055, 37.795461, 0]
goal_local_position = global_to_local(goal_global_position,
self.global_home)
grid_goal = (int(goal_local_position[0] - north_offset),
int(goal_local_position[1] - east_offset))
print("goal_local_position: {0}".format(goal_local_position))
print("grid_goal: {0}".format(grid_goal))
print("grid.shape: {0}".format(grid.shape))
if grid_goal[0] > grid.shape[0] or grid_goal[1] > grid.shape[
1] or grid_goal[0] < 0 or grid_goal[1] < 0:
print(
"\n * NOTE: Destination is outside the grid map. setting it back to start "
)
grid_goal = grid_start
self.landing_transition()
#if grid[grid_goal[0]][grid_goal[1]] > 0: # checking that the destination is not colliding with any obstacle
# print("\n * NOTE: Destination is inside an obstacle. Setting it to start position. ")
# grid_goal = grid_start
# self.landing_transition()
# REZA: Adding Skeleton median axis
skeleton = medial_axis(invert(grid))
skel_start, skel_goal = find_start_goal(skeleton, grid_start,
grid_goal)
print('\nLocal Skelton Start and Goal: ', 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)
print('Local Grid Start and Goal: ', grid_start, grid_goal)
# Choose one of the following A*
#path, _ = a_star(grid, heuristic, grid_start, grid_goal) #Run A* on the grid
path, _ = a_star(
invert(skeleton).astype(np.int), heuristic, tuple(skel_start),
tuple(skel_goal))
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
print("path: ", path)
path = prune_path(path)
print("revised 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
# *** Initiating
gridsize3d = np.array([6, 6, 6])
print("Initiating a 3D grid, size: " + str(gridsize3d))
self.grid3D = HorisonMap3D(gridsize3d, self.local_position, 2)
# 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', 'r') as f:
latlon = f.readline()
ll = latlon.strip().replace(',', '').split(' ')
lat0, lon0 = float(ll[1]), float(ll[3])
# TODO: set home position to (lon0, lat0, 0)
# self.set_home_position(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()
current_local_position = global_to_local(global_position,
self.global_home)
print('new local position:{}'.format(current_local_position))
self._north, self._east, self._down = current_local_position
print('self.local_position:{}'.format(self.local_position))
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
grid_start_off = (-north_offset, -east_offset)
# TODO: convert start position to current position rather than map center
# grid_start = (int(current_local_position[0] + grid_start[0]), int(current_local_position[1] + grid_start[1]))
grid_start = tuple(
map(sum, zip(map(int, current_local_position), grid_start_off)))
# 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
# arbitrary_global_position = (10, 10) # lat/lon
# arbitrary_local_position = global_to_local(arbitrary_global_position, self.global_home)
arbitrary_local_position = self.grid_goal
print('grid_goal:{}'.format(arbitrary_local_position))
grid_goal = tuple(
map(sum, zip(grid_start_off, arbitrary_local_position)))
# 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)
# graph A* and graph_goal/start and medial_axis
skeleton = medial_axis(invert(grid))
skel_start, skel_goal = find_start_goal(skeleton, grid_start,
grid_goal)
print('skel start and goal: {}, {}'.format(skel_start, skel_goal))
# todo graph A*
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('len(path):{}'.format(len(path)))
path = prune_path(path)
print('new len(path):{}'.format(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 prob_road_map(self):
"""Probablistic Road Map implementation to create configuration space, set start and goal positions, find path from start to goal, and condense and set waypoints for navigation.
"""
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
lat0, lon0 = self.read_home_lat_lon('colliders.csv')
# TODO: set home position to (lon0, lat0, 0)
print(f'Setting home to (lon0, lat0, 0): ({lon0, lat0, 0})')
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
global_position = self.global_position
# TODO: convert to current local position using global_to_local()
local_home = global_to_local(global_position, self.global_home)
print(f'Home coordinates in local form: {local_home}')
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
print(f'Constructing Probablistic Road Map...')
st = time.time()
grid, G, north_offset, east_offset = construct_road_map(
data, TARGET_ALTITUDE, SAFETY_DISTANCE, 500, 4)
time_taken = time.time() - st
print(f'construct_road_map() took: {time_taken} seconds')
# TODO: convert start position to current position rather than map center
grid_start = (int(self.local_position[0] - north_offset),
int(self.local_position[1] - east_offset))
# goal_lon_lat = [-122.398525, 37.793510, 0]
# goal_lon_lat = [-122.398250, 37.793875, 0]
# goal_lon_lat = [-122.398275, 37.796875, 0] # (point3) reachable if SAFETY_DISTANCE = 5
# goal_lon_lat = [-122.399970, 37.795947, 0] # reachable if SAFETY_DISTANCE = 5
# goal_lon_lat = [-122.393284, 37.790545, 0] # (point2)
goal_lon_lat = [-122.401173, 37.795514, 0] # (point1)
# goal_lon_lat = [-122.393805, 37.795825, 0] # not reachable if TARGET_ALTITUDE = 5
# (point1) -> (point2) -> (point3) this sequence results in some
# trees getting in the way of the drone. This is a good example
# of imprecise mapping and we may have to use receding horizon
# planning which utilizes a 3D state space for a limited region
# around the drone.
grid_goal = global_to_local(goal_lon_lat, self.global_home)
grid_goal = (int(grid_goal[0] - north_offset),
int(grid_goal[1] - east_offset))
# Find closest node on probablistic road map graph
g_start, g_goal = find_start_goal(G, grid_start, grid_goal)
print("Start and Goal location:", grid_start, grid_goal)
print("Start and Goal location on graph:", g_start, g_goal)
path, _ = a_star_graph(G, heuristic, g_start, g_goal)
# Add the final goal state (instead of the approximated
# goal on the graph)
path.append(grid_goal)
print(path)
# Reduce waypoints
path = condense_waypoints(grid, path)
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
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 = load_lat_lon()
# set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# retrieve current global position
glob_p = self.global_position
# convert to current local position using global_to_local()
loc_p = global_to_local(glob_p, 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)
# convert start position to current position rather than map center
grid_start = (-north_offset + int(loc_p[0]),
-east_offset + int(loc_p[1]))
# Set goal as some arbitrary position on the grid
# set goal as latitude / longitude position and convert 37.792480, lon0 -122.397450
loc_goal = global_to_local(self.glob_goal, self.global_position)
grid_goal = (-north_offset + int(loc_goal[0]),
-east_offset + int(loc_goal[1]))
# Run A* to find a path from start to goal
# path, _ = a_star(grid, heuristic, grid_start, grid_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)
skeleton = medial_axis(invert(grid))
skel_start, skel_goal = find_start_goal(skeleton, grid_start,
grid_goal)
print(skel_start, skel_goal)
# Run A* on the skeleton
path, cost = a_star(
invert(skeleton).astype(np.int), heuristic_func, tuple(skel_start),
tuple(skel_goal))
# prune path to minimize number of waypoints
path = prune_path(path)
plotter.plot_all(grid, grid_start, grid_goal, path, skeleton)
# Convert path to waypoints
waypoints = [[
int(p[0]) + north_offset,
int(p[1]) + east_offset, TARGET_ALTITUDE, 0
] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# send waypoints to sim (this is just for visualization of waypoints)
self.connection.start()
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
with open('colliders.csv') as f:
home_pos_data = f.readline().split(',')
lat0 = float(home_pos_data[0].strip().split(' ')[1])
lon0 = float(home_pos_data[1].strip().split(' ')[1])
# DONE: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# DONE: retrieve current global position
current_global_position = [
self._longitude, self._latitude, self._altitude
]
# DONE: 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))
print(
'global home {0}, position {1}, current local position {2}'.format(
self.global_home, current_global_position,
current_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, points = create_grid(
data, TARGET_ALTITUDE, SAFETY_DISTANCE)
edges = create_edges_from_grid(grid, points)
print("Grid = {}, north offset = {}, east offset = {}".format(
grid.shape, north_offset, east_offset))
# Define starting point on the grid
# DONE: 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
# DONE: adapt to set goal as latitude / longitude position and convert
# global_goal = local_to_global((750 + north_offset, 370 + east_offset, 0), self.global_home)
# print('global_goal', global_goal)
global_goal = (-122.39827335, 37.79639627, 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)
) #(750., 370.) #(920, 920)
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_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)
"""
edges = create_edges_from_grid(grid, 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)
closest_grid_start = closest_point(G, grid_start)
closest_grid_goal = closest_point(G, grid_goal)
"""
skeleton = medial_axis(invert(grid))
closest_grid_start, closest_grid_goal = find_start_goal(
skeleton, grid_start, grid_goal)
print('Closest Local Start and Goal: ', closest_grid_start,
closest_grid_goal)
# A*
#path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# A* for Voronoi Graph
#path, _ = a_star_graph(G, heuristic, closest_grid_start, closest_grid_goal)
# A* for Medial Axis
path, _ = a_star(
invert(skeleton).astype(np.int), heuristic,
tuple(closest_grid_start), tuple(closest_grid_goal))
# DONE: prune path to minimize number of waypoints
# DONE (if you're feeling ambitious): Try a different approach altogether!
pruned_path = prune_path(path)
# Convert path to 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
print(self.waypoints)
# DONE: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 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 = 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
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_graph(self):
"""
Construct a configuration space using a graph representation, set destination GPS position, find a path from start (current) position to destination, and minimize and set waypoints.
"""
self.flight_state = States.PLANNING
print("Searching for a path ...")
data = np.loadtxt('map_obstacle_course.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
TARGET_ALTITUDE = 0.3 / 0.0254
SAFETY_DISTANCE = 0.25 / 0.0254
st = time.time()
# grid, G = create_graph_and_edges_crazyflie(
# data, 130, 86, TARGET_ALTITUDE, SAFETY_DISTANCE)
GRID_NORTH_SIZE = 130
GRID_EAST_SIZE = 86
NUMBER_OF_NODES = 200
OUTDEGREE = 4
grid, G = construct_road_map_crazyflie(data, GRID_NORTH_SIZE,
GRID_EAST_SIZE, TARGET_ALTITUDE,
SAFETY_DISTANCE,
NUMBER_OF_NODES, OUTDEGREE)
# visualize_prob_road_map_crazyflie(data, grid, G)
time_taken = time.time() - st
print(f'create_graph_and_edges() took: {time_taken} seconds')
grid_start = (32, 40, 0)
grid_goal = (110, 40, 0)
# Find closest node on the graph
g_start, g_goal = find_start_goal(G, grid_start, grid_goal)
print("Start and Goal location:", grid_start, grid_goal)
print("Start and Goal location on graph:", g_start, g_goal)
path, _ = a_star_graph(G, heuristic, g_start, g_goal)
path.append(grid_goal)
new_path = []
# new_path.append(grid_start)
new_path.append(g_start)
new_path.extend(path)
print(new_path)
unique_path = []
for p in new_path:
if p not in unique_path:
unique_path.append(p)
print(unique_path)
# Reduce waypoints
reduced_path = condense_waypoints_crazyflie(grid, unique_path)
print(f'reduced_path: {reduced_path}')
waypoints = []
# modify path coordinates as offset from the takeoff position
# i.e., we treat the firt position as origin
for p in reduced_path:
print(np.array(p) - np.array(grid_start))
offset = (np.array(p) - np.array(grid_start)) * 0.0254
offset[2] = TARGET_ALTITUDE * 0.0254
waypoints.append(list(offset))
print(f'All waypoints: {waypoints}')
self.all_waypoints = waypoints
visualize_prob_road_map_crazyflie(data,
grid,
G,
grid_start,
grid_goal,
unique_path,
all_nodes=False)
visualize_prob_road_map_crazyflie(data,
grid,
G,
grid_start,
grid_goal,
reduced_path,
all_nodes=False)
return self.all_waypoints
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 4
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
with open('colliders.csv', newline='') as f:
reader = csv.reader(f)
row1 = next(reader) # gets the first line
lat0 = float(row1[0].split()[1])
lon0 = float(row1[1].split()[1])
print('lat0:', lat0, ', lon0:', lon0)
# TODO: set home position to (lon0, lat0, 0)
self.global_home[0] = lon0
self.global_home[1] = lat0
self.global_home[2] = 0
#self.global_home[0]=self._longitude
#self.global_home[1]=self._latitude
#self.global_home[2]=0
print('global home {0}'.format(self.global_home))
# TODO: retrieve current global position
global_position = self.global_position
# TODO: convert to current local position using global_to_local()
local_position = global_to_local(global_position, self.global_home)
print('local position:', local_position)
self.local_position[0] = local_position[0]
self.local_position[1] = local_position[1]
self.local_position[2] = local_position[2]
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
# grid_start = (-north_offset, -east_offset)
grid_start = (int(self.local_position[0] - north_offset),
int(self.local_position[1] - east_offset))
# TODO: convert start position to current position rather than map center
# Set goal as some arbitrary position on the grid
# grid_goal = (-north_offset , -east_offset )
# TODO: adapt to set goal as latitude / longitude position and convert
goal_north = 100
goal_east = -150
grid_goal = (-north_offset + goal_north, -east_offset + goal_east)
if self.choices_list[self.choice_idx] == 'astar':
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
if self.choices_list[self.choice_idx] == 'medial_axis':
skeleton = medial_axis(invert(grid))
skel_start, skel_goal = find_start_goal(skeleton, grid_start,
grid_goal)
print('Local Start and Goal: ', skel_start, skel_goal)
path, _ = a_star(
invert(skeleton).astype(np.int), heuristic, tuple(skel_start),
tuple(skel_goal))
if self.choices_list[self.choice_idx] == 'voronoi_graph_search':
grid, edges, north_offset, east_offset = create_grid_and_edges(
data, TARGET_ALTITUDE, SAFETY_DISTANCE)
G = create_weighted_graph(edges)
start_ne_g = closest_point(G, grid_start)
goal_ne_g = closest_point(G, grid_goal)
print('Local Start and Goal: ', start_ne_g, goal_ne_g)
path, cost = a_star_graph(G, heuristic, start_ne_g, goal_ne_g)
print('path found from a_star')
if self.if_pruning:
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
pruned_path = prune_path(path)
print('path found from a_star is pruned')
# Convert path to waypoints
# waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE,
np.arctan2((q[1] - p[1]), (q[0] - p[0]))
] for p, q in zip(pruned_path[:-1], pruned_path[1:])]
else:
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
goal_set_latlon = True
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
# TODO: set home position to (lon0, lat0, 0)
# TODO: retrieve current global position
# TODO: convert to current local position using global_to_local()
file = open("colliders.csv", 'r')
lines = file.readlines()
words = [i.split(' ') for i in lines[0].split(',')]
lat0 = float(words[0][1])
lon0 = float(words[1][2])
#lat0 = 37.792480
#lon0 = -122.397450
self.set_home_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
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)
skeleton = medial_axis(invert(grid))
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# TODO: convert start position to current position rather than map center
local_position = global_to_local(
(self._longitude, self._latitude, self._altitude),
self.global_home)
grid_start = (int(-north_offset + local_position[1]),
int(-east_offset + local_position[0]))
# Set goal as some arbitrary position on the grid
if goal_set_latlon:
lat_goal = 37.794050
lon_goal = -122.392900
local_goal = global_to_local((lon_goal, lat_goal, 0),
self.global_home)
grid_goal = (-north_offset + local_goal[0],
-east_offset + local_goal[1])
else:
grid_goal = (490, 850)
skel_start, skel_goal = find_start_goal(skeleton, grid_start,
grid_goal)
# TODO: adapt to set goal as latitude / longitude position and convert
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', skel_start, skel_goal)
#path, _ = a_star(grid, heuristic, grid_start, grid_goal)
path, _ = a_star(np.invert(skeleton), heuristic, tuple(skel_start),
tuple(skel_goal))
path = [(int(i[0]), int(i[1])) for i in path]
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
pruned_path = prune_path(grid, path, 20)
# Convert path to waypoints
waypoints = find_waypoints(
pruned_path, [self.local_position[0], self.local_position[1]],
north_offset, east_offset, TARGET_ALTITUDE)
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
self.path_planned = True
plt.figure()
plt.imshow(grid, cmap='Greys', origin='lower')
plt.plot(grid_start[1], grid_start[0], 'x')
plt.plot(grid_goal[1], grid_goal[0], 'o')
pp = np.array(pruned_path)
plt.plot(pp[:, 1], pp[:, 0], 'g')
plt.scatter(pp[:, 1], pp[:, 0])
plt.xlabel('EAST')
plt.ylabel('NORTH')
plt.savefig("Path.png")
plt.close()
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
# for max_rows attribute a numpy vers. >= 1.16 is required, make sure to have an up to date scikit-image package
# data_pos = np.loadtxt('colliders.csv',dtype='str', max_rows=1)
# (lat0,lon0) = [float(data_pos[1][:-1]),float(data_pos[3][:-1])]
# for numpy vers <1.16
with open('colliders.csv') as f:
latLonStrArr = f.readline().rstrip().replace('lat0', '').replace(
'lon0 ', '').split(',')
lat0 = float(latLonStrArr[0])
lon0 = float(latLonStrArr[1])
# DONE: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# DONE: retrieve current global position
global_position = [self._longitude, self._latitude, self._altitude]
# DONE: convert to current local position using global_to_local()
current_local_pos = 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)
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
skeleton = medial_axis(invert(grid))
# DONE: convert start position to current position rather than map center
# Define starting point on the grid (this is just grid center)
start_ne = (int(self.local_position[0] - north_offset),
int(self.local_position[1] - east_offset))
# Set goal as some arbitrary position on the grid
# arb_goal = (750, 370, 0)
# Set random goal on map in local coordinate system
found = False
while not found:
goal_ne = (randrange(0, len(grid[:, 1] - 1)),
randrange(0, len(grid[1, :] - 1)))
if grid[goal_ne] == 0:
found = True
# DONE: adapt to set goal as latitude / longitude position and convert (can be)
# global_goal = local_to_global(arb_goal, self.global_home)
# local_goal to show expected transformation
# goal_ne = global_to_local(global_goal, self.global_home)
print(
"Drone is starting from {0} and the goal was randomly set to {1}".
format(start_ne, goal_ne))
skel_start, skel_goal = find_start_goal(skeleton, start_ne, goal_ne)
# 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(
invert(skeleton).astype(np.int), tuple(skel_start),
tuple(skel_goal))
print("Path length = {0}, path cost = {1}".format(len(path_), cost))
# DONE: prune path to minimize number of waypoints
path = collinearity(path_)
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [[
int(p[0]) + north_offset,
int(p[1]) + east_offset, TARGET_ALTITUDE
] for p in path]
# get heading angle for next point
theta = heading(path)
for i in range(len(waypoints)):
waypoints[i].append(theta[i])
print(waypoints)
# Set self.waypoints
self.waypoints = waypoints
# send waypoints to sim
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
#----------------------------------------------------------------------------
# HOME POSITION
# TODO: read lat0, lon0 from colliders into floating point values
# Open coliders.csv and read lat0 and lon0
colliders_file = 'colliders.csv'
with open(colliders_file) as f:
latlon = f.readline().strip().split(',')
lat0 = float(latlon[0].replace("lat0 ", ""))
lon0 = float(latlon[1].replace("lon0 ", ""))
print('lat: ', lat0)
print('lon: ', lon0)
# finished but latitude longitude are hardcoded for now: set home position to (lat0, lon0, 0)
# I have an issue where float() truncates the last 0 and therefore provides a different value
#self.set_home_position(lon0, lat0, TARGET_ALTITUDE)
self.set_home_position(-122.397450, 37.792480, TARGET_ALTITUDE)
print('Home position is set to local position: ', self.local_position)
# END HOME POSITION
#----------------------------------------------------------------------------
#----------------------------------------------------------------------------
# CURRENT POSITION
# finished: convert self.global_position to current local position using global_to_local()
current_global_position = [
self._longitude, self._latitude, self._altitude
]
print('current_global_position converted using global_to_local: {0}'.
format(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))
# END CURRENT 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)
skeleton = medial_axis(invert(grid))
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# create_grid_and_edges uses Voronoi, it is slower than the previous create_grid() but it's suppose to create a safer path
#grid, edges, north_offset, east_offset = create_grid_and_edges(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
#skeleton = medial_axis(invert(grid))
#print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
#----------------------------------------------------------------------------
# START POINT
# Define starting point on the grid (this is just grid center)
# start_position = (-north_offset, -east_offset)
# finished: convert start position to current position rather than map center
grid_start = (-north_offset + int(self.local_position[0]),
-east_offset + int(self.local_position[1]))
print('start: ', grid_start)
# END START POINT
#----------------------------------------------------------------------------
#----------------------------------------------------------------------------
# SET GOAL POINT
# Set goal as some arbitrary position on the grid
#grid_goal = (-north_offset + 10, -east_offset + 10)
# finished: adapt to set goal as latitude / longitude position and convert
# some lat lon that are working
# first path
goal_lat = float(37.795240)
goal_lon = float(-122.393136)
# second path
#goal_lat = float(37.796874)
#goal_lon = float(-122.399683)
# third path
#goal_lat = float(37.793155)
#goal_lon = float(-122.402035)
# fourth path and back to center
#goal_lat = float(37.792480)
#goal_lon = float(-122.397450)
goal_position = global_to_local((goal_lon, goal_lat, 0),
self.global_home)
print('goal position converted using global_to_local: {0}'.format(
global_to_local((goal_lon, goal_lat, 0), self.global_home)))
grid_goal = (-north_offset + int(goal_position[0]),
-east_offset + int(goal_position[1]))
# END START POINT
#----------------------------------------------------------------------------
#----------------------------------------------------------------------------
# RUN A* USING MEDIAN PATH AND COST OF SQRT(2)
#Run A* to find a path from start to goal
# finished: 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)
skel_start, skel_goal = find_start_goal(skeleton, grid_start,
grid_goal)
print('original start and goal: ', grid_start, grid_goal)
print('calibrated start and goal on skeleton: ', skel_start, skel_goal)
median_path, cost = a_star(
invert(skeleton).astype(np.int), heuristic, tuple(skel_start),
tuple(skel_goal))
print('Median path and cost: ', len(median_path))
pruned_path = prune_path(median_path)
print('pruned path and cost: ', len(pruned_path))
# END RUN A*
#----------------------------------------------------------------------------
# Convert path to waypoints
#waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in pruned_path]
print(waypoints)
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim
self.send_waypoints()
