Skip to content

Commit

Permalink
Update Sega-image.py
Browse files Browse the repository at this point in the history
  • Loading branch information
@nursultanramazanov
nursultanramazanov authored Oct 31, 2024
1 parent 46c7353 commit e0aad7d
Showing 1 changed file with 31 additions and 296 deletions.
327 changes: 31 additions & 296 deletions .github/workflows/Sega-image.py
View file
Original file line number Diff line number Diff line change
@@ -1,301 +1,36 @@
name: Docker Image CI

on: #!/usr/bin/env python

################################################################################################
# @File MissionCheck.py
# Automated mission loading, execution and monitoring
# for Continuous Integration
#
# @author <>
#
# Code partly based on DroneKit (c) Copyright 2015-2016, 3D Robotics.
################################################################################################


################################################################################################
# Settings
################################################################################################

connection_string = '127.0.0.1:14540'

import_mission_filename = 'VTOLmission.txt'
max_execution_time = 250
alt_acceptance_radius = 10

################################################################################################
# Init
################################################################################################

# Import DroneKit-Python
from dronekit import connect, Command
from pymavlink import mavutil
import time, sys, argparse

parser = argparse.ArgumentParser()
parser.add_argument("-c", "--connect", help="connection string")
parser.add_argument("-f", "--filename", help="mission filename")
parser.add_argument("-t", "--timeout", help="execution timeout", type=float)
parser.add_argument("-a", "--altrad", help="altitude acceptance radius", type=float)
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
if args.altrad:
alt_acceptance_radius = args.altrad



mission_failed = False
MAV_MODE_AUTO = 4

# start time counter
start_time = time.time()
elapsed_time = time.time() - start_time



# Connect to the Vehicle
print "Connecting"
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:
print "FAILED: SITL did not reach standby with GPS fix within 20 seconds"
sys.exit(98)
print "Waiting for vehicle to initialise... %s " % vehicle.system_status.state
time.sleep(1)

# Display basic vehicle state
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


################################################################################################
# Functions
################################################################################################


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().
"""
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)

#Clear existing mission from vehicle
cmds = vehicle.commands
cmds.clear()
#Add new mission to vehicle
for command in missionlist:
cmds.add(command)
print ' Uploaded mission with %s items' % len(missionlist)
vehicle.commands.upload()
return missionlist


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()


################################################################################################
# Listeners
################################################################################################

current_sequence = -1
current_sequence_changed = False
current_landed_state = -1
home_position_set = False

#Create a message listener for mission sequence number
@vehicle.on_message('MISSION_CURRENT')
def listener(self, name, mission_current):
global current_sequence, current_sequence_changed
if (current_sequence <> mission_current.seq):
current_sequence = mission_current.seq;
current_sequence_changed = True
print 'current mission sequence: %s' % mission_current.seq

#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
if (current_landed_state <> extended_sys_state.landed_state):
current_landed_state = extended_sys_state.landed_state;

#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



################################################################################################
# Start mission test
################################################################################################


while not home_position_set:
if time.time() - start_time > 30:
print "FAILED: getting home position 30 seconds"
sys.exit(98)
print "Waiting for home position..."
time.sleep(1)


#Upload mission from file
missionlist = upload_mission(import_mission_filename)
time.sleep(2)

# set mission mode the hard way
vehicle._master.mav.command_long_send(vehicle._master.target_system, vehicle._master.target_component,
mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0,
MAV_MODE_AUTO,
0, 0, 0, 0, 0, 0)
time.sleep(1)


# Arm vehicle
vehicle.armed = True

while not vehicle.system_status.state == "ACTIVE":
if time.time() - start_time > 30:
print "FAILED: vehicle did not arm within 30 seconds"
sys.exit(98)
print "Waiting for vehicle to arm..."
time.sleep(1)



# Wait for completion of mission items
while (current_sequence < len(missionlist)-1 and elapsed_time < max_execution_time):
time.sleep(.2)
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:
print "waypoint %s out of bounds altitude %s gps altitude: %s" % (current_sequence, missionlist[current_sequence-1].z, vehicle.location.global_relative_frame.alt)
mission_failed = True
current_sequence_changed = False
elapsed_time = time.time() - start_time

if elapsed_time < max_execution_time:
print "Mission items have been executed"

# 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

if elapsed_time < max_execution_time:
print "Vehicle has landed"

# Disarm vehicle
vehicle.armed = False

# count elapsed time
elapsed_time = time.time() - start_time

# Close vehicle object before exiting script
vehicle.close()
time.sleep(2)

# Validate time constraint
if elapsed_time <= max_execution_time and not mission_failed:
print "Mission succesful time elapsed %s" % elapsed_time
sys.exit(0)

if elapsed_time > max_execution_time:
print "Mission FAILED to execute within %s seconds" % max_execution_time
sys.exit(99)

if mission_failed:
print "Mission FAILED out of bounds"
sys.exit(100)

print "Mission FAILED something strange happened"
sys.exit(101)
on: # Workflow derived from https://github.com/r-lib/actions/tree/master/examples
# Need help debugging build failures? Start at https://github.com/r-lib/actions#where-to-find-help
on:
push:
branches: [main, master, dev]
pull_request:
branches: [main, master, dev]

name: R-CMD-check

jobs:
R-CMD-check:
runs-on: ubuntu-20.04
env:
GITHUB_PAT: ${{ secrets.GITHUB_TOKEN }}
R_KEEP_PKG_SOURCE: yes
steps:
- uses: actions/checkout@v2

- uses: r-lib/actions/setup-r@v2
with:
use-public-rspm: true

- uses: r-lib/actions/setup-r-dependencies@v2
with:
extra-packages: any::rcmdcheck

- uses: r-lib/actions/check-r-package@v2
with:
args: 'c("--no-build-vignettes", "--no-manual", "--as-cran")'
error-on: '"error"'
push: # Doxyfile 1.8.10

# This file describes the settings to be used by the documentation system
Expand Down

0 comments on commit e0aad7d

Please sign in to comment.