def grid_plan(self, data,local_start, local_goal, TARGET_ALTITUDE, SAFETY_DISTANCE):
print('Simple Grid-based Planning selected')
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# convert start position to current position rather than map center
grid_start = ( int(local_start[0]) - north_offset, int(local_start[1]) - east_offset)
grid_goal = ( int(local_goal[0]) - north_offset, int(local_goal[1]) - east_offset )
# make sure goal point is not inside an obstacle or safety margin
if (grid[grid_goal[0]][grid_goal[1]] == 1):
print("Invalid goal point: Aborting")
self.landing_transition()
return
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# Prune path to minimize number of waypoints
if (args.prune == 'collinearity'):
print('Pruning path with collinearity check')
path = collinearity_prune(path)
elif (args.prune == 'bresenham'):
print('Pruning path with bresenham')
path = bresenham_prune(path,grid)
else:
print('Unrecognized prune option returning full path')
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]
return waypoints
def run(self):
# 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
TARGET_ALTITUDE = 0.01
SAFETY_DISTANCE = 5
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
# print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
home_lon, home_lat = -122.397450, 37.792480,
self.global_home = (home_lon, home_lat)
grid_start = (0 - north_offset, 0 - east_offset)
grid_goal = (900, 522)
grid_goal = lontat2grid(
[-1.22396533e+02, 3.77977389e+01, -1.00000000e-02], north_offset,
east_offset, self.global_home)
# grid_goal = (276 , 116)
# grid_goal = lontat2grid([-1.22401189e+02, 3.77921385e+01, -1.00000000e-02], north_offset, east_offset, self.global_home)
# grid_goal = (531, 13)
# grid_goal = lontat2grid([-1.22402341e+02, 3.77944427e+01, -1.00000000e-02], north_offset, east_offset, self.global_home)
print("grid_start={}, grid_goal={}".format(grid_start, grid_goal))
self.grid2lonlat(grid_goal, north_offset, east_offset, TARGET_ALTITUDE)
b_show_map = False
if b_show_map:
self.show_map(grid, grid_start, grid_goal)
return
# path = raw_grid_method(grid, grid_start, grid_goal, check_linear = False)
# path, skeleton, start_ne, goal_ne = media_axis_method(grid, grid_start, grid_goal, check_linear = False)
# self.show_media_axis(grid, skeleton, start_ne, goal_ne, path)
path, edges, start_ne, start_ne_g, goal_ne, goal_ne_g = voronoi_method(
data,
TARGET_ALTITUDE,
SAFETY_DISTANCE,
grid_start,
grid_goal,
check_linear=False)
self.show_voronoi(grid, edges, start_ne, start_ne_g, goal_ne,
goal_ne_g, path)
if len(path) == 0:
print("failed to find the path!!!")
return
self.show_map(grid, grid_start, grid_goal, path)
return
def create_graph(data, alt, safety_distance):
"""
returns a graph over open space given occupancy grid
args:
data - occupancy data
alt - drone altitude
safety_distance - safety distance from obstacle
returns:
graph structure over unobstructed space
"""
# get occupancy grid and offsets.
grid, north_offset, east_offset = create_grid(data, alt, safety_distance)
# find object centers
centers = get_object_centers(data, (north_offset, east_offset), alt,
safety_distance)
# create Voronoi object
voronoi = Voronoi(centers)
# find clear edges
edges = find_open_edges(voronoi, grid)
# return graph and offsets
return (create_graph_from_edges(edges), north_offset, east_offset)
def __init__(self, TARGET_ALTITUDE, SAFETY_DISTANCE):
self.TARGET_ALTITUDE = TARGET_ALTITUDE
self.SAFETY_DISTANCE = SAFETY_DISTANCE
n_samples = 300
# 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("grid# = {0}".format(len(grid)))
print("grid shape = {0}".format(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)
with open('colliders.csv') as f:
first_line = f.readline().strip()
latlon = first_line.split(',')
self.lon0 = float(latlon[0].strip().split(' ')[1])
self.lat0 = float(latlon[1].strip().split(' ')[1])
self.grid = grid
self.grid_shape = grid.shape
self.north_offset = north_offset
self.east_offset = east_offset
self.polygons = self.extract_polygons(data)
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 8
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
with open('colliders.csv') as f:
home_pos_data = f.readline().split(",")
lat0 = float(home_pos_data[0].strip().split(" ")[1])
lon0 = float(home_pos_data[1].strip().split(" ")[1])
print(lat0, lon0)
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
global_pos = [self._longitude, self._latitude, self._altitude]
# TODO: convert to current local position using global_to_local()
local_pos = global_to_local(global_pos, self.global_home)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
grid_start = (-north_offset, -east_offset)
# TODO: convert start position to current position rather than map center
grid_start = (int(local_pos[0]-north_offset), int(local_pos[1]- east_offset))
# Set goal as some arbitrary position on the grid
grid_goal = (-north_offset + 10, -east_offset + 10)
# TODO: adapt to set goal as latitude / longitude position and convert
goal_pos = global_to_local([-122.400886, 37.795174, self._altitude], self.global_home)
grid_goal = (int(goal_pos[0]-north_offset), int(goal_pos[1]-east_offset))
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
path = prune_path(path)
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def _plan_helix(self):
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
# Starting at flying altitude
# self.target_position[2] = 10
with open('colliders.csv') as f:
_, lat0, _, lon0 = f.readline().replace(",", " ").replace("\n", "").split()
# Set home position to (lon0, lat0, 0)
self.set_home_position(float(lon0), float(lat0), 0)
# To NED
local_pos = global_to_local(self.global_position, self.global_home)
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Is TARGET_ALTITUDE necessary?
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
# Define starting point on the grid, grid center
grid_start = (int(-north_offset + local_pos[0]), int(-east_offset + local_pos[1]))
# wayps = generate_helix(3, (-north_offset, -east_offset), (0, 0, 200))
wayps = generate_helix(3, (0, 0), (0, 0, 20))
print("wayps", wayps)
# if waypoint list is empty don't do it
# self.waypoints = []
self.waypoints = wayps
def plan_medial_axis(self, data,local_start, local_goal, TARGET_ALTITUDE, SAFETY_DISTANCE):
print('Medial-Axis planning')
# create grid and skeleton
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
skeleton = medial_axis(invert(grid))
print('Skeleton generated')
# calculate the closest start/goal points on the "graph"
start_ne = ( local_start[0] - north_offset, local_start[1] - east_offset)
goal_ne = ( local_goal[0] - north_offset, local_goal[1] - east_offset)
skel_start, skel_goal = find_start_goal(skeleton, start_ne, goal_ne)
# run A* to search for the goal along the road network
path, cost = a_star(invert(skeleton).astype(np.int), heuristic, tuple(skel_start), tuple(skel_goal))
# Prune path to minimize number of waypoints
if (args.prune == 'collinearity'):
print('Pruning path with collinearity check')
path = collinearity_prune(path)
elif (args.prune == 'bresenham'):
print('Pruning path with bresenham')
path = bresenham_prune(path,grid)
else:
print('Unrecognized prune option returning full path')
# Convert path to waypoints
waypoints = [[int(p[0]) + north_offset,int(p[1]) + east_offset, TARGET_ALTITUDE, 0] for p in path]
return waypoints
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 10
SAFETY_DISTANCE = 5
GOAL_LAT = 37.797638
GOAL_LON = -122.394808
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
data = np.loadtxt('colliders.csv', delimiter=',', dtype='U16', usecols = (0, 1))
lat0 = np.float64(data[0, 0].split(' ')[1])
lon0 = np.float64(data[0, 1].split(' ')[2])
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
global_pos = self.global_position
# TODO: convert to current local position using global_to_local()
local_pos = global_to_local(global_pos, self.global_home)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows = 2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
# TODO: convert start position to current position rather than map center
grid_start = (int(-north_offset + local_pos[0]), int(-east_offset + local_pos[1]))
# Set goal as some arbitrary position on the grid
# TODO: adapt to set goal as latitude / longitude position and convert
local_goal = global_to_local([GOAL_LON, GOAL_LAT, 0], self.global_home)
grid_goal = (int(-north_offset + local_goal[0]), int(-east_offset + local_goal[1]))
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
pruned_path = copy.deepcopy(path)
for i in range(1, len(path) - 1):
p1 = path[i-1]
p2 = path[i]
p3 = path[i+1]
det = p1[0]*(p2[1] - p3[1]) + p2[0]*(p3[1] - p1[1]) + p3[0]*(p1[1] - p2[1])
if det == 0:
pruned_path.remove(p2)
path = pruned_path
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 7
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
fileptr = open('colliders.csv')
latitudeLongitudeArr = fileptr.readline().rstrip().replace('lat0','').replace('lon0','').split(',')
lat0 = float(latitudeLongitudeArr[0])
lon0 = float(latitudeLongitudeArr[1])
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
global_position = self.global_position
# TODO: convert to current local position using global_to_local()
north, east, down = global_to_local(global_position,self.global_home)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
grid_start = (int(np.ceil(north - north_offset)), int(np.ceil(east - east_offset)))
# TODO: convert start position to current position rather than map center
if len(self.target_global_position) == 0:
lon0t = (lon0 + self.targetlongitudeDelta)
lat0t = (lat0 + self.targetlatitudeDelta)
print('Target Longitude and latitude: ', lon0t, lat0t)
northg, eastg, downg = global_to_local([lon0t,lat0t,self.global_home[2]],self.global_home)
else:
self.target_global_position[2] = self.global_home[2]
northg, eastg, downg = global_to_local(self.target_global_position,self.global_home)
# Set goal as some arbitrary position on the grid
grid_goal = (int(np.ceil(northg - north_offset)),int(np.ceil(eastg - east_offset)))
# TODO: adapt to set goal as latitude / longitude position and convert
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
path = collinearity_prune(path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# Read lat0, lon0 from colliders into floating point values
lat0, lon0 = read_home('colliders.csv')
# Set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# Convert to current local position using global_to_local()
current_position = [self._longitude, self._latitude, self._altitude]
self._north, self._east, self._down = global_to_local(
current_position, self.global_home)
print('global home: {0}, global pos: {1}, local pos: {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset: {0}, East offset: {1}".format(
north_offset, east_offset))
# Define starting point on the grid
grid_start = (int(np.ceil(-north_offset + self.local_position[0])),
int(np.ceil(-east_offset + self.local_position[1])))
# Set goal as some arbitrary position on the grid
local_target_position = global_to_local(self.global_target_position,
self.global_home)
grid_goal = (int(np.ceil(-north_offset + local_target_position[0])),
int(np.ceil(-east_offset + local_target_position[1])))
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# Prune path to minimize number of waypoints
pruned_path = prune_path(path)
print('Length of pruned path: {}'.format(len(pruned_path)))
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# Send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def main():
print('main')
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
grid, north_offset, east_offset = planning_utils.create_grid(data, 5, 5)
print('data: {}'.format(grid.shape))
#grid_start = (-north_offset, -east_offset)
# grid_goal = (-north_offset + 50, -east_offset + 50)
grid_start = planning_utils.random_free_location_in_grid(grid)
grid_goal = planning_utils.random_free_location_in_grid(grid)
print('grid_start: {} grid_goal: {}'.format(grid_start, grid_goal))
path, path_cost = planning_utils.a_star(grid, planning_utils.heuristic,
grid_start, grid_goal)
print('path: {} path_cost: {}'.format(len(path), path_cost))
# path = planning_utils.prune_path(path)
# print("pruned_path: {}".format(len(path)))
smoothed_path = planning_utils.smooth_path(grid, path)
print("smoothed_path: {}".format(len(smoothed_path)))
greedy_path = planning_utils.greedy_smooth_path(grid, smoothed_path)
print('greedy_path: {}'.format(len(greedy_path)))
if True:
plt.imshow(grid, cmap='Greys', origin='lower')
for i in range(len(path) - 1):
p1 = path[i]
p2 = path[i + 1]
plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'b-')
for i in range(len(smoothed_path) - 1):
p1 = smoothed_path[i]
p2 = smoothed_path[i + 1]
plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'g-')
for i in range(len(greedy_path) - 1):
p1 = greedy_path[i]
p2 = greedy_path[i + 1]
plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'r-')
# plot in x and y
plt.plot(grid_start[1], grid_start[0], 'rx')
plt.plot(grid_goal[1], grid_goal[0], 'gx')
# plt.plot([grid_start[1], grid_goal[1]], [grid_start[0], grid_goal[0]], 'g-')
# zoom up to the area
# plt.xlim((grid_start[1] - 10, grid_goal[1] + 10))
# plt.ylim((grid_start[0] - 10, grid_goal[0] + 10))
plt.xlabel('EAST')
plt.ylabel('NORTH')
#plt.show()
plt.rcParams['figure.figsize'] = 12, 12
plt.savefig('misc/path2.png')
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 3
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
# TODO: set home position to (lat0, lon0, 0)
# TODO: retrieve current global position
# TODO: convert to current local position using global_to_local()
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=3)
# Determine offsets between grid and map
north_offset = int(np.abs(np.min(data[:, 0])))
east_offset = int(np.abs(np.min(data[:, 1])))
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define a grid for a particular altitude and safety margin around obstacles
grid = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
# Define starting point on the grid (this is just grid center)
grid_start = (north_offset, east_offset)
# TODO: convert start position to current position rather than map center
#start = (int(current_local_pos[0]+north_offset), int(current_local_pos[1]+east_offset))
# Set goal as some arbitrary position on the grid
grid_goal = (north_offset + 10, east_offset + 10)
# TODO: adapt to set goal as latitude / longitude position and convert
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [(p[0] - north_offset, p[1] - east_offset,
TARGET_ALTITUDE + 1) for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
colliders_file = 'colliders.csv'
# TODO: read lat0, lon0 from colliders into floating point values
lat0, lon0 = read_home(colliders_file)
print(f'Home lat : {lat0}, lon : {lon0}')
# # # TODO: set home position to (lat0, lon0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
local_north, local_east, local_down = global_to_local(self.global_position, self.global_home)
print(f'Local => north : {local_north}, east : {local_east}, down : {local_down}')
# TODO: convert to current local position using global_to_local()
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt(colliders_file, delimiter=',', dtype='Float64', skiprows=3)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
grid_start_north = int(np.ceil(local_north - north_offset))
grid_start_east = int(np.ceil(local_east - east_offset))
grid_start = (grid_start_north, grid_start_east)
# TODO: convert start position to current position rather than map center
# Set goal as some arbitrary position on the grid
goal_north, goal_east, goal_alt = global_to_local(self.goal_global_position, self.global_home)
grid_goal = (int(np.ceil(goal_north - north_offset)), int(np.ceil(goal_east - east_offset)))
# TODO: adapt to set goal as latitude / longitude position and convert
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
path = collinearity_prune(path)
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [[int(p[0] + north_offset), int(p[1] + east_offset), TARGET_ALTITUDE, 0] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
lat_lon_data = np.loadtxt('colliders.csv', usecols=(0, 1), delimiter=',', dtype='str')
# read lat0, lon0 from colliders into floating point values
lat0 = float(lat_lon_data[0,0][5:])
lon0 = float(lat_lon_data[0,1][5:])
# set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# retrieve current global position
current_global_position = self.global_position
# convert to current local position using global_to_local()
current_local_position = global_to_local(current_global_position, self.global_home)
print("current local position: ", current_local_position[0], " ", current_local_position[1])
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# convert start position to current position rather than map center
grid_start = (int(current_local_position[0])-north_offset, int(current_local_position[1])-east_offset)
# set goal as latitude / longitude position and convert
lon_goal = -122.3961
lat_goal = 37.7940
goal_global = np.array([lon_goal, lat_goal, 0])
goal_local = global_to_local(goal_global, self.global_home)
grid_goal = (int(goal_local[0])-north_offset, int(goal_local[1])-east_offset)
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# prune path to minimize number of waypoints
pruned_path = prune_path(path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
file = open('colliders.csv', 'r')
reader = csv.reader(file)
header = next(reader)
lat0, lon0 = [float(header[i].split()[1]) for i, j in enumerate(header)]
# TODO: set home position to (lon0, lat0, 0)
global_home = (lon0, lat0, 0)
print("global home is ", global_home)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
global_position = (self._longitude, self._latitude, self._altitude)
# print(global_position)
# TODO: convert to current local position using global_to_local()
local_position = global_to_local(global_position, global_home)
local_north, local_east, local_down = local_position
print(local_position)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
# TODO: convert start position to current position rather than map center
grid_start = (-north_offset + int(local_north), -east_offset + int(local_east))
# Set goal as some arbitrary position on the grid
# TODO: adapt to set goal as latitude / longitude position and convert
goal_north, goal_east, goal_alt = global_to_local([-122.400150, 37.796005, TARGET_ALTITUDE], self.global_home)
grid_goal = (int(np.ceil((-north_offset + goal_north))), int(np.ceil(-east_offset + goal_east)))
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
pruned_path = prune_path(path)
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
print("Searching for a path ...")
self.flight_state = States.PLANNING
self.target_position[2] = self.TARGET_ALTITUDE
# Read lat0, lon0 from colliders into floating point values, and set home position
with open('colliders.csv', 'r') as f:
latlon_string = f.readline()
f.close()
m = re.search('lat0 (?P<lat>-*\d+.\d+), lon0 (?P<lon>-*\d+.\d+)\n',
latlon_string)
lat0, lon0 = [m.group('lat'), m.group('lon')]
self.set_home_position(lon0, lat0, 0)
# Convert current posiiton to local frame
local_position = global_to_local(self.global_position,
self.global_home)
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data,
self.TARGET_ALTITUDE,
self.SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
grid_start = local_to_grid(local_position, north_offset, east_offset)
# Set goal as some arbitrary position on the grid
local_goal = global_to_local(self.GOAL_LATLON, self.global_home)
grid_goal = local_to_grid(local_goal, north_offset, east_offset)
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
pruned_path = prune(path)
# Convert path to waypoints
print("Path computed, publishing waypoints...")
waypoints = [[
p[0] + north_offset, p[1] + east_offset, self.TARGET_ALTITUDE, 0
] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
self.send_waypoints()
def plan_path(self):
def local_to_grid(local_coord):
"""
Convert a local frame to a grid frame location
"""
return (int(np.ceil(local_coord[0] - north_offset)),
int(np.ceil(local_coord[1] - east_offset)))
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
self.load_home('colliders.csv')
local = global_to_local(self.global_position, self.global_home)
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define starting point on the grid
grid_start = local_to_grid(local)
# Set goal as some arbitrary position on the grid
grid_goal = (grid_start[0] + 10, grid_start[1] - 10
) #(-north_offset + 10, -east_offset + 10)
# TODO: adapt to set goal as latitude / longitude position and convert
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# read lat0, lon0 from colliders into floating point values
with open('colliders.csv') as f:
first_line = f.readline()
items = first_line.split(',')
lat0 = float(items[0].split()[1])
lon0 = float(items[1].split()[1])
# set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# retrieve current global position
print ("current global position:", self.global_position)
local_pos = global_to_local(self.global_position, self.global_home)
print("current local position", local_pos)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Set goal: latitude / longitude
goal_lon = -122.398470
goal_lat = 37.793445
local_goal = global_to_local((goal_lon, goal_lat, 0), self.global_home)
grid_start = (int(local_pos[0])-north_offset, int(local_pos[1])-east_offset)
grid_goal = (int(local_goal[0]-north_offset), int(local_goal[1]-east_offset))
# Adapt to set goal as position and convert
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
if grid_start[0] != grid_goal[0] or grid_start[1] != grid_goal[1]:
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# prune path to minimize number of waypoints
pruned_path = self.prune_path(path)
print ("prunned path is :", pruned_path)
if len(pruned_path):
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 6
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
#posX,posY,posZ,halfSizeX,halfSizeY,halfSizeZ
# TODO: read lat0, lon0 from colliders into floating point values
lat_lon = pd.read_csv('colliders.csv')
lat = float(lat_lon.columns[0][5:])
lon = float(lat_lon.columns[1][6:])
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position = (lon, lat, 0)
print("home position set to {0}, {1}".format(lon,lat))
# TODO: retrieve current global position
local_north, local_east, local_down = global_to_local(self.global_position, self.global_home)
print("local position set to {0}, {1}, {2}".format(local_north,local_east,local_down))
# TODO: convert to current local position using global_to_local()
local_coordinates_NED = global_to_local( self.global_position, self.global_home)
print('local_coordinates_NED {0}'.format(local_coordinates_NED))
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64',skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
#grid_start = (-north_offset, -east_offset)
# TODO: convert start position to current position rather than map center
grid_start = (int(np.ceil(local_north - north_offset)), int(np.ceil(local_east - east_offset)))
# Set goal as some arbitrary position on the grid
goal = global_to_local(self.goal_position,self.global_home)
grid_goal = ( int(np.ceil(goal[0]-north_offset)), int(np.ceil(goal[1]-east_offset)))
# TODO: adapt to set goal as latitude / longitude position and convert
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
print('path: ',path)
pruned_path = prune_path(path)
print('pruned path: ',pruned_path)
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def test():
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
grid, _, _ = create_grid(data, 5, 5)
grid_start = (316, 445)
grid_goal = (268, 790)
find_path_using_grid(grid, grid_start, grid_goal)
find_path_using_graph(grid, grid_start, grid_goal)
exit(0)
def _get_grid_and_centers(self, drone_altitude):
grid, _ = create_grid(self._cdata.data, drone_altitude,
self._safety_distance)
centers = self._cdata.data[:, 0:2].tolist()
height = self._cdata.data[:,
2] + self._cdata.data[:,
5] + self._safety_distance
centers = list(compress(centers, (height > drone_altitude).tolist()))
return grid, centers
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 20
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# Setting the home position based on the latitude and longitude given
# on the first line of colliders.csv
file = 'colliders.csv'
latlon = []
latlon = open(file).readline().split()
lat_h, lon_h = float(latlon[1].replace(',', '')), float(latlon[3])
self.set_home_position(lon_h, lat_h, 0.0)
global_position = self.global_position
# creating a 2D grid from the colliders.csv
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
# Acquiring start and goal position
grid_start_latlon = (self._longitude, self._latitude, self._altitude)
grid_start_l = global_to_local(grid_start_latlon, self.global_home)
grid_start = (int(grid_start_l[0]) - north_offset,
int(grid_start_l[1]) - east_offset)
goal_lon = input('Goal longitude: ') #goal_lon = -122.396071
goal_lat = input('Goal latitude: ') #goal_lat = 37.793077
goal_position = (float(goal_lon), float(goal_lat), 0.0)
grid_goal_l = global_to_local(goal_position, self.global_home)
grid_goal = (int(grid_goal_l[0]) - north_offset,
int(grid_goal_l[1]) - east_offset)
print('Local Start: ', grid_start)
print('Local Goal: ', grid_goal)
# path planning and pruning using collinearity check and bresenham
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
pruned_path = prune_path(path, grid)
print('Path length: ', len(pruned_path))
print(pruned_path)
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
lat0, lon0 = self.get_global_home_from_file()
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
# TODO: convert to current local position using global_to_local()
local_position = global_to_local(self.global_position, self.global_home)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
# TODO: convert start position to current position rather than map center
grid_start = (int(local_position[0])-north_offset, int(local_position[1])-east_offset)
# Set goal as some arbitrary position on the grid
# grid_goal = (grid_start[0] + 10, grid_start[1] + 10)
# TODO: adapt to set goal as latitude / longitude position and convert
goal_geo = np.array([-122.401763, 37.795809, 0])
local_goal = global_to_local(goal_geo, self.global_home)
grid_goal = (int(local_goal[0])-north_offset, int(local_goal[1])-east_offset)
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
print('Path length: {0}'.format(len(path)))
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
"""
I have implemented both Collinearity and Bresenham methods to prune the path.
You can call either below to test
"""
# pruned_path = prune_coll(path)
pruned_path = prune_bres(path, grid)
print('Pruned-path length: {0}'.format(len(pruned_path)))
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def get_grid_start_and_goal_position(
self,
input_data,
target_altitude,
safety_distance,
use_gedoetic_frame_for_goal_position=True):
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = plu.create_grid(
input_data, target_altitude, safety_distance)
# retrieve current global position
current_global_position = [
self._longitude, self._latitude, self._altitude
]
# convert to current local position relative to new global home based on colliders.csv using global_to_local()
curr_local_position_rel_new_global_home = global_to_local(
current_global_position, self.global_home)
# Define starting point on the grid
grid_start = (int(np.floor(
curr_local_position_rel_new_global_home[0])),
int(np.floor(
curr_local_position_rel_new_global_home[1])),
curr_local_position_rel_new_global_home[2])
if use_gedoetic_frame_for_goal_position:
goal_position_valid = False
count = 0
max_proposals_allowed = 30
while not goal_position_valid or count < max_proposals_allowed:
goal_lon = np.random.uniform(-122.42, -122.3)
goal_lat = np.random.uniform(37.7, 37.82)
goal_position_valid, local_goal_position = self.is_goal_position_within_grid_boundaries(
goal_lon, goal_lat, 0.0, north_offset, east_offset)
if (goal_position_valid):
grid_goal = (int(np.floor(local_goal_position[0])),
int(np.floor(local_goal_position[1])),
-target_altitude)
break
else:
count += 1
if not goal_position_valid:
print("no valid goal position found")
else:
# Set goal as some arbitrary position on the grid
goal_north_position = np.random.randint(0, 2 * abs(north_offset))
goal_east_position = np.random.randint(0, 2 * abs(east_offset))
grid_goal = (goal_north_position, goal_east_position,
-target_altitude)
return grid, north_offset, east_offset, grid_start, grid_goal
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
# TODO: set home position to (lon0, lat0, 0)
if (self.set_home):
self.set_home_position(self.init_long, self.init_lat, 0)
# TODO: retrieve current global position
global_position = self.global_position
# TODO: retrieve current global position
north, east, att = global_to_local(self.global_position,
self.global_home)
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(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(north - north_offset), int(east - east_offset))
#new_start=self.sub_plan(grid_start, -122.397632,37.793108,2,data)
#point2=self.sub_plan(new_start, -122.398017,37.795503,100,data)
#point3=self.sub_plan(point2, -122.394965,37.796663,200,data)
old_start = grid_start
alt = 2
for i in range(len(self.points)):
new_start = self.sub_plan(old_start, self.points[i][0],
self.points[i][1], self.alts[i], data)
alt = alt + 100
old_start = new_start
#new_start=self.sub_plan(grid_start, self.points[0][0],self.points[0][1],2,data)
#point2=self.sub_plan(new_start, self.points[1][0],self.points[1][1],100,data)
#point3=self.sub_plan(point2, self.points[2][0],self.points[2][1],200,data)
self.old_start = old_start
def loadGrid(safetyDistance=5, targetAltitude=3):
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=3)
# Determine offsets between grid and map
north_offset = int(np.abs(np.min(data[:, 0])))
east_offset = int(np.abs(np.min(data[:, 1])))
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define a grid for a particular altitude and safety margin around obstacles
grid = create_grid(data, targetAltitude, safetyDistance)
return grid, east_offset, north_offset
def get_main_plan(global_home,
goal_lat,
goal_lon,
current_local_pos,
safety_distance=3,
target_altitude=5,
filename='colliders.csv'):
"""
returns a medial axis global plan.
also returns data, north_offset & east_offset for reuse later
"""
# Read in obstacle map
data = np.loadtxt(filename, delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, nmin, emin = create_grid(data, target_altitude, safety_distance)
north_offset = -1 * nmin
east_offset = -1 * emin
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_pos[0] + north_offset),
int(current_local_pos[1] + east_offset))
local_goal = global_to_local((goal_lon, goal_lat, 0), global_home)
grid_goal = (int(local_goal[0] + north_offset),
int(local_goal[1] + east_offset))
print("Local Goal: {0}" % local_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
# see planning_utils.py
print('Local Start and Goal: ', grid_start, grid_goal)
# Medial Axis
path, _ = get_medial_axis_path(grid, grid_goal, grid_start)
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
# step 1: prune collinear points from path
print("number of waypoints: %d" % len(path))
path = prune_path(path)
print("number of waypoints after pruning: %d" % len(path))
print("number of waypoints after collinearity pruning: %d" % len(path))
path = prune_path_using_raytracing(grid, path)
print("number of waypoints after raytracing pruning: %d" % len(path))
# Convert path to waypoints
waypoints = [[p[0] - north_offset, p[1] - east_offset, target_altitude, 0]
for p in path]
return waypoints, data, north_offset, east_offset
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 20
SAFETY_DISTANCE = 10
self.target_position[2] = TARGET_ALTITUDE
#initial localtion in world coordinate frame
global_home =(-122.397450,37.792480)
lat0 ,lon0 = global_home[1],global_home[0]
#intialize the drone position to the global home then that is going to referance the local frame from there local_home (0,0,0)
self.set_home_position(lon0,lat0,0)
#self.set_home_as_current_position()
global_loc = self.global_position
local_position = global_to_local(global_loc , global_home)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
grid_start = (-north_offset, -east_offset)
grid_start=(int(local_position[0] - north_offset) ,int(local_position[1] -east_offset) )
# Set goal as some arbitrary position on the grid
grid_goal = (-north_offset + 750, -east_offset + 370)
grid_goal =(int(750) ,int( 370) )
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
cleared_path = prune_path(path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in cleared_path]
# Set self.waypoints
self.waypoints = waypoints
# send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 4
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
pos = open("colliders.csv").readline().split(",")
lat0 = float(pos[0].replace("lat0 ",""))
lon0 = float(pos[1].replace("lon0 ",""))
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0.0)
# TODO: retrieve current global position
#print('global position {}'.format(self.global_position))
# TODO: convert to current local position using global_to_local()
local_pos = global_to_local(self.global_position, self.global_home)
print('global home {0}, \n position {1}, \n local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# convert start position to current position rather than map center
grid_start = [int(np.ceil(local_pos[0] - north_offset)), int(np.ceil(local_pos[1] - east_offset))]
# Set goal as some arbitrary position on the grid
#grid_goal = (-north_offset + 300, -east_offset + 300)
# adapt to set goal as latitude / longitude position and convert
local_goal_pos = global_to_local(self.global_goal_position, self.global_home)
grid_goal = (int(np.ceil(local_goal_pos[0] - north_offset)), int(np.ceil(local_goal_pos[1] - east_offset)))
print(grid_goal)
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# prune path to minimize number of waypoints
reduced_path = prune_path(path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in reduced_path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def create_graph(data, drone_altitude, safety_distance):#STARTHERE
"""
Returns a graph from the colliders `data`.
"""
# Find grid and offsets.
grid, north_offset, east_offset = create_grid(data, drone_altitude, safety_distance)
# Find object centers.
centers = get_object_centers(data, north_offset, east_offset, drone_altitude, safety_distance)
# Create Voronoid from centers
voronoi = Voronoi(centers)
# Find open edges
edges = find_open_edges_voronoi(voronoi, grid)
# Create graph.
return (create_graph_from_edges(edges), north_offset, east_offset)
def probabilistic_roadmap(self,grid_start,grid_goal):
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
SAMPLE_NUMBER =30
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("random x" , np.random.choice(grid.shape[0],SAMPLE_NUMBER))
print("random y" , np.random.choice(grid.shape[1],SAMPLE_NUMBER))
print("Generating random points")
random_points = [ (x,y) for x in np.random.choice(grid.shape[0],SAMPLE_NUMBER) for y in np.random.choice(grid.shape[1],SAMPLE_NUMBER)]
random_points.append(grid_start)
random_points.append(grid_goal)
print("random_points = ",random_points)
print("length of random_points = ",len(random_points))
print("Filtering to free space points")
free_space_random_points = [ p for p in random_points if grid[p[0],p[1]] == 0 ]
print("free_space_random_points = " , free_space_random_points)
print("length of free_space_random_points = " , len(free_space_random_points))
print("Generating valid actions map")
valid_actions_map ={}
for p in free_space_random_points:
valid_actions_map[p] = []
for p in free_space_random_points:
for q in free_space_random_points:
if p != q:
pathClear = True
cells = list(bresenham(p[0], p[1], q[0], q[1]))
for cell in cells:
if grid[ cell[0],cell[1] ] == 1:
pathClear = False
break
if pathClear:
valid_actions_map[p].append( [ q[0]-p[0] , q[1]-p[1] , np.sqrt( (q[0]-p[0])**2 + (q[1]-p[1])**2 ) ] )
return valid_actions_map
