def __init__(self,
boundary_list,
obstacles,
alt_threshold=10,
alt_default=30):
self.boundary_list = boundary_list
self.boundary_poly = makeBoundaryPoly(self.boundary_list)
self.obstalces = obstacles
self.altitude_threshold = alt_threshold
self.altitude_default = alt_default
def __init__(self, flight_boundaries, obstacles, waypoint_distance=50.):
"""
Initialize the variables for the planning algorithm.
The flight boundaries are converted from a list to shapely object to allow checking if points are within the boundaries
Parameters
----------
flight_boundaries : list of NED class
The points that define the fly zone
obstacles : list of NED class
The location and description of the obstacles
waypoint_distance : float
The distance each point in the lawn mower path should be from its neighbors
"""
self.flight_boundaries = flight_boundaries
self.flight_poly = makeBoundaryPoly(self.flight_boundaries)
self.obstalces = obstacles
self.waypoint_distance = waypoint_distance
self.height = 30.
def __init__(self, boundary_list, obstacles):
self.boundary_list = boundary_list
self.boundary_poly = makeBoundaryPoly(self.boundary_list)
self.obstalces = obstacles
import sys
sys.path.append('..')
from tools.tools import makeBoundaryPoly, convert, #collisionCheck
from messages.ned import msg_ned
from payloadPlanner import PayloadPlanner
import numpy as np
#List of obastacles and boundaries
obstaclesList = []
boundariesList = []
boundariesList.append(msg_ned(-1000, 1000))
boundariesList.append(msg_ned(-1000, -1000))
boundariesList.append(msg_ned(1000, -1000))
boundariesList.append(msg_ned(1000, 1000))
boundaryPoly = makeBoundaryPoly(boundariesList)
dropLocation = np.array([0.0, 0.0, 0.0])
wind_vel = 5.36
wind = np.array([
wind_vel * np.cos(np.radians(30)), -wind_vel * np.sin(np.radians(30)), 0.0
])
test = PayloadPlanner(dropLocation, obstaclesList, boundariesList,
boundaryPoly, wind)
test.drop_altitude = 41.611332
test.chi_offset = np.pi / 6.
test.time_delay = 0.0
test.time_to_open_parachute = 0.5
result = test.plan(wind)
test.plot()
print(test.NED_release_location)
def plan(self,
search_boundaries,
current_pos=msg_ned(0., 0., -100.),
clearance=10,
visualize=True):
"""
Creates a lawn mower path over the search area
Plans a lawnmower path that covers the search area and does not get within the clearance value of the fly zone.
Parameters
----------
search_boundaries : list of NED class
The boundaries that define the search area
current_pos : NED class
The current position of the airplane
clearance : float
The closest distance a waypoint is permitted to be to the fly zone boundaries
visualize : boolean
For debugging only. If true, the planned points and boundaries will be displayed
Returns
-------
final_waypoints : list of NED class
The points that define the planned search path
Warnings
--------
The altitude of the search path is set to the attitude of the current position.
Obstacles are not currently accounted for in planning the path.
If visualize is true, the code looks like it stops and does not continue executing until the plot is closed.
"""
self.search_boundaries = makeBoundaryPoly(search_boundaries)
# Find the box that fits the search area
num_points = len(search_boundaries)
n_pos = np.zeros(num_points)
e_pos = np.zeros(num_points)
for i in np.arange(num_points):
n_pos[i] = search_boundaries[i].n
e_pos[i] = search_boundaries[i].e
w_bound = np.min(e_pos) - self.waypoint_distance
e_bound = np.max(e_pos) + self.waypoint_distance
n_bound = np.max(n_pos) + self.waypoint_distance
s_bound = np.min(n_pos) - self.waypoint_distance
search_box = [
msg_ned(n_bound, e_bound),
msg_ned(s_bound, e_bound),
msg_ned(s_bound, w_bound),
msg_ned(n_bound, w_bound)
]
search_box_p = makeBoundaryPoly(search_box)
# Initialize variables for creating the search path
all_points = []
cur_pos = msg_ned(n_bound, w_bound, current_pos.d, -33.0)
direction = 1 # Positive advances the path to the right, negative moves the path to the left. Changes each time it needs to turn around to stay in the search area
# Create the lawn mower path
all_points.append(
msg_ned(cur_pos.n, cur_pos.e, current_pos.d)
) #Start path at the North-East corner of the bounding box
while cur_pos.n >= s_bound: #Stop adding points once we are below the bounding box
while cur_pos.e >= w_bound and cur_pos.e <= e_bound: #Add points on an East-West or West-East line until we leave the bounding box
cur_pos.e = cur_pos.e + direction * self.waypoint_distance
all_points.append(msg_ned(cur_pos.n, cur_pos.e, -self.height))
direction = -direction #When we leave the bounding box, turn around
cur_pos.n = cur_pos.n - self.waypoint_distance #Advance path one step in a South direction
all_points.append(msg_ned(cur_pos.n, cur_pos.e, -self.height))
while cur_pos.e <= w_bound or cur_pos.e >= e_bound: #Bring the path back inside the bounding box area
cur_pos.e = cur_pos.e + direction * self.waypoint_distance
all_points.append(msg_ned(cur_pos.n, cur_pos.e, -self.height))
# Eliminate points that are too close to the flight boundary or too far from the search area
final_waypoints = []
for waypoint in all_points:
waypoint_circle_large = Point(waypoint.n, waypoint.e).buffer(
clearance) # Creates a point with a radius of clearance
waypoint_circle_small = Point(waypoint.n, waypoint.e).buffer(
self.waypoint_distance
) # Point with a radius of waypoint_distance
if waypoint_circle_large.within(
self.flight_poly
) and waypoint_circle_small.intersects(
self.search_boundaries
): # Check if the point is too close or far from boundaries
final_waypoints.append(
msg_ned(waypoint.n, waypoint.e, -self.height))
# Plot the planned points and all boundaries
if visualize:
fig, ax = plt.subplots()
for point in final_waypoints:
ax.scatter(point.e, point.n, c='k')
for point in self.flight_boundaries:
ax.scatter(point.e, point.n, c='r')
for point in search_boundaries:
ax.scatter(point.e, point.n, c='g')
plt.show()
return final_waypoints