def calcSupportingPoints(self):
"""
given a command release location plot X points for a straight line of deployment run
Returns
-------
waypoints : list of NED class objects
A list of NED class objects where each object describes the NED position of each waypoint
"""
length = self.supporting_points * 2 + 1
self.waypoints_array = np.zeros((length, 3))
# North and East difference at course angle for given distance between waypoints
dNorth = self.waypoint_spread * np.cos(self.course_command)
dEast = self.waypoint_spread * np.sin(self.course_command)
# create waypoints before the release location
for ii in range(self.supporting_points):
self.waypoints_array[ii] = self.NED_release_location - (
self.supporting_points - ii) * np.array([dNorth, dEast, 0.0])
# add the release location waypoint
self.waypoints_array[
self.supporting_points] = self.NED_release_location
# create waypoints after the release location
for ii in range(self.supporting_points):
self.waypoints_array[ii + self.supporting_points +
1] = self.NED_release_location + (
ii + 1.0) * np.array([dNorth, dEast, 0.0])
waypointsList = []
for ii in range(length):
waypointsList.append(
msg_ned(self.waypoints_array[ii, 0],
self.waypoints_array[ii, 1], self.waypoints_array[ii,
2]))
return waypointsList
def clear_waypoints(self, req):
print("Clearing waypoints")
rospy.wait_for_service('waypoint_path')
#Set up a service call to poll the interop server
waypoint_update = rospy.ServiceProxy('waypoint_path', NewWaypoints)
waypoints = []
new_point = Waypoint()
new_point.w = [
self.DEFAULT_POS[0], self.DEFAULT_POS[1], self.DEFAULT_POS[2]
]
new_point.clear_wp_list = True
waypoints.append(new_point)
#Send the service call with the desired mission type number
resp = waypoint_update(waypoints)
# set the last waypoint to the origin
self.last_waypoint = msg_ned(*self.DEFAULT_POS)
print("Waypoints cleared")
return True
def convert_waypoints(ref_pos, msg):
"""
Converts the waypoints obtained from the interop server to NED coordinates
This function doesn't care about what mission is being run, it just gets the waypoints
which can be for the drop location, flight location, or search boundaries, and converts them
Parameters
----------
msg : JudgeMission message
The message received from the interop server
ref_pos : list
The reference latitude, longitude, and height (in m)
Returns
-------
waypoint_list : list of NED classes
List describing the NED position of each waypoint
"""
ref_lat = ref_pos[0]
ref_lon = ref_pos[1]
ref_h = ref_pos[2]
waypoint_list = []
for i in msg.waypoints:
wpt_NED = convert(ref_lat, ref_lon, ref_h, i.point.latitude, i.point.longitude, i.point.altitude)
waypoint_list.append(msg_ned(wpt_NED[0], wpt_NED[1], wpt_NED[2]))
return waypoint_list
def convert_boundaries(ref_pos, msg):
"""
Converts the boundary points obtained from the interop server to NED coordinates
Parameters
----------
msg : JudgeMission message
The message received from the interop server
ref_pos : list
The reference latitude, longitude, and height (in m)
Returns
-------
list
a list of lists describing all the boundary points
each inner list has the North location, East location, and Down location of a single boundary point
all measurements are in meters
"""
ref_lat = ref_pos[0]
ref_lon = ref_pos[1]
ref_h = ref_pos[2]
boundary_list = []
for i in msg.boundaries:
bnd_NED = convert(ref_lat, ref_lon, ref_h, i.point.latitude, i.point.longitude, i.point.altitude)
boundary_list.append(msg_ned(bnd_NED[0], bnd_NED[1]))
boundary_poly = makeBoundaryPoly(boundary_list)
return boundary_list, boundary_poly
def convert_obstacles(ref_pos, msg):
"""
Converts the obstacles from the rospy message to a list of NED classes
Parameters
----------
msg : JudgeMission message
The message received from the interop server
ref_pos : list
The reference latitude, longitude, and height (in m)
Returns
-------
obstacle_list : list of NED classes
Describes the position and height of each obstacle
"""
ref_lat = ref_pos[0]
ref_lon = ref_pos[1]
ref_h = ref_pos[2]
obstacle_list = []
for i in msg.stationary_obstacles:
obs_NED = convert(ref_lat, ref_lon, ref_h, i.point.latitude, i.point.longitude, i.point.altitude)
obstacle_list.append(msg_ned(obs_NED[0], obs_NED[1], i.cylinder_height, i.cylinder_radius))
return obstacle_list
def shortestPath(self, tree, endNode):
"""RRT class function that takes in a tree with successful paths and finds which one is the shortest
Parameters
----------
tree : float
An Nx7 array of N leaves in this format: N, E, D, cost, parentIndex, connectsToGoalFlag, chi
endNode : msg_ned
The ending waypoint.
Returns
-------
path : msg_ned
An array of waypoints that expresses the shortest (but not smoothed), successful path from one waypoint
to another
"""
# Find the leaves that connect to the end node
connectedNodes = []
for i in range(0, np.size(tree, 0)):
if tree[i, 5] == 1:
connectedNodes.append(i)
# Find the path with the shortest distance (could find a different heuristic for choosing which path to go with,
# especially because we are going to shorten the path anyway??). Choose shortest after smoothing?? Or choose for
# least turns.
minIndex = np.argmin(tree[connectedNodes, 3])
minIndex = connectedNodes[minIndex]
path = []
path.append(endNode)
path.append(
msg_ned(tree[minIndex, 0], tree[minIndex, 1], tree[minIndex, 2]))
parentNode = int(tree[minIndex, 4])
while parentNode > 0:
path.append(
msg_ned(tree[parentNode, 0], tree[parentNode, 1],
tree[parentNode, 2]))
parentNode = int(tree[parentNode, 4])
path.append(
msg_ned(tree[parentNode, 0], tree[parentNode, 1],
tree[parentNode, 2])) #This adds the starting point
# # The commented lines prints the shortest, but not yet smoothed, path
# if self.animate:
# self.drawPath(path,'r')
return path
def msg2wypts(message):
"""
Converts a ros message to a list of msg_ned classes
"""
waypoints = []
for point in message.waypoint_list:
waypoints.append(msg_ned(point.N, point.E, point.D))
return waypoints
def plan(self, waypoints):
point = Point(waypoints.n, waypoints.e).buffer(self.radius+self.clearance)
safe_point = True
if point.within(self.bound_poly):
for obs in self.obstacles:
if point.intersects(Point(obs.n, obs.e).buffer(obs.r)):
safe_point = False
if safe_point:
if waypoints.d > -45.0:
waypoints.d = -45.0
return([waypoints])
north = waypoints.n
east = waypoints.e
std = 50
count = 0
safe_point = False
while safe_point == False:
count = count + 1
n = np.random.normal(loc=north, scale=std)
e = np.random.normal(loc=east, scale=std)
print(n,e)
point = Point(n, e).buffer(self.radius+self.clearance)
safe_point = True
if point.within(self.bound_poly):
for obs in self.obstacles:
if point.intersects(Point(obs.n, obs.e).buffer(obs.r)):
safe_point = False
if np.mod(count,5) == 0:
std = std + 25
return([msg_ned(n,e,-45.0)])
from rrt import RRT
import numpy as np
import time
from messages.ned import msg_ned
if __name__ == '__main__':
show_animation = True
#List of obastacles and boundaries
obstaclesList = []
obstaclesList.append(msg_ned(25.,-25.,150.,20.))
obstaclesList.append(msg_ned(60.,60.,150.,20.))
# obstaclesList.append(msg_ned(50.,50.,75.,5.))
boundariesList = []
boundariesList.append(msg_ned(-100,100))
boundariesList.append(msg_ned(-100,50))
boundariesList.append(msg_ned(-75,50))
boundariesList.append(msg_ned(-75,0))
boundariesList.append(msg_ned(-100,0))
boundariesList.append(msg_ned(-100,-100))
boundariesList.append(msg_ned(100,-100))
boundariesList.append(msg_ned(100,100))
clearance = 5. # The minimum distance between the path and all obstacles
maxDistance = 10. # Max distance between each added leaf
maxIncline = .5 # Max slope the vehicle can climb or decend at
maxRelChi = np.pi / 2 # Max relative chi between leaves
iterations = 50 # How many sets of random points it will add each time until solution is found
resolution = 1.1 # The segment lengths checked for collisions
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
def update_task_callback(self, req):
"""
This function is called when the desired task is changed by the GUI. The proper mission is then called.
"""
self.task = req.mission_type
self.landing = False
# change this to determine how tight of turns the path will create
# 15*pi/16 seems reasonable for most tasks - large planning tasks will
# take an excessive amount of time if this is too low.
self.rrt.maxRelChi = 15 * np.pi / 16
# interop server doesn't provide landing info
if (self.task != JudgeMission.MISSION_TYPE_LAND):
mission_type, obstacles, boundary_list, boundary_poly, waypoints = tools.get_server_data(
self.task, self.ref_pos)
#Each task_planner class function should return a NED_list msg
#These classes can be switched out depending on the desired functionality
connect = False
if (self.task == JudgeMission.MISSION_TYPE_SEARCH):
rospy.loginfo('SEARCH TASK BEING PLANNED')
planned_points = self._plan_search.plan(waypoints)
elif (self.task == JudgeMission.MISSION_TYPE_DROP):
rospy.loginfo('PAYLOAD TASK BEING PLANNED')
# be more picky about tight turns while performing the payload drop
self.rrt.maxRelChi = 10 * np.pi / 16
try:
state_msg = rospy.wait_for_message("/state", State, timeout=10)
wind = np.array([state_msg.wn, state_msg.we, 0.])
except rospy.ROSException as e:
print("no state message received")
wind = np.array([0.0, 0.0, 0.0])
planned_points, drop_location = self._plan_payload.plan(wind)
rospy.set_param('DROP_LOCATION', drop_location)
elif (self.task == JudgeMission.MISSION_TYPE_LOITER):
rospy.loginfo('LOITER PLANNER TASK BEING PLANNED')
try:
pos_msg = rospy.wait_for_message("/state", State, timeout=1)
current_pos = msg_ned(pos_msg.position[0], pos_msg.position[1],
pos_msg.position[2])
except rospy.ROSException as e:
print("Loiter - No State msg recieved")
current_pos = msg_ned(*self.DEFAULT_POS)
planned_points = self._plan_loiter.plan(current_pos)
elif (
self.task == JudgeMission.MISSION_TYPE_WAYPOINT
): # This is the task that deals with flying the mission waypoints. We call it objective to avoid confusion with the waypoints that are used to define the drop flight path or search flight path
rospy.loginfo('OBJECTIVE PLANNER TASK BEING PLANNED')
planned_points = self._plan_objective.plan(waypoints)
connect = True
elif (self.task == JudgeMission.MISSION_TYPE_OFFAXIS):
rospy.loginfo('OFFAXIS PLANNER TASK BEING PLANNED')
planned_points = self._plan_offaxis.plan(waypoints)
elif (self.task == JudgeMission.MISSION_TYPE_LAND):
rospy.loginfo('LANDING PATH BEING PLANNED')
landing_msg = req.landing_waypoints
self.rrt.maxRelChi = 10 * np.pi / 16
if (len(landing_msg.waypoint_list) == 2):
try:
pos_msg = rospy.wait_for_message("/state",
State,
timeout=1)
curr_altitude = pos_msg.position[2]
except rospy.ROSException as e:
print("Landing - No State msg recieved")
curr_altitude = 0.0
landing_wypts = tools.msg2wypts(landing_msg)
planned_points = self._plan_landing.plan(
landing_wypts, curr_altitude)
else:
planned_points = [msg_ned(*self.DEFAULT_POS)]
print("No landing waypoints specified")
print(len(landing_msg.waypoint_list))
self.landing = True
elif (
self.task == JudgeMission.MISSION_TYPE_EMERGENT
): # I believe the emergent object is just within the normal search boundaries
pass
else:
rospy.logfatal(
'TASK ASSIGNED BY GUI DOES NOT HAVE ASSOCIATED PLANNER')
if self.last_exists == True:
print("Planning from last waypoint of previous path")
current_pos = self.last_waypoint
else:
print("Planning from origin")
current_pos = msg_ned(*self.DEFAULT_POS)
planned_points.insert(0, current_pos)
print("Starting RRT to find full path")
final_path = self.rrt.findFullPath(planned_points, connect=connect)
#Convert python NED class to rospy ned msg
wypts_msg = tools.wypts2msg(final_path, self.task)
self.planned_waypoints = final_path
self.init_mission_plotter()
self.mp.addPathway(final_path, "Planned pathway")
self.mp.show(block=True)
print(wypts_msg)
return wypts_msg
N = self.waypoints_array[:, 0]
E = self.waypoints_array[:, 1]
D = self.waypoints_array[:, 2]
clearance = 1.0
if collisionCheck(self.obstacles, self.boundariesPolygon, N, E, D,
clearance):
return True
else:
return False
if __name__ == '__main__':
#List of obastacles and boundaries
obstaclesList = []
obstaclesList.append(msg_ned(-350., 450., 50., 50.))
obstaclesList.append(msg_ned(300., -150., 100., 25.))
obstaclesList.append(msg_ned(-300., -250., 75., 75.))
boundariesList = []
home = [38.146269, -76.428164, 0.0]
bd0 = [38.146269, -76.428164, 0.0]
bd1 = [38.151625, -76.428683, 0.0]
bd2 = [38.151889, -76.431467, 0.0]
bd3 = [38.150594, -76.435361, 0.0]
bd4 = [38.147567, -76.432342, 0.0]
bd5 = [38.144667, -76.432947, 0.0]
bd6 = [38.143256, -76.434767, 0.0]
bd7 = [38.140464, -76.432636, 0.0]
bd8 = [38.140719, -76.426014, 0.0]
bd9 = [38.143761, -76.421206, 0.0]
def pointsAlongPath(self, startN, endN, stepSize, third_node=None, R=None):
""" RRT class function that takes two nodes and returns the N, E, and D position of many points along the line
between the two points spaced according to the step size passed in.
Parameters
----------
startN : msg_ned
The starting node
endN : msg_ned
The ending node
stepSize : double
The desired spacing between each point along the line between the two nodes
third_node : msg_ned or None
The next node in the waypoint path. If this is not None then the points will be along a fillet path
R : float or None
The minimum turn radius. If None, straight line path is used
Returns
-------
N : double
An np.array of the north position of points
E : double
An np.array of the east position of points
D : double
An np.array of the down position of points
"""
if third_node is None or R is None:
N = np.array([startN.n])
E = np.array([startN.e])
D = np.array([startN.d])
q = np.array(
[endN.n - startN.n, endN.e - startN.e, endN.d - startN.d])
L = np.linalg.norm(q)
q = q / L
w = np.array([startN.n, startN.e, startN.d])
for i in range(1, int(np.ceil(L / stepSize))):
w += stepSize * q
N = np.append(N, w.item(0))
E = np.append(E, w.item(1))
D = np.append(D, w.item(2))
N = np.append(N, endN.n)
E = np.append(E, endN.e)
D = np.append(D, endN.d)
else: #Plan fillet path
R = float(R)
N = np.array([])
E = np.array([])
D = np.array([])
start_node = np.array([[startN.n], [startN.e], [startN.d]])
mid_node = np.array([[endN.n], [endN.e], [endN.d]])
last_node = np.array([[third_node.n], [third_node.e],
[third_node.d]])
q_pre = (mid_node - start_node) / np.linalg.norm(mid_node -
start_node)
q_next = (last_node - mid_node) / np.linalg.norm(last_node -
mid_node)
np.seterr(all='raise')
if abs(np.matmul(-q_pre.T, q_next)) >= 1:
if np.matmul(-q_pre.T, q_next) > 0:
theta = 0
elif np.matmul(-q_pre.T, q_next) < 0:
theta = -np.pi
else:
theta = np.arccos(np.matmul(-q_pre.T, q_next)).item(0)
if np.linalg.norm(q_pre - q_next) > 0.000001 and theta > 0 and abs(
theta) < np.pi:
C = mid_node - (R / np.sin(theta / 2)) * (
q_pre - q_next) / np.linalg.norm(q_pre - q_next)
r1 = mid_node - (R / np.tan(theta / 2)) * q_pre
r2 = mid_node + (R / np.tan(theta / 2)) * q_next
else:
C = mid_node
r1 = C
r2 = C
r1v = r1 - C
r2v = r2 - C
if np.linalg.norm(r1v - r2v) > 0:
rot_theta = np.arccos(
np.matmul(r1v.T / np.linalg.norm(r1v.T),
r2v / np.linalg.norm(r2v))).item(0)
else:
rot_theta = 0
rot_direction = -np.sign(np.cross(r1v[:, 0], r2v[:, 0]).item(2))
start2hp = r1v - start_node
start2mid = mid_node - start_node
end2hp = r2v - last_node
end2mid = mid_node - last_node
dir1 = np.matmul(start2hp.T, start2mid)
dir2 = np.matmul(end2hp.T, end2mid)
forwards = True
if dir1 < 0 or dir2 < 0:
forwards = False
# Checking here to see if the halfplanes or start and end nodes are closer
if forwards and (np.linalg.norm(r1 - mid_node) <
np.linalg.norm(start_node - mid_node)) and (
np.linalg.norm(r2 - mid_node) <
np.linalg.norm(last_node - mid_node)):
current_position = np.array([[startN.n], [startN.e],
[startN.d]])
pre_position = current_position
while (np.linalg.norm(pre_position - r1) >= stepSize):
N = np.append(N, current_position.item(0))
E = np.append(E, current_position.item(1))
D = np.append(D, current_position.item(2))
pre_position = current_position
current_position = current_position + q_pre * stepSize
# print("Part 1")
# print(current_position)
current_position = r1
ang_inc = float(stepSize) / (2 * R)
angle = 0
while (rot_theta is not np.nan) and (angle <= rot_theta):
N = np.append(N, current_position.item(0))
E = np.append(E, current_position.item(1))
D = np.append(D, current_position.item(2))
angle = angle + ang_inc
Rot = np.array([[
np.cos(rot_direction * ang_inc),
np.sin(rot_direction * ang_inc), 0
],
[
-np.sin(rot_direction * ang_inc),
np.cos(rot_direction * ang_inc), 0
], [0, 0, 1]])
current_position = np.matmul(Rot, current_position - C) + C
# print("Part 2")
# print(current_position)
current_position = r2
pre_position = current_position
while (np.linalg.norm(pre_position - last_node) >= stepSize):
N = np.append(N, current_position.item(0))
E = np.append(E, current_position.item(1))
D = np.append(D, current_position.item(2))
pre_position = current_position
current_position = current_position + q_next * stepSize
# print("Part 3")
# print(current_position)
current_position = last_node
N = np.append(N, current_position.item(0))
E = np.append(E, current_position.item(1))
D = np.append(D, current_position.item(2))
# if False: # This will plot each individual path when this function is called. Good for debugging but you don't want it accidentially running when you're trying to actually do a flight. Hence the hardcoded false option. For debuggin, switch to true
# fig = plt.figure()
# ax = fig.add_subplot(111)
# ax.scatter(E, N)
# ax.scatter(r1.item(1), r1.item(0),c='r')
# ax.scatter(r2.item(1), r2.item(0),c='r')
# ax.scatter(C.item(1), C.item(0),c='r')
# ax.axis('equal')
# plt.show()
else:
start_node = msg_ned(start_node.item(0), start_node.item(1),
start_node.item(2))
mid_node = msg_ned(mid_node.item(0), mid_node.item(1),
mid_node.item(2))
last_node = msg_ned(last_node.item(0), last_node.item(1),
last_node.item(2))
N, E, D = self.pointsAlongPath(start_node, mid_node, stepSize)
N2, E2, D2 = self.pointsAlongPath(mid_node, last_node,
stepSize)
N = np.append(N, N2)
E = np.append(E, E2)
D = np.append(D, D2)
return N, E, D
def extendTree(self, tree, startN, endN):
"""RRT class function that extends the passed-in tree. It will continue to attempt adding a leaf until it finds a
successful one. This is the basic RRT algorithm.
Parameters
----------
tree : float
@param tree: An Nx7 array of N leaves in this format: N, E, D, cost, parentIndex, connectsToGoalFlag, chi
startN : msg_ned
@param startN: The starting waypoint.
endN : msg_ned
The ending waypoint.
Returns
-------
tree: float
An Nx7 array of N leaves in this format: N, E, D, cost, parentIndex, connectsToGoalFlag, chi
int
Returns a 1 if a path to the end node was found, 0 if not.
"""
successFlag = False
while not successFlag:
# Generate Random Point
northP, eastP = self.randomPoint()
# Find nearest leaf. Preference given to leaves that are at the correct altitude
distances = ((northP - tree[:, 0])**2 + (eastP - tree[:, 1])**2 +
self.scaleHeight * (endN.d - tree[:, 2])**2)
minIndex = np.argmin(
distances
) # could loop through a second time to try second best node?? This might help with smoother ascending and descending.
# Need to find way to get more smooth descents and ascents?? not zig zaggy
chi = np.arctan2((eastP - tree[minIndex, 1]),
(northP - tree[minIndex, 0]))
# Calculate the new node location
if (tree[minIndex, 2] == endN.d
): # If the chosen leaf is at the ending waypoint altitude
# A new leaf only extends a maximum distance from a previous leaf
L = min(
np.sqrt((northP - tree[minIndex, 0])**2 +
(eastP - tree[minIndex, 1])**2), self.maxDistance)
downP = endN.d
tmp = np.array([northP, eastP, downP]) - np.array(
[tree[minIndex, 0], tree[minIndex, 1], tree[minIndex, 2]])
newPoint = np.array([
tree[minIndex, 0], tree[minIndex, 1], tree[minIndex, 2]
]) + L * (tmp / np.linalg.norm(tmp))
newNode = np.array([[
newPoint.item(0),
newPoint.item(1),
newPoint.item(2), tree[minIndex, 3] + L, minIndex, 0., chi
]])
# # The following commented lines are for seeing the randomly chosen point
# if self.animate:
# scat = self.ax.scatter([northP, northP], [eastP, eastP], [0, -downP], c='r', marker='+')
# scat.remove()
else:
# This case is for when the nearest leaf isn't yet at the correct altitude for the ending waypoint
hyp = np.sqrt((northP - tree[minIndex, 0])**2 +
(eastP - tree[minIndex, 1])**2)
lessInclineWhilePlanning = .3
if startN.d > endN.d:
downP = tree[
minIndex,
2] - hyp * self.maxIncline * lessInclineWhilePlanning
else:
downP = tree[
minIndex,
2] + hyp * self.maxIncline * lessInclineWhilePlanning
q = np.array([
northP - tree[minIndex, 0], eastP - tree[minIndex, 1],
downP - tree[minIndex, 2]
])
L = np.linalg.norm(q)
L = min(L, self.maxDistance)
tmp = np.array([northP, eastP, downP]) - np.array(
[tree[minIndex, 0], tree[minIndex, 1], tree[minIndex, 2]])
newPoint = np.array([
tree[minIndex, 0], tree[minIndex, 1], tree[minIndex, 2]
]) + L * (tmp / np.linalg.norm(tmp))
if (startN.d > endN.d and newPoint.item(2) < endN.d) or (
startN.d < endN.d and newPoint.item(2) > endN.d
): # Check for overshooting the correct altitude
newPoint[2] = endN.d
newNode = np.array([[
newPoint.item(0),
newPoint.item(1),
newPoint.item(2), tree[minIndex, 3] + L, minIndex, 0., chi
]])
# # The following commented lines are for seeing the randomly chosen point
# if self.animate:
# scat = self.ax.scatter([northP, northP], [eastP, eastP], [0, -downP], c='r', marker='+')
# scat.remove()
if minIndex > 0:
startIdx = int(tree[minIndex, 4])
node_1 = msg_ned(tree[startIdx, 0], tree[startIdx, 1],
tree[startIdx, 2])
node_2 = msg_ned(tree[minIndex, 0], tree[minIndex, 1],
tree[minIndex, 2])
node_3 = msg_ned(newNode.item(0), newNode.item(1),
newNode.item(2))
else:
node_1 = msg_ned(tree[minIndex, 0], tree[minIndex, 1],
tree[minIndex, 2])
node_2 = msg_ned(newNode.item(0), newNode.item(1),
newNode.item(2))
node_3 = None
# Check for Collision
#if self.flyablePath(msg_ned(tree[minIndex,0],tree[minIndex,1],tree[minIndex,2]), msg_ned(newNode.item(0),newNode.item(1),newNode.item(2)), tree[minIndex,6] ,chi):
if self.flyablePath(node_1, node_2, tree[minIndex, 6], chi, node_3,
self.R):
successFlag = True
# if minIndex == 0: # Attempt to get forced waypoints around the start node 5/23/2019 JTA
# q = np.array([[node_2.n],[node_2.e],[node_2.d]]) - np.array([[node_1.n],[node_1.e],[node_1.d]])
# forced_point = np.array([[node_1.n],[node_1.e],[node_1.d]]) + self.distance*q
# forced_node = np.array([[forced_point.item(0), forced_point.item(1), forced_point.item(2), np.inf, minIndex, 0., chi]])
# tree = np.append(tree, forced_node, axis=0)
# forced_idx = tree.shape[0]
# print(tree.shape)
# newNode[0,4] = forced_idx - 1
# # The animate lines below draw the fully explored RRT Tree
# if self.animate:
# if(endN.d==startN.d):
# scaler = 1
# else:
# scaler = (endN.d - newNode.item(2))/(endN.d-startN.d)
# spider = self.ax.plot([tree[minIndex,0],newNode.item(0)], [tree[minIndex,1],newNode.item(1)], [-tree[minIndex,2],-newNode.item(2)], color=self.viridis(scaler))
tree = np.append(tree, newNode,
axis=0) # Append new node to the full tree
if np.mod(tree.shape[0], 100) == 0:
print(tree.shape)
# Check to see if the new node can connect to the end node
dist = np.sqrt((endN.n - newNode.item(0))**2 +
(endN.e - newNode.item(1))**2 +
(endN.d - newNode.item(2))**2)
chi = np.arctan2((endN.e - newNode.item(1)),
(endN.n - newNode.item(0)))
if self.flyablePath(
msg_ned(newNode.item(0), newNode.item(1),
newNode.item(2)), endN, newNode.item(6), chi):
tree[np.size(tree, 0) - 1, 5] = 1
return tree, 1 # Return the extended tree with the flag of a successful path to ending node
else:
return tree, 0
def findPath(self, waypoint1, waypoint2, start_chi=8888, connect=False):
"""RRT class function that finds a path between two waypoints passed in. This solved path takes into account obstacles,
boundaries, and all other parameters set in the init function.
Parameters
----------
waypoint1 : msg_ned
The starting waypoint
waypoint2 : msg_ned
@param waypoint2: The ending waypoint. Super creative names, I know.
start_chi : float
The current heading of the path. If 8888, indicates first step in the full path and is ignored
connect : boolean
If true, the path will be generated such that an additional waypoint is created after every primary waypoint to
force the plane to go through the primary waypoint before beginning a turn
Returns
-------
smoothedPath : msg_ned
The list of waypoints which outlines a safe path to follow in order to reach the two waypoints passed in.
"""
if self.animate: # draws the two waypoints
begEndPoints = self.ax.scatter([waypoint1.n, waypoint2.n],
[waypoint1.e, waypoint2.e],
[-waypoint1.d, -waypoint2.d],
c='r',
marker='o')
# node state vector format: N, E, D, cost, parentIndex, connectsToGoalFlag, chi
startNode = np.array(
[waypoint1.n, waypoint1.e, waypoint1.d, 0., -1., 0., start_chi])
tree = np.array([startNode])
# check for if solution at the beginning
dist = np.sqrt((waypoint1.n - waypoint2.n)**2 +
(waypoint1.e - waypoint2.e)**2 +
(waypoint1.d - waypoint2.d)**2)
chi = np.arctan2((waypoint2.e - waypoint1.e),
(waypoint2.n - waypoint1.n))
if self.flyablePath(waypoint1, waypoint2, startNode[6], chi):
return waypoint1, waypoint2 # Returns the two waypoints as the succesful path
#START NEW TESTING CODE
prep_node = np.array([[waypoint1.n], [waypoint1.e], [waypoint1.d]])
last_node = np.array([[waypoint2.n], [waypoint2.e], [waypoint2.d]])
q = (last_node - prep_node) / np.linalg.norm(last_node - prep_node)
add_node = last_node + q * self.distance
if self.flyablePath(
waypoint1,
msg_ned(add_node.item(0), add_node.item(1), add_node.item(2)),
start_chi, chi) and connect:
return waypoint1, waypoint2, msg_ned(add_node.item(0),
add_node.item(1),
add_node.item(2))
elif self.flyablePath(waypoint1, waypoint2, start_chi,
chi) and not connect:
return waypoint1, waypoint2
#END NEW TESTING CODE
else:
foundSolution = 0
while foundSolution < 3: # This will keep expanding the tree the amount of iterations until solution found
for i in range(0, self.iterations):
tree, flag = self.extendTree(tree, waypoint1, waypoint2)
if flag > 0:
print("Found Potential Path")
foundSolution += flag
# # Find the shortest path
# path = self.shortestPath(tree, waypoint2)
# # Smooth the path
# smoothedPath = self.smoothPath(path)
# return smoothedPath
# Find complete paths
connectedPaths = []
for i in range(0, np.size(tree, 0)):
if tree[i, 5] == 1:
connectedNodes = []
connectedNodes.append(waypoint2)
connectedNodes.append(
msg_ned(tree[i, 0], tree[i, 1], tree[i, 2]))
parentNode = int(tree[i, 4])
while parentNode > 0:
connectedNodes.append(
msg_ned(tree[parentNode, 0], tree[parentNode, 1],
tree[parentNode, 2]))
parentNode = int(tree[parentNode, 4])
connectedNodes.append(waypoint1)
connectedPaths.append(connectedNodes)
# Smooth all paths and save the best
bestPath = []
bestCost = np.inf
for path in connectedPaths:
smoothedPath, cost = self.smoothPath(path, start_chi)
if connect:
print("Connect")
last_node = np.array([[smoothedPath[-1].n],
[smoothedPath[-1].e],
[smoothedPath[-1].d]])
prep_node = np.array([[smoothedPath[-2].n],
[smoothedPath[-2].e],
[smoothedPath[-2].d]])
q = (last_node - prep_node) / np.linalg.norm(last_node -
prep_node)
add_node = last_node + q * self.distance
if self.flyablePath(
smoothedPath[-1],
msg_ned(add_node.item(0), add_node.item(1),
add_node.item(2)), 0, 0):
smoothedPath.append(
msg_ned(add_node.item(0), add_node.item(1),
add_node.item(2)))
# #Add node between start and first node to force plane to go through the desired node
# start_node = np.array([[smoothedPath[0].n],[smoothedPath[0].e],[smoothedPath[0].d]])
# next_node = np.array([[smoothedPath[1].n],[smoothedPath[1].e],[smoothedPath[1].d]])
# q = (next_node - start_node)/np.linalg.norm(next_node - start_node)
# if np.linalg.norm(next_node - start_node) < self.distance:
# spacing = np.linalg.norm(next_node - start_node) / 2.0
# else:
# spacing = self.distance
# insert_node = start_node + spacing * q
# smoothedPath.insert(1,msg_ned(insert_node.item(0), insert_node.item(1), insert_node.item(2)))
if cost < bestCost:
bestPath = smoothedPath
bestCost = cost
# if len(bestPath) > 3:
# node1 = bestPath[0]
# node1v = np.array([[node1.n], [node1.e], [node1.d]])
# node2 = bestPath[1]
# node2v = np.array([[node2.n], [node2.e], [node2.d]])
# node_nextlast = bestPath[-2]
# node_nextlastv = np.array([[node_nextlast.n], [node_nextlast.e], [node_nextlast.d]])
# node_last = bestPath[-1]
# node_lastv = np.array([[node_last.n], [node_last.e], [node_last.d]])
# q_start = node2v - node1v
# q_end = node_lastv - node_nextlastv
# new_node2 = node1v + 5.0*q_start/np.linalg.norm(q_start)
# new_last = node_lastv + 5*q_end/np.linalg.norm(q_end)
# node2 = msg_ned(new_node2.item(0), new_node2.item(1), new_node2.item(2))
# nodel = msg_ned(new_last.item(0), new_last.item(1), new_last.item(2))
# bestPathNew = []
# bestPathNew.append(bestPath[0])
# bestPathNew.append(node2)
# bestPathNew.append(bestPath[1:])
# bestPathNew = bestPathNew.append(nodel)
# bestPath = bestPathNew
# elif len(bestPath) > 2:
# pass
# elif len(bestPath) == 2:
# pass
return bestPath
Returns the wrapped angle
"""
while chi_c - chi > np.pi:
chi_c = chi_c - 2.0 * np.pi
while chi_c - chi < -np.pi:
chi_c = chi_c + 2.0 * np.pi
return chi_c
def wrapToPi(self, x):
wrap = np.mod(x, 2 * np.pi)
if np.abs(wrap) > np.pi:
wrap -= 2 * np.pi * np.sign(wrap)
return wrap
def wrapAminusBToPi(self, A, B):
diff_wrap = self.wrapToPi(A - B)
return diff_wrap
if __name__ == "__main__":
test = RRT([msg_ned(-500, -500, 10, 5)], [
msg_ned(-500, -500, 10, 5),
msg_ned(-500, 500, 0),
msg_ned(500, 500, 0),
msg_ned(500, -500, 0)
])
test.findFullPath(
[msg_ned(0., 0., 0.0),
msg_ned(0., 100., 0.0),
msg_ned(100., 0., 0.0)])
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)
bd11 = [38.146131, -76.426653, 0.0] bd0_m = convert(home[0],home[1],home[2],bd0[0],bd0[1],bd0[2]) bd1_m = convert(home[0],home[1],home[2],bd1[0],bd1[1],bd1[2]) bd2_m = convert(home[0],home[1],home[2],bd2[0],bd2[1],bd2[2]) bd3_m = convert(home[0],home[1],home[2],bd3[0],bd3[1],bd3[2]) bd4_m = convert(home[0],home[1],home[2],bd4[0],bd4[1],bd4[2]) bd5_m = convert(home[0],home[1],home[2],bd5[0],bd5[1],bd5[2]) bd6_m = convert(home[0],home[1],home[2],bd6[0],bd6[1],bd6[2]) bd7_m = convert(home[0],home[1],home[2],bd7[0],bd7[1],bd7[2]) bd8_m = convert(home[0],home[1],home[2],bd8[0],bd8[1],bd8[2]) bd9_m = convert(home[0],home[1],home[2],bd9[0],bd9[1],bd9[2]) bd10_m = convert(home[0],home[1],home[2],bd10[0],bd10[1],bd10[2]) bd11_m = convert(home[0],home[1],home[2],bd11[0],bd11[1],bd11[2]) boundariesList.append(msg_ned(bd0_m[0],bd0_m[1])) boundariesList.append(msg_ned(bd1_m[0],bd1_m[1])) boundariesList.append(msg_ned(bd2_m[0],bd2_m[1])) boundariesList.append(msg_ned(bd3_m[0],bd3_m[1])) boundariesList.append(msg_ned(bd4_m[0],bd4_m[1])) boundariesList.append(msg_ned(bd5_m[0],bd5_m[1])) boundariesList.append(msg_ned(bd6_m[0],bd6_m[1])) boundariesList.append(msg_ned(bd7_m[0],bd7_m[1])) boundariesList.append(msg_ned(bd8_m[0],bd8_m[1])) boundariesList.append(msg_ned(bd9_m[0],bd9_m[1])) boundariesList.append(msg_ned(bd10_m[0],bd10_m[1])) boundariesList.append(msg_ned(bd11_m[0],bd11_m[1])) boundaryPoly = makeBoundaryPoly(boundariesList) drop_location = msg_ned(north=100., east=-100.)
def plan(self,
object_location_list,
altitude=70,
clearance=10,
waypoint_distance=100):
object_location = object_location_list[0]
location_point = Point(object_location.n,
object_location.e).buffer(clearance)
if location_point.within(self.boundary_poly):
waypoint = msg_ned(object_location.n, object_location.e, -altitude)
final_waypoints = [waypoint]
else:
dist = np.zeros(len(self.boundary_list))
norm = np.zeros((len(self.boundary_list), 2))
slope = np.zeros((len(self.boundary_list), 2))
for i in np.arange(len(self.boundary_list)):
j = i + 1
if i == len(self.boundary_list) - 1:
j = 0
dn = self.boundary_list[i].n - self.boundary_list[j].n
de = self.boundary_list[i].e - self.boundary_list[j].e
pn = object_location.n - self.boundary_list[j].n
pe = object_location.e - self.boundary_list[j].e
slope[i, :] = np.array([[dn, de]]) / np.linalg.norm(
np.array([[dn, de]]))
norm[i, :] = np.array([[de, -dn]]) / np.linalg.norm(
np.array([[de, -dn]]))
point = np.array([[pn, pe]])
dist[i] = np.abs(np.dot(norm[i, :], point[0]))
line_idx = np.argmin(dist)
waypoint_n = object_location.n + norm[line_idx, 0] * (
dist[line_idx] + clearance)
waypoint_e = object_location.e + norm[line_idx, 1] * (
dist[line_idx] + clearance)
direction = 1
if Point(waypoint_n, waypoint_e).within(self.boundary_poly):
waypoint = msg_ned(waypoint_n, waypoint_e, -altitude)
else:
direction = -1
waypoint_n = object_location.n - norm[line_idx, 0] * (
dist[line_idx] + clearance)
waypoint_e = object_location.e - norm[line_idx, 1] * (
dist[line_idx] + clearance)
waypoint = msg_ned(waypoint_n, waypoint_e, -altitude)
parallel_point = msg_ned(
waypoint.n + waypoint_distance * slope[line_idx, 0],
waypoint.e + waypoint_distance * slope[line_idx, 1], altitude)
if Point(parallel_point.n, parallel_point.e).buffer(
clearance * .99).within(self.boundary_poly):
pass
else:
parallel_point.n = waypoint.n - waypoint_distance * slope[
line_idx, 0]
parallel_point.e = waypoint.e - waypoint_distance * slope[
line_idx, 1]
end_point = msg_ned(
waypoint.n + direction * waypoint_distance * norm[line_idx, 0],
waypoint.e + direction * waypoint_distance * norm[line_idx, 1],
altitude)
scale = 1.
while (not Point(end_point.n,
end_point.e).buffer(clearance).within(
self.boundary_poly)):
end_point.n = waypoint.n + direction * waypoint_distance * norm[
line_idx, 0] * scale
end_point.e = waypoint.e + direction * waypoint_distance * norm[
line_idx, 1] * scale
scale = scale - .05
final_waypoints = [parallel_point, waypoint, end_point]
if True: # This is for debugging/visualization purposes only. I don't think we need/want this during the competition but it is nice to use when debugging
fig, ax = plt.subplots()
for point in final_waypoints:
ax.scatter(point.e, point.n, c='k')
ax.scatter(object_location.e, object_location.n)
for point in self.boundary_list:
ax.scatter(point.e, point.n, c='r')
ax.plot([self.boundary_list[0].e, self.boundary_list[1].e],
[self.boundary_list[0].n, self.boundary_list[1].n], 'r')
ax.plot([self.boundary_list[1].e, self.boundary_list[2].e],
[self.boundary_list[1].n, self.boundary_list[2].n], 'r')
ax.plot([self.boundary_list[2].e, self.boundary_list[3].e],
[self.boundary_list[2].n, self.boundary_list[3].n], 'r')
ax.plot([self.boundary_list[3].e, self.boundary_list[0].e],
[self.boundary_list[3].n, self.boundary_list[0].n], 'r')
ax.axis('equal')
plt.show()
return final_waypoints
def __init__(self, Va=17):
"""brief Creates a new main planner objectives
This initializes a new main planner. The reference latitude, longitude, and altitude are taken from the .launch files
A service call is made to get the stationary obstacles, boundaries, and payload drop location.
The various mission planners are initilized with the obstacles and boundaries. The paylod planer additionally is initialized
with the drop location known.
"""
#Get ref lat, lon from launch file
ref_lat = rospy.get_param("ref_lat")
ref_lon = rospy.get_param("ref_lon")
ref_h = rospy.get_param("ref_h")
self.target_h = rospy.get_param("target_h")
self.ref_pos = [ref_lat, ref_lon, ref_h]
self.DEFAULT_POS = (0., 0., -self.target_h)
#Keeps track of what task is currently being executed
self.task = 0
self._ser_waypoints = rospy.Service('approved_path', UploadPath,
self.update_path_callback)
self._ser_clear = rospy.Service('clear_wpts', UploadPath,
self.clear_waypoints)
#Proposing a switch to a service call rather than a topic to get info from GUI. If that holds then delete this line
#self._sub_mission = rospy.Subscriber('task_command', JudgeMission, self.update_task, queue_size=5)
self._pub_task = rospy.Publisher('current_task',
JudgeMission,
queue_size=5)
#Proposing a switch to a service call rather than a topic to get info to the GUI. If that holds, delete this line
#self._pub_waypoints = rospy.Publisher('desired_waypoints', NED_list, queue_size=5)
#self._plan_server = rospy.Service('plan_mission', PlanMissionPoints, self.update_task_callback)
self._plan_server = rospy.Service('plan_path', PlanMissionPoints,
self.update_task_callback)
self._ser_search_params = rospy.Service('update_search_params',
UpdateSearchParams,
self.update_search_params)
#Get the obstacles, boundaries, and drop location in order to initialize the planner classes
mission_type, obstacles, boundary_list, boundary_poly, drop_location = tools.get_server_data(
JudgeMission.MISSION_TYPE_DROP, self.ref_pos)
#Initiate the planner classes
self._plan_payload = PayloadPlanner(drop_location[0], obstacles,
boundary_list, boundary_poly)
self._plan_loiter = LoiterPlanner(obstacles, boundary_poly)
self._plan_search = SearchPlanner(boundary_list, obstacles)
self._plan_objective = ObjectivePointsPlanner(obstacles)
self._plan_offaxis = OffaxisPlanner(boundary_list, obstacles)
self._plan_landing = LandingPlanner(boundary_list, obstacles)
self.landing = False
self.last_exists = False
self.last_waypoint = msg_ned(*self.DEFAULT_POS)
self.rrt = RRT(
obstacles, boundary_list, animate=False
) #Other arguments are available but have been given default values in the RRT constructor
#-----START DEBUG----
#This code is just used to visually check that everything worked ok. Can be removed anytime.
print("Obstacles")
for obstacle in obstacles:
print(obstacle.n, obstacle.e, obstacle.d, obstacle.r)
print("Boundaries")
for boundary in boundary_list:
print(boundary.n, boundary.e)
print("Drop")
for drop in drop_location:
print(drop.n, drop.e, drop.d)
_, _, _, _, objective_waypts = tools.get_server_data(
JudgeMission.MISSION_TYPE_WAYPOINT, self.ref_pos)
_, _, _, _, search_boundary = tools.get_server_data(
JudgeMission.MISSION_TYPE_SEARCH, self.ref_pos)
self.obstacles = obstacles
self.drop_location = drop_location
self.objective_waypts = objective_waypts
self.search_boundary = search_boundary
self.boundary_list = boundary_list
#-----END DEBUG----
self.Va = Va