def read(self, file_): got_header = False for data in csv.reader(file_, self.CSVDialect): if data[0].startswith('#'): continue # skip comments (i think in next format version they add this) if not got_header: qgc, wpl, ver = data[0].split(' ', 3) ver = int(ver) if qgc == 'QGC' and wpl == 'WPL' and ver in self.known_versions: got_header = True else: yield Waypoint(is_current=bool(int(data[1])), frame=int(data[2]), command=int(data[3]), param1=float(data[4]), param2=float(data[5]), param3=float(data[6]), param4=float(data[7]), x_lat=float(data[8]), y_long=float(data[9]), z_alt=float(data[10]), autocontinue=bool(int(data[11])))
def start_with_takeoff(self): start = self.make_global_waypoint(0, 0) takeoff = Waypoint() takeoff.command = 22 takeoff.z_alt = DEFAULT_ALT self.waypoints.insert(0, takeoff) self.waypoints.insert(0, start)
def get_mission(mission_num=0): rospack = rospkg.RosPack() package_path = rospack.get_path('drone_control') tree = ET.parse(package_path + '/scripts/missions.xml') for mission in tree.getroot(): if mission.attrib['id'] == mission_num: break waypoint_list = [] for item in mission: waypoint = Waypoint() waypoint.frame = int(item.find('frame').text) waypoint.command = int(item.find('command').text) waypoint.is_current = bool(item.find('is_current').text) waypoint.autocontinue = bool(item.find('autocontinue').text) waypoint.param1 = float(item.find('param1').text) waypoint.param2 = float(item.find('param2').text) waypoint.param3 = float(item.find('param3').text) waypoint.param4 = float(item.find('param4').text) waypoint.x_lat = float(item.find('latitude').text) waypoint.y_long = float(item.find('longitude').text) waypoint.z_alt = float(item.find('altitude').text) waypoint_list.append(waypoint) return waypoint_list
def make_global_waypoint(lat, lon): waypoint = Waypoint() waypoint.frame = 3 waypoint.command = 16 waypoint.is_current = 0 waypoint.autocontinue = False waypoint.param1 = 5 #hold time waypoint.param2 = 2 waypoint.param3 = 0 waypoint.param4 = 0 waypoint.x_lat = lat waypoint.y_long = lon waypoint.z_alt = 5 return waypoint
def make_global_waypoint(self, lat, lon, alt=DEFAULT_ALT, hold=5): rospack = rospkg.RosPack() package_path = rospack.get_path('drone_control') waypoint = Waypoint() waypoint.frame = 3 waypoint.command = 16 waypoint.is_current = 0 waypoint.autocontinue = False waypoint.param1 = hold #hold time waypoint.param2 = 2 waypoint.param3 = 0 waypoint.param4 = 0 waypoint.x_lat = lat waypoint.y_long = lon waypoint.z_alt = alt return waypoint
def end_with_rtl(self): rtl = Waypoint() rtl.command = 20 self.waypoints.append(rtl)