mission.py
Go to the documentation of this file.
00001 # -*- python -*-
00002 # vim:set ts=4 sw=4 et:
00003 #
00004 # Copyright 2014 Vladimir Ermakov.
00005 #
00006 # This file is part of the mavros package and subject to the license terms
00007 # in the top-level LICENSE file of the mavros repository.
00008 # https://github.com/mavlink/mavros/tree/master/LICENSE.md
00009 
00010 import csv
00011 import time
00012 
00013 import rospy
00014 import mavros
00015 
00016 from mavros_msgs.msg import Waypoint, WaypointList, CommandCode
00017 from mavros_msgs.srv import WaypointPull, WaypointPush, WaypointClear, \
00018     WaypointSetCurrent
00019 
00020 
00021 FRAMES = {
00022     Waypoint.FRAME_GLOBAL: 'GAA',
00023     Waypoint.FRAME_GLOBAL_REL_ALT: 'GRA',
00024     Waypoint.FRAME_LOCAL_ENU: 'LOC-ENU',
00025     Waypoint.FRAME_LOCAL_NED: 'LOC-NED',
00026     Waypoint.FRAME_MISSION: 'MIS'
00027 }
00028 
00029 NAV_CMDS = {
00030     CommandCode.NAV_LAND: 'LAND',
00031     CommandCode.NAV_LOITER_TIME: 'LOITER-TIME',
00032     CommandCode.NAV_LOITER_TURNS: 'LOITER-TURNS',
00033     CommandCode.NAV_LOITER_UNLIM: 'LOITER-UNLIM',
00034     CommandCode.NAV_RETURN_TO_LAUNCH: 'RTL',
00035     CommandCode.NAV_TAKEOFF: 'TAKEOFF',
00036     CommandCode.NAV_WAYPOINT: 'WAYPOINT',
00037     # Maybe later i will add this enum to message
00038     112: 'COND-DELAY',
00039     113: 'COND-CHANGE-ALT',
00040     114: 'COND-DISTANCE',
00041     115: 'COND-YAW',
00042     177: 'DO-JUMP',
00043     178: 'DO-CHANGE-SPEED',
00044     181: 'DO-SET-RELAY',
00045     182: 'DO-REPEAT-RELAY',
00046     183: 'DO-SET-SERVO',
00047     184: 'DO-REPEAT-SERVO',
00048     201: 'DO-SET-ROI',
00049 }
00050 
00051 
00052 class WaypointFile(object):
00053     """Base class for waypoint file parsers"""
00054     def read(self, file_):
00055         """Returns a iterable of waypoints"""
00056         raise NotImplementedError
00057 
00058     def write(self, file_, waypoints):
00059         """Writes waypoints to file"""
00060         raise NotImplementedError
00061 
00062 
00063 class QGroundControlWP(WaypointFile):
00064     """Parse QGC waypoint file"""
00065 
00066     file_header = 'QGC WPL 120'
00067     known_versions = (110, 120)
00068 
00069     class CSVDialect(csv.Dialect):
00070         delimiter = '\t'
00071         doublequote = False
00072         skipinitialspace = True
00073         lineterminator = '\r\n'
00074         quoting = csv.QUOTE_NONE
00075 
00076     def read(self, file_):
00077         got_header = False
00078         for data in csv.reader(file_, self.CSVDialect):
00079             if data[0].startswith('#'):
00080                 continue; # skip comments (i think in next format version they add this)
00081 
00082             if not got_header:
00083                 qgc, wpl, ver = data[0].split(' ', 3)
00084                 ver = int(ver)
00085                 if qgc == 'QGC' and wpl == 'WPL' and ver in self.known_versions:
00086                     got_header = True
00087 
00088             else:
00089                 yield Waypoint(
00090                     is_current = bool(int(data[1])),
00091                     frame = int(data[2]),
00092                     command = int(data[3]),
00093                     param1 = float(data[4]),
00094                     param2 = float(data[5]),
00095                     param3 = float(data[6]),
00096                     param4 = float(data[7]),
00097                     x_lat = float(data[8]),
00098                     y_long = float(data[9]),
00099                     z_alt = float(data[10]),
00100                     autocontinue = bool(int(data[11]))
00101                 )
00102 
00103     def write(self, file_, waypoints):
00104         writer = csv.writer(file_, self.CSVDialect)
00105         writer.writerow((self.file_header ,))
00106         for seq, w in enumerate(waypoints):
00107             writer.writerow((
00108                 seq,
00109                 int(w.is_current),
00110                 w.frame,
00111                 w.command,
00112                 w.param1,
00113                 w.param2,
00114                 w.param3,
00115                 w.param4,
00116                 w.x_lat,
00117                 w.y_long,
00118                 w.z_alt,
00119                 int(w.autocontinue)
00120             ))
00121 
00122 
00123 
00124 pull = None
00125 push = None
00126 clear = None
00127 set_current = None
00128 
00129 
00130 def subscribe_waypoints(cb, **kvargs):
00131     return rospy.Subscriber(mavros.get_topic('mission', 'waypoints'), WaypointList, cb, **kvargs)
00132 
00133 
00134 def _setup_services():
00135     global pull, push, clear, set_current
00136 
00137     def _get_proxy(name, type):
00138         return rospy.ServiceProxy(mavros.get_topic('mission', name), type)
00139 
00140     pull = _get_proxy('pull', WaypointPull)
00141     push = _get_proxy('push', WaypointPush)
00142     clear = _get_proxy('clear', WaypointClear)
00143     set_current = _get_proxy('set_current', WaypointSetCurrent)
00144 
00145 
00146 # register updater
00147 mavros.register_on_namespace_update(_setup_services)


mavros
Author(s): Vladimir Ermakov
autogenerated on Thu Feb 9 2017 04:00:17