Skip to content

Commit

Permalink
python: Move common mission classes to mavros.mission module.
Browse files Browse the repository at this point in the history 
Issue #157.
  • Loading branch information
@vooon
vooon committed Sep 17, 2014
1 parent 60b1df5 commit b0a16ad
Show file tree
Hide file tree
Showing 2 changed files with 132 additions and 112 deletions.
117 changes: 5 additions & 112 deletions mavros/scripts/mavwp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,128 +19,21 @@

from __future__ import print_function

import csv
import time
import argparse
import threading

import roslib; roslib.load_manifest("mavros")
import rospy
from mavros.utils import *
from mavros.msg import Waypoint, WaypointList
from mavros.srv import WaypointPull, WaypointPush, WaypointClear, \
WaypointSetCurrent, WaypointGOTO
from mavros.mission import *

no_prettytable = False
try:
from prettytable import PrettyTable
except ImportError:
print("Waring: 'show' action disabled. install python-prettytable", file=sys.stderr);
print("Waring: 'show' action disabled. install python-prettytable", file=sys.stderr)
no_prettytable = True


FRAMES = {
Waypoint.FRAME_GLOBAL: 'GAA',
Waypoint.FRAME_GLOBAL_REL_ALT: 'GRA',
Waypoint.FRAME_LOCAL_ENU: 'LOC-ENU',
Waypoint.FRAME_LOCAL_NED: 'LOC-NED',
Waypoint.FRAME_MISSION: 'MIS'
}

NAV_CMDS = {
Waypoint.NAV_LAND: 'LAND',
Waypoint.NAV_LOITER_TIME: 'LOITER-TIME',
Waypoint.NAV_LOITER_TURNS: 'LOITER-TURNS',
Waypoint.NAV_LOITER_UNLIM: 'LOITER-UNLIM',
Waypoint.NAV_RETURN_TO_LAUNCH: 'RTL',
Waypoint.NAV_TAKEOFF: 'TAKEOFF',
Waypoint.NAV_WAYPOINT: 'WAYPOINT',
# Maybe later i will add this enum to message
112: 'COND-DELAY',
113: 'COND-CHANGE-ALT',
114: 'COND-DISTANCE',
115: 'COND-YAW',
177: 'DO-JUMP',
178: 'DO-CHANGE-SPEED',
181: 'DO-SET-RELAY',
182: 'DO-REPEAT-RELAY',
183: 'DO-SET-SERVO',
184: 'DO-REPEAT-SERVO',
201: 'DO-SET-ROI',
}



class WaypointFile(object):
"""Base class for waypoint file parsers"""
def read(self, file_):
"""Returns a iterable of waypoints"""
raise NotImplementedError

def write(self, file_, waypoints):
"""Writes waypoints to file"""
raise NotImplementedError


class QGroundControlWP(WaypointFile):
"""Parse QGC waypoint file"""

file_header = 'QGC WPL 120'
known_versions = (110, 120)

class CSVDialect(csv.Dialect):
delimiter = '\t'
doublequote = False
skipinitialspace = True
lineterminator = '\r\n'
quoting = csv.QUOTE_NONE

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 write(self, file_, waypoints):
writer = csv.writer(file_, self.CSVDialect)
writer.writerow((self.file_header ,))
for seq, w in enumerate(waypoints):
writer.writerow((
seq,
int(w.is_current),
w.frame,
w.command,
w.param1,
w.param2,
w.param3,
w.param4,
w.x_lat,
w.y_long,
w.z_alt,
int(w.autocontinue)
))


def get_wp_file_io(args):
return QGroundControlWP()
Expand Down Expand Up @@ -214,10 +107,10 @@ def do_show(args):

# Waypoints topic is latched type, and it updates after pull
sub = rospy.Subscriber(args.mavros_ns + "/mission/waypoints", WaypointList, _show_table)
if not args.follow and not done_evt.wait(30.0):
fault("Something went wrong. Topic timed out.")
else:
if args.follow:
rospy.spin()
elif not done_evt.wait(30.0):
fault("Something went wrong. Topic timed out.")


def do_load(args):
Expand Down
127 changes: 127 additions & 0 deletions mavros/src/mavros/mission.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,127 @@
# -*- python -*-
# vim:set ts=4 sw=4 et:
#
# Copyright 2014 Vladimir Ermakov.
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
# for more details.
#
# You should have received a copy of the GNU General Public License along
# with this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA

import csv
import time

from mavros.msg import Waypoint, WaypointList
from mavros.srv import WaypointPull, WaypointPush, WaypointClear, \
WaypointSetCurrent, WaypointGOTO


FRAMES = {
Waypoint.FRAME_GLOBAL: 'GAA',
Waypoint.FRAME_GLOBAL_REL_ALT: 'GRA',
Waypoint.FRAME_LOCAL_ENU: 'LOC-ENU',
Waypoint.FRAME_LOCAL_NED: 'LOC-NED',
Waypoint.FRAME_MISSION: 'MIS'
}

NAV_CMDS = {
Waypoint.NAV_LAND: 'LAND',
Waypoint.NAV_LOITER_TIME: 'LOITER-TIME',
Waypoint.NAV_LOITER_TURNS: 'LOITER-TURNS',
Waypoint.NAV_LOITER_UNLIM: 'LOITER-UNLIM',
Waypoint.NAV_RETURN_TO_LAUNCH: 'RTL',
Waypoint.NAV_TAKEOFF: 'TAKEOFF',
Waypoint.NAV_WAYPOINT: 'WAYPOINT',
# Maybe later i will add this enum to message
112: 'COND-DELAY',
113: 'COND-CHANGE-ALT',
114: 'COND-DISTANCE',
115: 'COND-YAW',
177: 'DO-JUMP',
178: 'DO-CHANGE-SPEED',
181: 'DO-SET-RELAY',
182: 'DO-REPEAT-RELAY',
183: 'DO-SET-SERVO',
184: 'DO-REPEAT-SERVO',
201: 'DO-SET-ROI',
}


class WaypointFile(object):
"""Base class for waypoint file parsers"""
def read(self, file_):
"""Returns a iterable of waypoints"""
raise NotImplementedError

def write(self, file_, waypoints):
"""Writes waypoints to file"""
raise NotImplementedError


class QGroundControlWP(WaypointFile):
"""Parse QGC waypoint file"""

file_header = 'QGC WPL 120'
known_versions = (110, 120)

class CSVDialect(csv.Dialect):
delimiter = '\t'
doublequote = False
skipinitialspace = True
lineterminator = '\r\n'
quoting = csv.QUOTE_NONE

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 write(self, file_, waypoints):
writer = csv.writer(file_, self.CSVDialect)
writer.writerow((self.file_header ,))
for seq, w in enumerate(waypoints):
writer.writerow((
seq,
int(w.is_current),
w.frame,
w.command,
w.param1,
w.param2,
w.param3,
w.param4,
w.x_lat,
w.y_long,
w.z_alt,
int(w.autocontinue)
))

0 comments on commit b0a16ad

Please sign in to comment.