2016-02-23 11:02:20 -04:00
#!/usr/bin/env python
2016-02-17 19:27:05 -04:00
################################################################################################
# @File MissionCheck.py
# Automated mission loading, execution and monitoring
# for Continuous Integration
#
# @author Sander Smeets <sander@droneslab.com>
#
2016-02-18 09:54:13 -04:00
# Code partly based on DroneKit (c) Copyright 2015-2016, 3D Robotics.
2016-02-17 19:27:05 -04:00
################################################################################################
2016-02-17 18:08:59 -04:00
################################################################################################
# Settings
################################################################################################
2019-09-22 11:42:15 -03:00
from __future__ import print_function
2016-02-17 19:27:05 -04:00
connection_string = ' 127.0.0.1:14540 '
2016-06-28 10:41:19 -03:00
import_mission_filename = ' VTOL_TAKEOFF.mission '
max_execution_time = 200
alt_acceptance_radius = 5
2016-02-17 18:08:59 -04:00
################################################################################################
# Init
################################################################################################
# Import DroneKit-Python
2016-12-28 05:12:53 -04:00
from dronekit import connect , Command , VehicleMode
2016-02-17 19:27:05 -04:00
from pymavlink import mavutil
2016-12-28 05:33:59 -04:00
import time , sys , argparse , json
2016-02-18 09:54:13 -04:00
parser = argparse . ArgumentParser ( )
parser . add_argument ( " -c " , " --connect " , help = " connection string " )
parser . add_argument ( " -f " , " --filename " , help = " mission filename " )
2016-02-19 06:47:34 -04:00
parser . add_argument ( " -t " , " --timeout " , help = " execution timeout " , type = float )
parser . add_argument ( " -a " , " --altrad " , help = " altitude acceptance radius " , type = float )
2016-02-18 09:54:13 -04:00
args = parser . parse_args ( )
if args . connect :
connection_string = args . connect
if args . filename :
import_mission_filename = args . filename
if args . timeout :
max_execution_time = args . timeout
2016-02-18 21:57:24 -04:00
if args . altrad :
alt_acceptance_radius = args . altrad
2016-02-18 09:54:13 -04:00
2016-02-18 21:57:24 -04:00
mission_failed = False
2016-02-17 19:27:05 -04:00
MAV_MODE_AUTO = 4
2016-02-17 18:08:59 -04:00
# start time counter
start_time = time . time ( )
elapsed_time = time . time ( ) - start_time
2016-02-18 09:54:13 -04:00
2016-02-17 19:50:28 -04:00
# Connect to the Vehicle
2019-09-22 11:42:15 -03:00
print ( " Connecting " )
2016-02-18 21:57:24 -04:00
vehicle = connect ( connection_string , wait_ready = True )
while not vehicle . system_status . state == " STANDBY " or vehicle . gps_0 . fix_type < 3 :
if time . time ( ) - start_time > 20 :
2019-09-22 11:42:15 -03:00
print ( " FAILED: SITL did not reach standby with GPS fix within 20 seconds " )
2016-02-18 21:57:24 -04:00
sys . exit ( 98 )
2019-09-22 11:42:15 -03:00
print ( " Waiting for vehicle to initialise... %s " % vehicle . system_status . state )
2016-02-18 21:57:24 -04:00
time . sleep ( 1 )
2016-02-17 18:08:59 -04:00
2016-02-18 09:54:13 -04:00
# Display basic vehicle state
2019-09-22 11:42:15 -03:00
print ( " Type: %s " % vehicle . _vehicle_type )
print ( " Armed?: %s " % vehicle . armed )
print ( " System status: %s " % vehicle . system_status . state )
print ( " GPS: %s " % vehicle . gps_0 )
print ( " Alt: %s " % vehicle . location . global_relative_frame . alt )
2016-02-18 09:54:13 -04:00
2016-02-17 18:08:59 -04:00
################################################################################################
# Functions
################################################################################################
2016-12-28 05:33:59 -04:00
def read_mission_json ( f ) :
d = json . load ( f )
current = True
2016-02-17 18:08:59 -04:00
missionlist = [ ]
2016-12-28 05:33:59 -04:00
for wp in d [ ' items ' ] :
2019-09-22 15:16:14 -03:00
cmd = Command ( 0 , 0 , 0 , int ( wp [ ' frame ' ] ) , int ( wp [ ' command ' ] ) , current , int ( wp [ ' autoContinue ' ] ) , float ( wp [ ' param1 ' ] ) , float ( wp [ ' param2 ' ] ) , float ( wp [ ' param3 ' ] ) , float ( wp [ ' param4 ' ] ) , float ( wp [ ' coordinate ' ] [ 0 ] ) , float ( wp [ ' coordinate ' ] [ 1 ] ) , float ( wp [ ' coordinate ' ] [ 2 ] ) )
missionlist . append ( cmd )
if current :
current = False
2016-02-17 18:08:59 -04:00
return missionlist
def upload_mission ( aFileName ) :
"""
Upload a mission from a file .
"""
#Read mission from file
2016-12-28 05:33:59 -04:00
with open ( aFileName ) as f :
2019-09-22 15:16:14 -03:00
missionlist = read_mission_json ( f )
2016-02-17 18:08:59 -04:00
#Clear existing mission from vehicle
cmds = vehicle . commands
cmds . clear ( )
#Add new mission to vehicle
for command in missionlist :
cmds . add ( command )
2019-09-22 11:42:15 -03:00
print ( ' Uploaded mission with %s items ' % len ( missionlist ) )
2016-02-17 18:08:59 -04:00
vehicle . commands . upload ( )
return missionlist
2016-02-18 21:57:24 -04:00
2016-02-17 18:08:59 -04:00
################################################################################################
# Listeners
################################################################################################
current_sequence = - 1
2016-02-18 21:57:24 -04:00
current_sequence_changed = False
2016-02-17 18:08:59 -04:00
current_landed_state = - 1
2016-02-18 21:57:24 -04:00
home_position_set = False
2016-02-17 18:08:59 -04:00
#Create a message listener for mission sequence number
@vehicle.on_message ( ' MISSION_CURRENT ' )
def listener ( self , name , mission_current ) :
2016-02-18 21:57:24 -04:00
global current_sequence , current_sequence_changed
2019-09-22 15:16:14 -03:00
if ( current_sequence != mission_current . seq ) :
2016-02-17 18:08:59 -04:00
current_sequence = mission_current . seq ;
2016-02-18 21:57:24 -04:00
current_sequence_changed = True
2019-09-22 11:42:15 -03:00
print ( ' current mission sequence: %s ' % mission_current . seq )
2016-02-17 18:08:59 -04:00
#Create a message listener for mission sequence number
@vehicle.on_message ( ' EXTENDED_SYS_STATE ' )
def listener ( self , name , extended_sys_state ) :
global current_landed_state
2019-09-22 15:16:14 -03:00
if ( current_landed_state != extended_sys_state . landed_state ) :
2016-02-17 18:08:59 -04:00
current_landed_state = extended_sys_state . landed_state ;
2016-02-18 21:57:24 -04:00
#Create a message listener for home position fix
@vehicle.on_message ( ' HOME_POSITION ' )
def listener ( self , name , home_position ) :
global home_position_set
home_position_set = True
2016-02-17 18:08:59 -04:00
################################################################################################
# Start mission test
################################################################################################
2016-02-18 21:57:24 -04:00
while not home_position_set :
if time . time ( ) - start_time > 30 :
2019-09-22 11:42:15 -03:00
print ( " FAILED: getting home position 30 seconds " )
2016-02-18 21:57:24 -04:00
sys . exit ( 98 )
2019-09-22 11:42:15 -03:00
print ( " Waiting for home position... " )
2016-02-18 21:57:24 -04:00
time . sleep ( 1 )
2016-02-17 18:08:59 -04:00
#Upload mission from file
missionlist = upload_mission ( import_mission_filename )
time . sleep ( 2 )
2016-12-28 05:12:53 -04:00
# set mission mode
vehicle . mode = VehicleMode ( " MISSION " )
2016-02-17 18:08:59 -04:00
time . sleep ( 1 )
2016-02-18 21:57:24 -04:00
2016-02-17 18:08:59 -04:00
# Arm vehicle
vehicle . armed = True
2016-02-18 21:57:24 -04:00
while not vehicle . system_status . state == " ACTIVE " :
if time . time ( ) - start_time > 30 :
2019-09-22 11:42:15 -03:00
print ( " FAILED: vehicle did not arm within 30 seconds " )
2016-02-18 21:57:24 -04:00
sys . exit ( 98 )
2019-09-22 11:42:15 -03:00
print ( " Waiting for vehicle to arm... " )
2016-02-18 21:57:24 -04:00
time . sleep ( 1 )
2016-02-17 18:08:59 -04:00
# Wait for completion of mission items
while ( current_sequence < len ( missionlist ) - 1 and elapsed_time < max_execution_time ) :
2016-02-19 06:47:34 -04:00
time . sleep ( .2 )
2016-02-18 21:57:24 -04:00
if current_sequence > 0 and current_sequence_changed :
if missionlist [ current_sequence - 1 ] . z - alt_acceptance_radius > vehicle . location . global_relative_frame . alt or missionlist [ current_sequence - 1 ] . z + alt_acceptance_radius < vehicle . location . global_relative_frame . alt :
2019-09-22 11:42:15 -03:00
print ( " waypoint %s out of bounds altitude %s gps altitude: %s " % ( current_sequence , missionlist [ current_sequence - 1 ] . z , vehicle . location . global_relative_frame . alt ) )
2016-02-18 21:57:24 -04:00
mission_failed = True
current_sequence_changed = False
2016-02-17 18:08:59 -04:00
elapsed_time = time . time ( ) - start_time
2016-02-18 21:57:24 -04:00
if elapsed_time < max_execution_time :
2019-09-22 11:42:15 -03:00
print ( " Mission items have been executed " )
2016-02-17 18:08:59 -04:00
# wait for the vehicle to have landed
while ( current_landed_state != 1 and elapsed_time < max_execution_time ) :
time . sleep ( 1 )
elapsed_time = time . time ( ) - start_time
2016-02-18 21:57:24 -04:00
if elapsed_time < max_execution_time :
2019-09-22 11:42:15 -03:00
print ( " Vehicle has landed " )
2016-02-17 18:08:59 -04:00
# Disarm vehicle
vehicle . armed = False
# count elapsed time
elapsed_time = time . time ( ) - start_time
# Close vehicle object before exiting script
vehicle . close ( )
2016-02-18 21:57:24 -04:00
time . sleep ( 2 )
2016-02-17 18:08:59 -04:00
# Validate time constraint
2016-02-18 21:57:24 -04:00
if elapsed_time < = max_execution_time and not mission_failed :
2019-09-22 11:42:15 -03:00
print ( " Mission succesful time elapsed %s " % elapsed_time )
2016-02-17 18:08:59 -04:00
sys . exit ( 0 )
2016-02-18 21:57:24 -04:00
if elapsed_time > max_execution_time :
2019-09-22 11:42:15 -03:00
print ( " Mission FAILED to execute within %s seconds " % max_execution_time )
2016-02-18 21:57:24 -04:00
sys . exit ( 99 )
if mission_failed :
2019-09-22 11:42:15 -03:00
print ( " Mission FAILED out of bounds " )
2016-02-18 21:57:24 -04:00
sys . exit ( 100 )
2019-09-22 11:42:15 -03:00
print ( " Mission FAILED something strange happened " )
2016-02-18 21:57:24 -04:00
sys . exit ( 101 )