mavwp.py
Go to the documentation of this file.
00001 '''
00002 module for loading/saving waypoints
00003 '''
00004 
00005 import time, copy
00006 import logging
00007 from . import mavutil
00008 try:
00009     from google.protobuf import text_format
00010     import mission_pb2
00011     HAVE_PROTOBUF = True
00012 except ImportError:
00013     HAVE_PROTOBUF = False
00014 
00015 
00016 class MAVWPError(Exception):
00017     '''MAVLink WP error class'''
00018     def __init__(self, msg):
00019         Exception.__init__(self, msg)
00020         self.message = msg
00021 
00022 
00023 class MAVWPLoader(object):
00024     '''MAVLink waypoint loader'''
00025     def __init__(self, target_system=0, target_component=0):
00026         self.wpoints = []
00027         self.target_system = target_system
00028         self.target_component = target_component
00029         self.last_change = time.time()
00030 
00031     def count(self):
00032         '''return number of waypoints'''
00033         return len(self.wpoints)
00034 
00035     def wp(self, i):
00036         '''return a waypoint'''
00037         try:
00038             the_wp = self.wpoints[i]
00039         except:
00040             the_wp = None
00041 
00042         return the_wp
00043 
00044     def wp_is_loiter(self, i):
00045         '''return true if waypoint is a loiter waypoint'''
00046         loiter_cmds = [mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM,
00047                 mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS,
00048                 mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME,
00049                 mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT]
00050 
00051         if (self.wpoints[i].command in loiter_cmds):
00052             return True    
00053 
00054         return False
00055 
00056     def add(self, w, comment=''):
00057         '''add a waypoint'''
00058         w = copy.copy(w)
00059         if comment:
00060             w.comment = comment
00061         w.seq = self.count()
00062         self.wpoints.append(w)
00063         self.last_change = time.time()
00064 
00065     def insert(self, idx, w, comment=''):
00066         '''insert a waypoint'''
00067         if idx >= self.count():
00068             self.add(w, comment)
00069             return
00070         if idx < 0:
00071             return
00072         w = copy.copy(w)
00073         if comment:
00074             w.comment = comment
00075         w.seq = idx
00076         self.wpoints.insert(idx, w)
00077         self.last_change = time.time()
00078         self.reindex()
00079 
00080     def reindex(self):
00081         '''reindex waypoints'''
00082         for i in range(self.count()):
00083             w = self.wpoints[i]
00084             w.seq = i
00085         self.last_change = time.time()
00086 
00087     def add_latlonalt(self, lat, lon, altitude, terrain_alt=False):
00088         '''add a point via latitude/longitude/altitude'''
00089         if terrain_alt:
00090             frame = mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT
00091         else:
00092             frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT
00093         p = mavutil.mavlink.MAVLink_mission_item_message(self.target_system,
00094                                                          self.target_component,
00095                                                          0,
00096                                                          frame,
00097                                                          mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
00098                                                          0, 0, 0, 0, 0, 0,
00099                                                          lat, lon, altitude)
00100         self.add(p)
00101 
00102     def set(self, w, idx):
00103         '''set a waypoint'''
00104         w.seq = idx
00105         if w.seq == self.count():
00106             return self.add(w)
00107         if self.count() <= idx:
00108             raise MAVWPError('adding waypoint at idx=%u past end of list (count=%u)' % (idx, self.count()))
00109         self.wpoints[idx] = w
00110         self.last_change = time.time()
00111 
00112     def remove(self, w):
00113         '''remove a waypoint'''
00114         self.wpoints.remove(w)
00115         self.last_change = time.time()
00116         self.reindex()
00117 
00118     def clear(self):
00119         '''clear waypoint list'''
00120         self.wpoints = []
00121         self.last_change = time.time()
00122 
00123     def _read_waypoints_v100(self, file):
00124         '''read a version 100 waypoint'''
00125         cmdmap = {
00126             2 : mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
00127             3 : mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH,
00128             4 : mavutil.mavlink.MAV_CMD_NAV_LAND,
00129             24: mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
00130             26: mavutil.mavlink.MAV_CMD_NAV_LAND,
00131             25: mavutil.mavlink.MAV_CMD_NAV_WAYPOINT ,
00132             27: mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM
00133             }
00134         comment = ''
00135         for line in file:
00136             if line.startswith('#'):
00137                 comment = line[1:].lstrip()
00138                 continue
00139             line = line.strip()
00140             if not line:
00141                 continue
00142             a = line.split()
00143             if len(a) != 13:
00144                 raise MAVWPError("invalid waypoint line with %u values" % len(a))
00145             if mavutil.mavlink10():
00146                 fn = mavutil.mavlink.MAVLink_mission_item_message
00147             else:
00148                 fn = mavutil.mavlink.MAVLink_waypoint_message
00149             w = fn(self.target_system, self.target_component,
00150                    int(a[0]),    # seq
00151                    int(a[1]),    # frame
00152                    int(a[2]),    # action
00153                    int(a[7]),    # current
00154                    int(a[12]),   # autocontinue
00155                    float(a[5]),  # param1,
00156                    float(a[6]),  # param2,
00157                    float(a[3]),  # param3
00158                    float(a[4]),  # param4
00159                    float(a[9]),  # x, latitude
00160                    float(a[8]),  # y, longitude
00161                    float(a[10])  # z
00162                    )
00163             if not w.command in cmdmap:
00164                 raise MAVWPError("Unknown v100 waypoint action %u" % w.command)
00165 
00166             w.command = cmdmap[w.command]
00167             self.add(w, comment)
00168             comment = ''
00169 
00170     def _read_waypoints_v110(self, file):
00171         '''read a version 110 waypoint'''
00172         comment = ''
00173         for line in file:
00174             if line.startswith('#'):
00175                 comment = line[1:].lstrip()
00176                 continue
00177             line = line.strip()
00178             if not line:
00179                 continue
00180             a = line.split()
00181             if len(a) != 12:
00182                 raise MAVWPError("invalid waypoint line with %u values" % len(a))
00183             if mavutil.mavlink10():
00184                 fn = mavutil.mavlink.MAVLink_mission_item_message
00185             else:
00186                 fn = mavutil.mavlink.MAVLink_waypoint_message
00187             w = fn(self.target_system, self.target_component,
00188                    int(a[0]),    # seq
00189                    int(a[2]),    # frame
00190                    int(a[3]),    # command
00191                    int(a[1]),    # current
00192                    int(a[11]),   # autocontinue
00193                    float(a[4]),  # param1,
00194                    float(a[5]),  # param2,
00195                    float(a[6]),  # param3
00196                    float(a[7]),  # param4
00197                    float(a[8]),  # x (latitude)
00198                    float(a[9]),  # y (longitude)
00199                    float(a[10])  # z (altitude)
00200                    )
00201             if w.command == 0 and w.seq == 0 and self.count() == 0:
00202                 # special handling for Mission Planner created home wp
00203                 w.command = mavutil.mavlink.MAV_CMD_NAV_WAYPOINT
00204             self.add(w, comment)
00205             comment = ''
00206 
00207     def _read_waypoints_pb_110(self, file):
00208         if not HAVE_PROTOBUF:
00209             raise MAVWPError(
00210                 'Cannot read mission file in protobuf format without protobuf '
00211                 'library. Try "easy_install protobuf".')
00212         explicit_seq = False
00213         warned_seq = False
00214         mission = mission_pb2.Mission()
00215         text_format.Merge(file.read(), mission)
00216         defaults = mission_pb2.Waypoint()
00217         # Set defaults (may be overriden in file).
00218         defaults.current = False
00219         defaults.autocontinue = True
00220         defaults.param1 = 0.0
00221         defaults.param2 = 0.0
00222         defaults.param3 = 0.0
00223         defaults.param4 = 0.0
00224         defaults.x = 0.0
00225         defaults.y = 0.0
00226         defaults.z = 0.0
00227         # Use defaults specified in mission file, if there are any.
00228         if mission.defaults:
00229             defaults.MergeFrom(mission.defaults)
00230         for seq, waypoint in enumerate(mission.waypoint):
00231             # Consecutive sequence numbers are automatically assigned
00232             # UNLESS the mission file specifies sequence numbers of
00233             # its own.
00234             if waypoint.seq:
00235                 explicit_seq = True
00236             else:
00237                 if explicit_seq and not warned_seq:
00238                     logging.warn(
00239                             'Waypoint file %s: mixes explicit and implicit '
00240                             'sequence numbers' % (file,))
00241                     warned_seq = True
00242             # The first command has current=True, the rest have current=False.
00243             if seq > 0:
00244                 current = defaults.current
00245             else:
00246                 current = True
00247             w = mavutil.mavlink.MAVLink_mission_item_message(
00248                 self.target_system, self.target_component,
00249                    waypoint.seq or seq,
00250                    waypoint.frame,
00251                    waypoint.command,
00252                    waypoint.current or current,
00253                    waypoint.autocontinue or defaults.autocontinue,
00254                    waypoint.param1 or defaults.param1,
00255                    waypoint.param2 or defaults.param2,
00256                    waypoint.param3 or defaults.param3,
00257                    waypoint.param4 or defaults.param4,
00258                    waypoint.x or defaults.x,
00259                    waypoint.y or defaults.y,
00260                    waypoint.z or defaults.z)
00261             self.add(w)
00262 
00263     def load(self, filename):
00264         '''load waypoints from a file.
00265         returns number of waypoints loaded'''
00266         f = open(filename, mode='r')
00267         version_line = f.readline().strip()
00268         if version_line == "QGC WPL 100":
00269             readfn = self._read_waypoints_v100
00270         elif version_line == "QGC WPL 110":
00271             readfn = self._read_waypoints_v110
00272         elif version_line == "QGC WPL PB 110":
00273             readfn = self._read_waypoints_pb_110
00274         else:
00275             f.close()
00276             raise MAVWPError("Unsupported waypoint format '%s'" % version_line)
00277 
00278         self.clear()
00279         readfn(f)
00280         f.close()
00281 
00282         return len(self.wpoints)
00283 
00284     def save_as_pb(self, filename):
00285         mission = mission_pb2.Mission()
00286         for w in self.wpoints:
00287             waypoint = mission.waypoint.add()
00288             waypoint.command = w.command
00289             waypoint.frame = w.frame
00290             waypoint.seq = w.seq
00291             waypoint.current = w.current
00292             waypoint.autocontinue = w.autocontinue
00293             waypoint.param1 = w.param1
00294             waypoint.param2 = w.param2
00295             waypoint.param3 = w.param3
00296             waypoint.param4 = w.param4
00297             waypoint.x = w.x
00298             waypoint.y = w.y
00299             waypoint.z = w.z
00300         with open(filename, 'w') as f:
00301             f.write('QGC WPL PB 110\n')
00302             f.write(text_format.MessageToString(mission))
00303 
00304     def save(self, filename):
00305         '''save waypoints to a file'''
00306         f = open(filename, mode='w')
00307         f.write("QGC WPL 110\n")
00308         for w in self.wpoints:
00309             if getattr(w, 'comment', None):
00310                 f.write("# %s\n" % w.comment)
00311             f.write("%u\t%u\t%u\t%u\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%u\n" % (
00312                 w.seq, w.current, w.frame, w.command,
00313                 w.param1, w.param2, w.param3, w.param4,
00314                 w.x, w.y, w.z, w.autocontinue))
00315         f.close()
00316 
00317     def is_location_command(self, cmd):
00318         '''see if cmd is a MAV_CMD with a latitide/longitude.
00319         We check if it has Latitude and Longitude params in the right indexes'''
00320         mav_cmd = mavutil.mavlink.enums['MAV_CMD']
00321         if not cmd in mav_cmd:
00322             return False
00323         if not mav_cmd[cmd].param.get(5,'').startswith('Latitude'):
00324             return False
00325         if not mav_cmd[cmd].param.get(6,'').startswith('Longitude'):
00326             return False
00327         return True
00328 
00329 
00330     def view_indexes(self, done=None):
00331         '''return a list waypoint indexes in view order'''
00332         ret = []
00333         if done is None:
00334             done = set()
00335         idx = 0
00336 
00337         # find first point not done yet
00338         while idx < self.count():
00339             if not idx in done:
00340                 break
00341             idx += 1
00342             
00343         while idx < self.count():
00344             w = self.wp(idx)
00345             if idx in done:
00346                 if w.x != 0 or w.y != 0:
00347                     ret.append(idx)
00348                 break
00349             done.add(idx)
00350             if w.command == mavutil.mavlink.MAV_CMD_DO_JUMP:
00351                 idx = int(w.param1)
00352                 w = self.wp(idx)
00353                 if w.x != 0 or w.y != 0:
00354                     ret.append(idx)
00355                 continue
00356             if (w.x != 0 or w.y != 0) and self.is_location_command(w.command):
00357                 ret.append(idx)
00358             idx += 1
00359         return ret
00360 
00361     def polygon(self, done=None):
00362         '''return a polygon for the waypoints'''
00363         indexes = self.view_indexes(done)
00364         points = []
00365         for idx in indexes:
00366             w = self.wp(idx)
00367             points.append((w.x, w.y))
00368         return points
00369 
00370     def polygon_list(self):
00371         '''return a list of polygons for the waypoints'''
00372         done = set()
00373         ret = []
00374         while len(done) != self.count():
00375             p = self.polygon(done)
00376             if len(p) > 0:
00377                 ret.append(p)
00378         return ret
00379 
00380     def view_list(self):
00381         '''return a list of polygon indexes lists for the waypoints'''
00382         done = set()
00383         ret = []
00384         while len(done) != self.count():
00385             p = self.view_indexes(done)
00386             if len(p) > 0:
00387                 ret.append(p)
00388         return ret
00389 
00390 class MAVRallyError(Exception):
00391     '''MAVLink rally point error class'''
00392     def __init__(self, msg):
00393         Exception.__init__(self, msg)
00394         self.message = msg
00395 
00396 class MAVRallyLoader(object):
00397     '''MAVLink Rally points and Rally Land ponts loader'''
00398     def __init__(self, target_system=0, target_component=0):
00399         self.rally_points = []
00400         self.target_system = target_system
00401         self.target_component = target_component
00402         self.last_change = time.time()
00403 
00404     def rally_count(self):
00405         '''return number of rally points'''
00406         return len(self.rally_points)
00407 
00408     def rally_point(self, i):
00409         '''return rally point i'''
00410         return self.rally_points[i]
00411 
00412     def reindex(self):
00413         '''reset counters and indexes'''
00414         for i in range(self.rally_count()):
00415             self.rally_points[i].count = self.rally_count()
00416             self.rally_points[i].idx = i
00417         self.last_change = time.time()
00418             
00419     def append_rally_point(self, p):
00420         '''add rallypoint to end of list'''
00421         if (self.rally_count() > 9):
00422            print("Can't have more than 10 rally points, not adding.")
00423            return
00424 
00425         self.rally_points.append(p)
00426         self.reindex()
00427 
00428     def create_and_append_rally_point(self, lat, lon, alt, break_alt, land_dir, flags):
00429         '''add a point via latitude/longitude'''
00430         p = mavutil.mavlink.MAVLink_rally_point_message(self.target_system, self.target_component,
00431                                                         self.rally_count(), 0, lat, lon, alt, break_alt, land_dir, flags)
00432         self.append_rally_point(p)
00433 
00434     def clear(self):
00435         '''clear all point lists (rally and rally_land)'''
00436         self.rally_points = []
00437         self.last_change = time.time()
00438 
00439     def remove(self, i):
00440         '''remove a rally point'''
00441         if i < 1 or i > self.rally_count():
00442             print("Invalid rally point number %u" % i)
00443         self.rally_points.pop(i-1)
00444         self.reindex()
00445 
00446     def move(self, i, lat, lng, change_time=True):
00447         '''move a rally point'''
00448         if i < 1 or i > self.rally_count():
00449             print("Invalid rally point number %u" % i)
00450         self.rally_points[i-1].lat = int(lat*1e7)
00451         self.rally_points[i-1].lng = int(lng*1e7)
00452         if change_time:
00453             self.last_change = time.time()
00454 
00455     def set_alt(self, i, alt, break_alt=None, change_time=True):
00456         '''set rally point altitude(s)'''
00457         if i < 1 or i > self.rally_count():
00458             print("Inavlid rally point number %u" % i)
00459             return
00460         self.rally_points[i-1].alt = int(alt)
00461         if (break_alt != None):
00462             self.rally_points[i-1].break_alt = break_alt
00463         if change_time:
00464             self.last_change = time.time()
00465 
00466     def load(self, filename):
00467         '''load rally and rally_land points from a file.
00468          returns number of points loaded'''
00469         f = open(filename, mode='r')
00470         self.clear()
00471         for line in f:
00472             if line.startswith('#'):
00473                 continue
00474             line = line.strip()
00475             if not line:
00476                 continue
00477             a = line.split()
00478             if len(a) != 7:
00479                 raise MAVRallyError("invalid rally file line: %s" % line)
00480 
00481             if (a[0].lower() == "rally"):
00482                 self.create_and_append_rally_point(float(a[1]) * 1e7, float(a[2]) * 1e7,
00483                                                    float(a[3]), float(a[4]), float(a[5]) * 100.0, int(a[6]))
00484         f.close()
00485         return len(self.rally_points)
00486 
00487     def save(self, filename):
00488         '''save fence points to a file'''
00489         f = open(filename, mode='w')
00490         for p in self.rally_points:
00491             f.write("RALLY %f\t%f\t%f\t%f\t%f\t%d\n" % (p.lat * 1e-7, p.lng * 1e-7, p.alt,
00492                                                         p.break_alt, p.land_dir, p.flags))
00493         f.close()
00494 
00495 class MAVFenceError(Exception):
00496         '''MAVLink fence error class'''
00497         def __init__(self, msg):
00498             Exception.__init__(self, msg)
00499             self.message = msg
00500 
00501 class MAVFenceLoader(object):
00502     '''MAVLink geo-fence loader'''
00503     def __init__(self, target_system=0, target_component=0):
00504         self.points = []
00505         self.target_system = target_system
00506         self.target_component = target_component
00507         self.last_change = time.time()
00508 
00509     def count(self):
00510         '''return number of points'''
00511         return len(self.points)
00512 
00513     def point(self, i):
00514         '''return a point'''
00515         return self.points[i]
00516 
00517     def add(self, p):
00518         '''add a point'''
00519         self.points.append(p)
00520         self.reindex()
00521 
00522     def reindex(self):
00523         '''reindex waypoints'''
00524         for i in range(self.count()):
00525             w = self.points[i]
00526             w.idx = i
00527             w.count = self.count()
00528             w.target_system = self.target_system
00529             w.target_component = self.target_component
00530         self.last_change = time.time()
00531 
00532     def add_latlon(self, lat, lon):
00533         '''add a point via latitude/longitude'''
00534         p = mavutil.mavlink.MAVLink_fence_point_message(self.target_system, self.target_component,
00535                                                         self.count(), 0, lat, lon)
00536         self.add(p)
00537 
00538     def clear(self):
00539         '''clear point list'''
00540         self.points = []
00541         self.last_change = time.time()
00542 
00543     def load(self, filename):
00544         '''load points from a file.
00545         returns number of points loaded'''
00546         f = open(filename, mode='r')
00547         self.clear()
00548         for line in f:
00549             if line.startswith('#'):
00550                 continue
00551             line = line.strip()
00552             if not line:
00553                 continue
00554             a = line.split()
00555             if len(a) != 2:
00556                 raise MAVFenceError("invalid fence point line: %s" % line)
00557             self.add_latlon(float(a[0]), float(a[1]))
00558         f.close()
00559         return len(self.points)
00560 
00561     def save(self, filename):
00562         '''save fence points to a file'''
00563         f = open(filename, mode='w')
00564         for p in self.points:
00565             f.write("%f\t%f\n" % (p.lat, p.lng))
00566         f.close()
00567 
00568     def move(self, i, lat, lng, change_time=True):
00569         '''move a fence point'''
00570         if i < 0 or i >= self.count():
00571             print("Invalid fence point number %u" % i)
00572         self.points[i].lat = lat
00573         self.points[i].lng = lng
00574         # ensure we close the polygon
00575         if i == 1:
00576                 self.points[self.count()-1].lat = lat
00577                 self.points[self.count()-1].lng = lng
00578         if i == self.count() - 1:
00579                 self.points[1].lat = lat
00580                 self.points[1].lng = lng
00581         if change_time:
00582             self.last_change = time.time()
00583 
00584     def remove(self, i, change_time=True):
00585         '''remove a fence point'''
00586         if i < 0 or i >= self.count():
00587             print("Invalid fence point number %u" % i)
00588         self.points.pop(i)
00589          # ensure we close the polygon
00590         if i == 1:
00591                 self.points[self.count()-1].lat = self.points[1].lat
00592                 self.points[self.count()-1].lng = self.points[1].lng
00593         if i == self.count():
00594                 self.points[1].lat = self.points[self.count()-1].lat
00595                 self.points[1].lng = self.points[self.count()-1].lng
00596         if change_time:
00597             self.last_change = time.time()
00598 
00599     def polygon(self):
00600             '''return a polygon for the fence'''
00601             points = []
00602             for fp in self.points[1:]:
00603                     points.append((fp.lat, fp.lng))
00604             return points


mavlink
Author(s): Lorenz Meier
autogenerated on Thu Jun 6 2019 19:01:57