def __init__( self, missionID, takeoffAlt=5, takeoffLoiterTime=5, takeoffLoiterAlt=15, outputDir=os.path.join(os.path.dirname(os.path.realpath(__file__)), "missions"), logName="mission", ): # setup logger self.logger = common.setupLogger(logName) # set ouput directory self.outputDir = outputDir # TODO: Check to make sure the dir exists self.latRef = None self.lonRef = None self.altRef = None self.frame = "ned" self.points = [] # setup altitude types self.altitudeTypes = {"relative": 3, "terrain": 10} self.availableFrames = ["ned", "lla"] self.missionID = missionID self.takeoffAlt = takeoffAlt self.takeoffLoiterTime = takeoffLoiterTime self.takeoffLoiterAlt = takeoffLoiterAlt self.filePath = os.path.join(self.outputDir, self.missionID + ".txt") self.fid = None self.missionLine = 0 self.autoContinue = 1 self.srtmGrid = None self.mavlinkEnums = mavlink_meta.getMavlinkEnums() ros_mission.subscribe_waypoints(self.mission_callback)
def do_load(args): #def do_load(filename_str): """ filename_str is the file location (and filename) to open that has the waypoints we are also assuming that we are not preserving the home location """ #def do_load(waypoint_list): """ waypoint_list is expected to be in a "list of Waypoint[(s)]" format (from mavros_msgs.msg import Waypoint) we are also assuming that we are not preserving the home location """ wps = [] wps_file = get_wp_file_io(args) #wps_file = get_wp_file_io() with args.file: wps = [w for w in wps_file.read(args.file)] #with filename_str: # wps = [w for w in wps_file.read(filename_str)] print("wps is %r" % wps) # this is from _load_call(waypoint_list) #try: # ret = M.push(waypoints=wps) #except rospy.ServiceException as ex: # fault(ex) def _load_call(waypoint_list): print("waypoint_list is: %r" % str(waypoint_list)) print("M.push is... %r" % str(M.push)) #print("Mpush is... %r" % str(Mpush)) try: ret = M.push(waypoints=waypoint_list) except rospy.ServiceException as ex: fault(ex) if not ret.success: fault("Request failed. Check mavros logs") print_if(args.verbose, "Waypoints transfered:", ret.wp_transfered) done_evt = threading.Event() def _fix_wp0(topic): if len(topic.waypoints) > 0: wps[0] = topic.waypoints[0] print_if(args.verbose, "HOME location: latitude:", wps[0].x_lat, "longitude:", wps[0].y_long, "altitude:", wps[0].z_alt) else: print("Failed to get WP0! WP0 will be loaded from file.", file=sys.stderr) done_evt.set() if not args.preserve_home: print("not args.preserve_home") _load_call(wps) else: print("args.preserve_home") # Note: _load_call() emit publish on this topic, so callback only changes # waypoint 0, and signal done event. sub = M.subscribe_waypoints(_fix_wp0) if not done_evt.wait(30.0): fault("Something went wrong. Topic timed out.") else: sub.unregister() _load_call(wps)
def mission_waypoints(self): self.waypoints = mission.pull() application_log.debug(f"waypoints: {self.waypoints}") mission.subscribe_waypoints(self.mission_callback)
def mission_waypoints(self): self.waypoints = mission.pull() mission.subscribe_waypoints(self.mission_callback)