/
mission_import_export.py
172 lines (138 loc) · 5.63 KB
/
mission_import_export.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
© Copyright 2015-2016, 3D Robotics.
mission_import_export.py:
This example demonstrates how to import and export files in the Waypoint file format
(http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format). The commands are imported
into a list, and can be modified before saving and/or uploading.
Documentation is provided at http://python.dronekit.io/examples/mission_import_export.html
"""
from dronekit import connect, Command
import time
#Set up option parsing to get connection string
import argparse
parser = argparse.ArgumentParser(description='Demonstrates mission import/export from a file.')
parser.add_argument('--connect',
help="Vehicle connection target string. If not specified, SITL automatically started and used.")
args = parser.parse_args()
connection_string = args.connect
sitl = None
#Start SITL if no connection string specified
if not connection_string:
import dronekit_sitl
sitl = dronekit_sitl.start_default()
connection_string = sitl.connection_string()
# Connect to the Vehicle
print 'Connecting to vehicle on: %s' % connection_string
vehicle = connect(connection_string, wait_ready=True)
# Check that vehicle is armable.
# This ensures home_location is set (needed when saving WP file)
while not vehicle.is_armable:
print " Waiting for vehicle to initialise..."
time.sleep(1)
def readmission(aFileName):
"""
Load a mission from a file into a list. The mission definition is in the Waypoint file
format (http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format).
This function is used by upload_mission().
"""
print "\nReading mission from file: %s" % aFileName
cmds = vehicle.commands
missionlist=[]
with open(aFileName) as f:
for i, line in enumerate(f):
if i==0:
if not line.startswith('QGC WPL 110'):
raise Exception('File is not supported WP version')
else:
linearray=line.split('\t')
ln_index=int(linearray[0])
ln_currentwp=int(linearray[1])
ln_frame=int(linearray[2])
ln_command=int(linearray[3])
ln_param1=float(linearray[4])
ln_param2=float(linearray[5])
ln_param3=float(linearray[6])
ln_param4=float(linearray[7])
ln_param5=float(linearray[8])
ln_param6=float(linearray[9])
ln_param7=float(linearray[10])
ln_autocontinue=int(linearray[11].strip())
cmd = Command( 0, 0, 0, ln_frame, ln_command, ln_currentwp, ln_autocontinue, ln_param1, ln_param2, ln_param3, ln_param4, ln_param5, ln_param6, ln_param7)
missionlist.append(cmd)
return missionlist
def upload_mission(aFileName):
"""
Upload a mission from a file.
"""
#Read mission from file
missionlist = readmission(aFileName)
print "\nUpload mission from a file: %s" % import_mission_filename
#Clear existing mission from vehicle
print ' Clear mission'
cmds = vehicle.commands
cmds.clear()
#Add new mission to vehicle
for command in missionlist:
cmds.add(command)
print ' Upload mission'
vehicle.commands.upload()
def download_mission():
"""
Downloads the current mission and returns it in a list.
It is used in save_mission() to get the file information to save.
"""
print " Download mission from vehicle"
missionlist=[]
cmds = vehicle.commands
cmds.download()
cmds.wait_ready()
for cmd in cmds:
missionlist.append(cmd)
return missionlist
def save_mission(aFileName):
"""
Save a mission in the Waypoint file format
(http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format).
"""
print "\nSave mission from Vehicle to file: %s" % export_mission_filename
#Download mission from vehicle
missionlist = download_mission()
#Add file-format information
output='QGC WPL 110\n'
#Add home location as 0th waypoint
home = vehicle.home_location
output+="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n" % (0,1,0,16,0,0,0,0,home.lat,home.lon,home.alt,1)
#Add commands
for cmd in missionlist:
commandline="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n" % (cmd.seq,cmd.current,cmd.frame,cmd.command,cmd.param1,cmd.param2,cmd.param3,cmd.param4,cmd.x,cmd.y,cmd.z,cmd.autocontinue)
output+=commandline
with open(aFileName, 'w') as file_:
print " Write mission to file"
file_.write(output)
def printfile(aFileName):
"""
Print a mission file to demonstrate "round trip"
"""
print "\nMission file: %s" % aFileName
with open(aFileName) as f:
for line in f:
print ' %s' % line.strip()
import_mission_filename = 'mpmission.txt'
export_mission_filename = 'exportedmission.txt'
#Upload mission from file
upload_mission(import_mission_filename)
#Download mission we just uploaded and save to a file
save_mission(export_mission_filename)
#Close vehicle object before exiting script
print "Close vehicle object"
vehicle.close()
# Shut down simulator if it was started.
if sitl is not None:
sitl.stop()
print "\nShow original and uploaded/downloaded files:"
#Print original file (for demo purposes only)
printfile(import_mission_filename)
#Print exported file (for demo purposes only)
printfile(export_mission_filename)