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