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)