mission.py
Go to the documentation of this file.
1 # -*- python -*-
2 # vim:set ts=4 sw=4 et:
3 #
4 # Copyright 2014 Vladimir Ermakov.
5 #
6 # This file is part of the mavros package and subject to the license terms
7 # in the top-level LICENSE file of the mavros repository.
8 # https://github.com/mavlink/mavros/tree/master/LICENSE.md
9 
10 import csv
11 import time
12 
13 import rospy
14 import mavros
15 
16 from mavros_msgs.msg import Waypoint, WaypointList, CommandCode
17 from mavros_msgs.srv import WaypointPull, WaypointPush, WaypointClear, \
18  WaypointSetCurrent
19 
20 
21 FRAMES = {
22  Waypoint.FRAME_GLOBAL: 'GAA',
23  Waypoint.FRAME_GLOBAL_REL_ALT: 'GRA',
24  Waypoint.FRAME_LOCAL_ENU: 'LOC-ENU',
25  Waypoint.FRAME_LOCAL_NED: 'LOC-NED',
26  Waypoint.FRAME_MISSION: 'MIS'
27 }
28 
29 NAV_CMDS = {
30  CommandCode.NAV_LAND: 'LAND',
31  CommandCode.NAV_LOITER_TIME: 'LOITER-TIME',
32  CommandCode.NAV_LOITER_TURNS: 'LOITER-TURNS',
33  CommandCode.NAV_LOITER_UNLIM: 'LOITER-UNLIM',
34  CommandCode.NAV_RETURN_TO_LAUNCH: 'RTL',
35  CommandCode.NAV_TAKEOFF: 'TAKEOFF',
36  CommandCode.NAV_WAYPOINT: 'WAYPOINT',
37  # Maybe later i will add this enum to message
38  112: 'COND-DELAY',
39  113: 'COND-CHANGE-ALT',
40  114: 'COND-DISTANCE',
41  115: 'COND-YAW',
42  177: 'DO-JUMP',
43  178: 'DO-CHANGE-SPEED',
44  181: 'DO-SET-RELAY',
45  182: 'DO-REPEAT-RELAY',
46  183: 'DO-SET-SERVO',
47  184: 'DO-REPEAT-SERVO',
48  201: 'DO-SET-ROI',
49 }
50 
51 
52 class WaypointFile(object):
53  """Base class for waypoint file parsers"""
54  def read(self, file_):
55  """Returns a iterable of waypoints"""
56  raise NotImplementedError
57 
58  def write(self, file_, waypoints):
59  """Writes waypoints to file"""
60  raise NotImplementedError
61 
62 
64  """Parse QGC waypoint file"""
65 
66  file_header = 'QGC WPL 120'
67  known_versions = (110, 120)
68 
69  class CSVDialect(csv.Dialect):
70  delimiter = '\t'
71  doublequote = False
72  skipinitialspace = True
73  lineterminator = '\r\n'
74  quoting = csv.QUOTE_NONE
75 
76  def read(self, file_):
77  got_header = False
78  for data in csv.reader(file_, self.CSVDialect):
79  if data[0].startswith('#'):
80  continue; # skip comments (i think in next format version they add this)
81 
82  if not got_header:
83  qgc, wpl, ver = data[0].split(' ', 3)
84  ver = int(ver)
85  if qgc == 'QGC' and wpl == 'WPL' and ver in self.known_versions:
86  got_header = True
87 
88  else:
89  yield Waypoint(
90  is_current = bool(int(data[1])),
91  frame = int(data[2]),
92  command = int(data[3]),
93  param1 = float(data[4]),
94  param2 = float(data[5]),
95  param3 = float(data[6]),
96  param4 = float(data[7]),
97  x_lat = float(data[8]),
98  y_long = float(data[9]),
99  z_alt = float(data[10]),
100  autocontinue = bool(int(data[11]))
101  )
102 
103  def write(self, file_, waypoints):
104  writer = csv.writer(file_, self.CSVDialect)
105  writer.writerow((self.file_header ,))
106  for seq, w in enumerate(waypoints):
107  writer.writerow((
108  seq,
109  int(w.is_current),
110  w.frame,
111  w.command,
112  w.param1,
113  w.param2,
114  w.param3,
115  w.param4,
116  w.x_lat,
117  w.y_long,
118  w.z_alt,
119  int(w.autocontinue)
120  ))
121 
122 
123 
124 pull = None
125 push = None
126 clear = None
127 set_current = None
128 
129 
130 def subscribe_waypoints(cb, **kvargs):
131  return rospy.Subscriber(mavros.get_topic('mission', 'waypoints'), WaypointList, cb, **kvargs)
132 
133 
135  global pull, push, clear, set_current
136 
137  def _get_proxy(name, type):
138  return rospy.ServiceProxy(mavros.get_topic('mission', name), type)
139 
140  pull = _get_proxy('pull', WaypointPull)
141  push = _get_proxy('push', WaypointPush)
142  clear = _get_proxy('clear', WaypointClear)
143  set_current = _get_proxy('set_current', WaypointSetCurrent)
144 
145 
146 # register updater
def subscribe_waypoints(cb, kvargs)
Definition: mission.py:130
def read(self, file_)
Definition: mission.py:54
def _get_proxy(service, type)
Definition: command.py:26
def read(self, file_)
Definition: mission.py:76
def write(self, file_, waypoints)
Definition: mission.py:58
def get_topic(args)
Definition: __init__.py:49
def write(self, file_, waypoints)
Definition: mission.py:103
def _setup_services()
Definition: mission.py:134
def register_on_namespace_update(cb)
Definition: __init__.py:41


mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 1 2021 02:36:26