def planMedialPath(grid, skeleton, start=(218, 371), goal=(419, 520), diagonals=False):
medial_start, medial_goal = find_start_goal(skeleton, start, goal)
print("medial_start : ", medial_start, "; medial_target : ", medial_goal)
medial_axis_grid = back_to_grid(skeleton)
path, cost = a_star(medial_axis_grid, heuristic, tuple(medial_start), tuple(medial_goal), diagonals)
return path, cost, medial_start, medial_goal
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = int(np.ceil(self.global_goal[2]))
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
obstacle_file = 'data/colliders.csv'
lat0, lon0 = getOrigin(obstacle_file)
self.set_home_position(lon0, lat0, 0)
local_home_ned = global_to_local(self.global_position, self.global_home)
print('home position set to [N : {:2f}, E : {:2f} D : {:2f}]'.format(
local_home_ned[0], local_home_ned[1], local_home_ned[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('data/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)
global_pos = (self._longitude, self._latitude, self._altitude)
global_home = self.global_home
local_pos = global_to_local(global_pos, global_home)
grid_start = (int(local_pos[0] - north_offset), int(local_pos[1] - east_offset))
# Set goal as some arbitrary position on the grid
local_goal_ned = global_to_local(self.global_goal, self.global_home)
grid_goal = (int(local_goal_ned[0] - north_offset), int(local_goal_ned[1] - east_offset))
skel = create_medial_axis(grid)
medial_start, medial_goal = find_start_goal(skel, grid_start, grid_goal)
medial_grid = back_to_grid(skel)
# 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(medial_grid, heuristic, tuple(medial_start), tuple(medial_goal), self.diagonal_search)
# Prune path
path = prunePath(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()
plt.xlabel('EAST')
plt.ylabel('NORTH')
plt.show()
def planMedialPath(grid, skeleton, start=(218, 371), goal=(419, 520), diagonals=False):
medial_start, medial_goal = find_start_goal(skeleton, start, goal)
print("medial_start : ", medial_start, "; medial_target : ", medial_goal)
medial_axis_grid = back_to_grid(skeleton)
path, cost = a_star(medial_axis_grid, heuristic, tuple(medial_start), tuple(medial_goal), diagonals)
return path, cost, medial_start, medial_goal
if __name__ == "__main__":
grid = generateGrid('data/colliders.csv', drone_altitude=50, safe_distance=2)
skeleton = create_medial_axis(grid)
# plotMedialAxis(grid, skeleton)
diagonals = True
path, cost, medial_start, medial_goal = planMedialPath(grid, skeleton)
path2, cost2 = a_star(grid, heuristic, tuple(medial_start), tuple(medial_goal), diagonals=diagonals)
if path and path2:
prunned_path = prunePath(path)
prunned_path2 = prunePath(path2)
plotPath(grid, skeleton, prunned_path, prunned_path2)
elif path2:
prunned_path = prunePath(path2)
plotPath(grid, skeleton, prunned_path)
else:
plotMedialAxis(grid, skeleton, medial_start, medial_goal)
def planPath(grid, start=(25, 100), goal=(650, 500), diagonals=False):
path, cost = a_star(grid, heuristic, start, goal, diagonals)
return path, cost