00001
00002
00003
00004
00005
00006
00007
00008
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
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;
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
00147 mavros.register_on_namespace_update(_setup_services)