Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/Tools/autotest/pysim/util.py

829 lines
25 KiB
Python
Raw Blame History

from __future__ import print_function
'''
AP_FLAKE8_CLEAN
'''
import atexit
import math
import os
import re
import shlex
import signal
import subprocess
import sys
import tempfile
import time
import pexpect
if sys.version_info[0] >= 3:
ENCODING = 'ascii'
else:
ENCODING = None
RADIUS_OF_EARTH = 6378100.0 # in meters
# List of open terminal windows for macosx
windowID = []
def m2ft(x):
"""Meters to feet."""
return float(x) / 0.3048
def ft2m(x):
"""Feet to meters."""
return float(x) * 0.3048
def kt2mps(x):
return x * 0.514444444
def mps2kt(x):
return x / 0.514444444
def topdir():
"""Return top of git tree where autotest is running from."""
d = os.path.dirname(os.path.realpath(__file__))
assert os.path.basename(d) == 'pysim'
d = os.path.dirname(d)
assert os.path.basename(d) == 'autotest'
d = os.path.dirname(d)
assert os.path.basename(d) == 'Tools'
d = os.path.dirname(d)
return d
def relcurdir(path):
"""Return a path relative to current dir"""
return os.path.relpath(path, os.getcwd())
def reltopdir(path):
"""Returns the normalized ABSOLUTE path for 'path', where path is a path relative to topdir"""
return os.path.normpath(os.path.join(topdir(), path))
def run_cmd(cmd, directory=".", show=True, output=False, checkfail=True):
"""Run a shell command."""
shell = False
if not isinstance(cmd, list):
cmd = [cmd]
shell = True
if show:
print("Running: (%s) in (%s)" % (cmd_as_shell(cmd), directory,))
if output:
return subprocess.Popen(cmd, shell=shell, stdout=subprocess.PIPE, cwd=directory).communicate()[0]
elif checkfail:
return subprocess.check_call(cmd, shell=shell, cwd=directory)
else:
return subprocess.call(cmd, shell=shell, cwd=directory)
def rmfile(path):
"""Remove a file if it exists."""
try:
os.unlink(path)
except (OSError, FileNotFoundError):
pass
def deltree(path):
"""Delete a tree of files."""
run_cmd('rm -rf %s' % path)
def relwaf():
return "./modules/waf/waf-light"
def waf_configure(board,
j=None,
debug=False,
math_check_indexes=False,
coverage=False,
ekf_single=False,
postype_single=False,
force_32bit=False,
extra_args=[],
extra_hwdef=None,
ubsan=False,
ubsan_abort=False,
num_aux_imus=0,
dronecan_tests=False,
extra_defines={}):
cmd_configure = [relwaf(), "configure", "--board", board]
if debug:
cmd_configure.append('--debug')
if coverage:
cmd_configure.append('--coverage')
if math_check_indexes:
cmd_configure.append('--enable-math-check-indexes')
if ekf_single:
cmd_configure.append('--ekf-single')
if postype_single:
cmd_configure.append('--postype-single')
if force_32bit:
cmd_configure.append('--force-32bit')
if ubsan:
cmd_configure.append('--ubsan')
if ubsan_abort:
cmd_configure.append('--ubsan-abort')
if num_aux_imus > 0:
cmd_configure.append('--num-aux-imus=%u' % num_aux_imus)
if dronecan_tests:
cmd_configure.append('--enable-dronecan-tests')
if extra_hwdef is not None:
cmd_configure.extend(['--extra-hwdef', extra_hwdef])
for nv in extra_defines.items():
cmd_configure.extend(['--define', "%s=%s" % nv])
if j is not None:
cmd_configure.extend(['-j', str(j)])
pieces = [shlex.split(x) for x in extra_args]
for piece in pieces:
cmd_configure.extend(piece)
run_cmd(cmd_configure, directory=topdir(), checkfail=True)
def waf_clean():
run_cmd([relwaf(), "clean"], directory=topdir(), checkfail=True)
def waf_build(target=None):
cmd = [relwaf(), "build"]
if target is not None:
cmd.append(target)
run_cmd(cmd, directory=topdir(), checkfail=True)
def build_SITL(
build_target,
board='sitl',
clean=True,
configure=True,
coverage=False,
debug=False,
ekf_single=False,
extra_configure_args=[],
extra_defines={},
j=None,
math_check_indexes=False,
postype_single=False,
force_32bit=False,
ubsan=False,
ubsan_abort=False,
num_aux_imus=0,
dronecan_tests=False,
):
# first configure
if configure:
waf_configure(board,
j=j,
debug=debug,
math_check_indexes=math_check_indexes,
ekf_single=ekf_single,
postype_single=postype_single,
coverage=coverage,
force_32bit=force_32bit,
ubsan=ubsan,
ubsan_abort=ubsan_abort,
extra_defines=extra_defines,
num_aux_imus=num_aux_imus,
dronecan_tests=dronecan_tests,
extra_args=extra_configure_args,)
# then clean
if clean:
waf_clean()
# then build
cmd_make = [relwaf(), "build", "--target", build_target]
if j is not None:
cmd_make.extend(['-j', str(j)])
run_cmd(cmd_make, directory=topdir(), checkfail=True, show=True)
return True
def build_examples(board, j=None, debug=False, clean=False, configure=True, math_check_indexes=False, coverage=False,
ekf_single=False, postype_single=False, force_32bit=False, ubsan=False, ubsan_abort=False,
num_aux_imus=0, dronecan_tests=False,
extra_configure_args=[]):
# first configure
if configure:
waf_configure(board,
j=j,
debug=debug,
math_check_indexes=math_check_indexes,
ekf_single=ekf_single,
postype_single=postype_single,
coverage=coverage,
force_32bit=force_32bit,
ubsan=ubsan,
ubsan_abort=ubsan_abort,
extra_args=extra_configure_args,
dronecan_tests=dronecan_tests)
# then clean
if clean:
waf_clean()
# then build
cmd_make = [relwaf(), "examples"]
run_cmd(cmd_make, directory=topdir(), checkfail=True, show=True)
return True
def build_replay(board, j=None, debug=False, clean=False):
# first configure
waf_configure(board, j=j, debug=debug)
# then clean
if clean:
waf_clean()
# then build
cmd_make = [relwaf(), "replay"]
run_cmd(cmd_make, directory=topdir(), checkfail=True, show=True)
return True
def build_tests(board,
j=None,
debug=False,
clean=False,
configure=True,
math_check_indexes=False,
coverage=False,
ekf_single=False,
postype_single=False,
force_32bit=False,
ubsan=False,
ubsan_abort=False,
num_aux_imus=0,
dronecan_tests=False,
extra_configure_args=[]):
# first configure
if configure:
waf_configure(board,
j=j,
debug=debug,
math_check_indexes=math_check_indexes,
ekf_single=ekf_single,
postype_single=postype_single,
coverage=coverage,
force_32bit=force_32bit,
ubsan=ubsan,
ubsan_abort=ubsan_abort,
num_aux_imus=num_aux_imus,
dronecan_tests=dronecan_tests,
extra_args=extra_configure_args,)
# then clean
if clean:
waf_clean()
# then build
run_cmd([relwaf(), "tests"], directory=topdir(), checkfail=True, show=True)
return True
# list of pexpect children to close on exit
close_list = []
def pexpect_autoclose(p):
"""Mark for autoclosing."""
global close_list
close_list.append(p)
def pexpect_close(p):
"""Close a pexpect child."""
global close_list
ex = None
if p is None:
print("Nothing to close")
return
try:
p.kill(signal.SIGTERM)
except IOError as e:
print("Caught exception: %s" % str(e))
ex = e
pass
if ex is None:
# give the process some time to go away
for i in range(20):
if not p.isalive():
break
time.sleep(0.05)
try:
p.close()
except Exception:
pass
try:
p.close(force=True)
except Exception:
pass
if p in close_list:
close_list.remove(p)
def pexpect_close_all():
"""Close all pexpect children."""
global close_list
for p in close_list[:]:
pexpect_close(p)
def pexpect_drain(p):
"""Drain any pending input."""
try:
p.read_nonblocking(1000, timeout=0)
except Exception:
pass
def cmd_as_shell(cmd):
return (" ".join(['"%s"' % x for x in cmd]))
def make_safe_filename(text):
"""Return a version of text safe for use as a filename."""
r = re.compile("([^a-zA-Z0-9_.+-])")
text.replace('/', '-')
filename = r.sub(lambda m: str(hex(ord(str(m.group(1))))).upper(), text)
return filename
def valgrind_log_filepath(binary, model):
return make_safe_filename('%s-%s-valgrind.log' % (os.path.basename(binary), model,))
def kill_screen_gdb():
cmd = ["screen", "-X", "-S", "ardupilot-gdb", "quit"]
subprocess.Popen(cmd)
def kill_mac_terminal():
global windowID
for window in windowID:
cmd = ("osascript -e \'tell application \"Terminal\" to close "
"(window(get index of window id %s))\'" % window)
os.system(cmd)
class FakeMacOSXSpawn(object):
"""something that looks like a pspawn child so we can ignore attempts
to pause (and otherwise kill(1) SITL. MacOSX using osascript to
start/stop sitl
"""
def __init__(self):
pass
def progress(self, message):
print(message)
def kill(self, sig):
# self.progress("FakeMacOSXSpawn: ignoring kill(%s)" % str(sig))
pass
def isalive(self):
self.progress("FakeMacOSXSpawn: assuming process is alive")
return True
class PSpawnStdPrettyPrinter(object):
'''a fake filehandle-like object which prefixes a string to all lines
before printing to stdout/stderr. To be used to pass to
pexpect.spawn's logfile argument
'''
def __init__(self, output=sys.stdout, prefix="stdout"):
self.output = output
self.prefix = prefix
self.buffer = ""
def close(self):
self.print_prefixed_line(self.buffer)
def write(self, data):
self.buffer += data
lines = self.buffer.split("\n")
self.buffer = lines[-1]
lines.pop()
for line in lines:
self.print_prefixed_line(line)
def print_prefixed_line(self, line):
print("%s: %s" % (self.prefix, line), file=self.output)
def flush(self):
pass
def start_SITL(binary,
valgrind=False,
callgrind=False,
gdb=False,
gdb_no_tui=False,
wipe=False,
synthetic_clock=True,
home=None,
model=None,
speedup=1,
sim_rate_hz=None,
defaults_filepath=None,
unhide_parameters=False,
gdbserver=False,
breakpoints=[],
disable_breakpoints=False,
customisations=[],
lldb=False,
enable_fgview_output=False,
supplementary=False,
stdout_prefix=None):
if model is None and not supplementary:
raise ValueError("model must not be None")
"""Launch a SITL instance."""
cmd = []
if (callgrind or valgrind) and os.path.exists('/usr/bin/valgrind'):
# we specify a prefix for vgdb-pipe because on Vagrant virtual
# machines the pipes are created on the mountpoint for the
# shared directory with the host machine. mmap's,
# unsurprisingly, fail on files created on that mountpoint.
vgdb_prefix = os.path.join(tempfile.gettempdir(), "vgdb-pipe")
log_file = valgrind_log_filepath(binary=binary, model=model)
cmd.extend([
'valgrind',
# adding this option allows valgrind to cope with the overload
# of operator new
"--soname-synonyms=somalloc=nouserintercepts",
'--vgdb-prefix=%s' % vgdb_prefix,
'-q',
'--log-file=%s' % log_file])
if callgrind:
cmd.extend(["--tool=callgrind"])
if gdbserver:
cmd.extend(['gdbserver', 'localhost:3333'])
if gdb:
# attach gdb to the gdbserver:
f = open("/tmp/x.gdb", "w")
f.write("target extended-remote localhost:3333\nc\n")
for breakingpoint in breakpoints:
f.write("b %s\n" % (breakingpoint,))
if disable_breakpoints:
f.write("disable\n")
f.close()
run_cmd('screen -d -m -S ardupilot-gdbserver '
'bash -c "gdb -x /tmp/x.gdb"')
elif gdb:
f = open("/tmp/x.gdb", "w")
f.write("set pagination off\n")
for breakingpoint in breakpoints:
f.write("b %s\n" % (breakingpoint,))
if disable_breakpoints:
f.write("disable\n")
if not gdb_no_tui:
f.write("tui enable\n")
f.write("r\n")
f.close()
if sys.platform == "darwin" and os.getenv('DISPLAY'):
cmd.extend(['gdb', '-x', '/tmp/x.gdb', '--args'])
elif os.environ.get('DISPLAY'):
cmd.extend(['xterm', '-e', 'gdb', '-x', '/tmp/x.gdb', '--args'])
else:
cmd.extend(['screen',
'-L', '-Logfile', 'gdb.log',
'-d',
'-m',
'-S', 'ardupilot-gdb',
'gdb', '--cd', os.getcwd(), '-x', '/tmp/x.gdb', binary, '--args'])
elif lldb:
f = open("/tmp/x.lldb", "w")
for breakingpoint in breakpoints:
f.write("b %s\n" % (breakingpoint,))
if disable_breakpoints:
f.write("disable\n")
f.write("settings set target.process.stop-on-exec false\n")
f.write("process launch\n")
f.close()
if sys.platform == "darwin" and os.getenv('DISPLAY'):
cmd.extend(['lldb', '-s', '/tmp/x.lldb', '--'])
elif os.environ.get('DISPLAY'):
cmd.extend(['xterm', '-e', 'lldb', '-s', '/tmp/x.lldb', '--'])
else:
raise RuntimeError("DISPLAY was not set")
cmd.append(binary)
if not supplementary:
if wipe:
cmd.append('-w')
if synthetic_clock:
cmd.append('-S')
if home is not None:
cmd.extend(['--home', home])
cmd.extend(['--model', model])
if speedup is not None and speedup != 1:
cmd.extend(['--speedup', str(speedup)])
if sim_rate_hz is not None:
cmd.extend(['--rate', str(sim_rate_hz)])
if unhide_parameters:
cmd.extend(['--unhide-groups'])
# somewhere for MAVProxy to connect to:
cmd.append('--serial1=tcp:2')
if not enable_fgview_output:
cmd.append("--disable-fgview")
if defaults_filepath is not None:
if isinstance(defaults_filepath, list):
defaults = [reltopdir(path) for path in defaults_filepath]
if len(defaults):
cmd.extend(['--defaults', ",".join(defaults)])
else:
cmd.extend(['--defaults', reltopdir(defaults_filepath)])
cmd.extend(customisations)
pexpect_logfile_prefix = stdout_prefix
if pexpect_logfile_prefix is None:
pexpect_logfile_prefix = os.path.basename(binary)
pexpect_logfile = PSpawnStdPrettyPrinter(prefix=pexpect_logfile_prefix)
if (gdb or lldb) and sys.platform == "darwin" and os.getenv('DISPLAY'):
global windowID
# on MacOS record the window IDs so we can close them later
atexit.register(kill_mac_terminal)
child = None
mydir = os.path.dirname(os.path.realpath(__file__))
autotest_dir = os.path.realpath(os.path.join(mydir, '..'))
runme = [os.path.join(autotest_dir, "run_in_terminal_window.sh"), 'mactest']
runme.extend(cmd)
print(cmd)
out = subprocess.Popen(runme, stdout=subprocess.PIPE).communicate()[0]
out = out.decode('utf-8')
p = re.compile('tab 1 of window id (.*)')
tstart = time.time()
while time.time() - tstart < 5:
tabs = p.findall(out)
if len(tabs) > 0:
break
time.sleep(0.1)
# sleep for extra 2 seconds for application to start
time.sleep(2)
if len(tabs) > 0:
windowID.append(tabs[0])
else:
print("Cannot find %s process terminal" % binary)
child = FakeMacOSXSpawn()
elif gdb and not os.getenv('DISPLAY'):
subprocess.Popen(cmd)
atexit.register(kill_screen_gdb)
# we are expected to return a pexpect wrapped around the
# stdout of the ArduPilot binary. Not going to happen until
# AP gets a redirect-stdout-to-filehandle option. So, in the
# meantime, return a dummy:
return pexpect.spawn("true", ["true"],
logfile=pexpect_logfile,
encoding=ENCODING,
timeout=5)
else:
print("Running: %s" % cmd_as_shell(cmd))
first = cmd[0]
rest = cmd[1:]
child = pexpect.spawn(first, rest, logfile=pexpect_logfile, encoding=ENCODING, timeout=5)
pexpect_autoclose(child)
if gdb or lldb:
# if we run GDB we do so in an xterm. "Waiting for
# connection" is never going to appear on xterm's output.
# ... so let's give it another magic second.
time.sleep(1)
# TODO: have a SITL-compiled ardupilot able to have its
# console on an output fd.
else:
child.expect('Waiting for ', timeout=300)
return child
def mavproxy_cmd():
"""return path to which mavproxy to use"""
return os.getenv('MAVPROXY_CMD', 'mavproxy.py')
def MAVProxy_version():
"""return the current version of mavproxy as a tuple e.g. (1,8,8)"""
command = "%s --version" % mavproxy_cmd()
output = subprocess.Popen(command, shell=True, stdout=subprocess.PIPE).communicate()[0]
output = output.decode('ascii')
match = re.search("MAVProxy Version: ([0-9]+)[.]([0-9]+)[.]([0-9]+)", output)
if match is None:
raise ValueError("Unable to determine MAVProxy version from (%s)" % output)
return int(match.group(1)), int(match.group(2)), int(match.group(3))
def start_MAVProxy_SITL(atype,
aircraft=None,
setup=False,
master=None,
options=[],
sitl_rcin_port=5501,
pexpect_timeout=60,
logfile=sys.stdout):
"""Launch mavproxy connected to a SITL instance."""
if master is None:
raise ValueError("Expected a master")
local_mp_modules_dir = os.path.abspath(
os.path.join(__file__, '..', '..', '..', 'mavproxy_modules'))
env = dict(os.environ)
old = env.get('PYTHONPATH', None)
env['PYTHONPATH'] = local_mp_modules_dir
if old is not None:
env['PYTHONPATH'] += os.path.pathsep + old
global close_list
cmd = []
cmd.append(mavproxy_cmd())
cmd.extend(['--master', master])
cmd.extend(['--sitl', "localhost:%u" % sitl_rcin_port])
if setup:
cmd.append('--setup')
if aircraft is None:
aircraft = 'test.%s' % atype
cmd.extend(['--aircraft', aircraft])
cmd.extend(options)
cmd.extend(['--default-modules', 'misc,wp,rally,fence,param,arm,mode,rc,cmdlong,output'])
print("PYTHONPATH: %s" % str(env['PYTHONPATH']))
print("Running: %s" % cmd_as_shell(cmd))
ret = pexpect.spawn(cmd[0], cmd[1:], logfile=logfile, encoding=ENCODING, timeout=pexpect_timeout, env=env)
ret.delaybeforesend = 0
pexpect_autoclose(ret)
return ret
def expect_setup_callback(e, callback):
"""Setup a callback that is called once a second while waiting for
patterns."""
def _expect_callback(pattern, timeout=e.timeout):
tstart = time.time()
while time.time() < tstart + timeout:
try:
ret = e.expect_saved(pattern, timeout=1)
return ret
except pexpect.TIMEOUT:
e.expect_user_callback(e)
print("Timed out looking for %s" % pattern)
raise pexpect.TIMEOUT(timeout)
e.expect_user_callback = callback
e.expect_saved = e.expect
e.expect = _expect_callback
def mkdir_p(directory):
"""Like mkdir -p ."""
if not directory:
return
if directory.endswith("/"):
mkdir_p(directory[:-1])
return
if os.path.isdir(directory):
return
mkdir_p(os.path.dirname(directory))
os.mkdir(directory)
def loadfile(fname):
"""Load a file as a string."""
f = open(fname, mode='r')
r = f.read()
f.close()
return r
def lock_file(fname):
"""Lock a file."""
import fcntl
f = open(fname, mode='w')
try:
fcntl.lockf(f, fcntl.LOCK_EX | fcntl.LOCK_NB)
except OSError:
return None
return f
def check_parent(parent_pid=None):
"""Check our parent process is still alive."""
if parent_pid is None:
try:
parent_pid = os.getppid()
except OSError:
pass
if parent_pid is None:
return
try:
os.kill(parent_pid, 0)
except OSError:
print("Parent had finished - exiting")
sys.exit(1)
def gps_newpos(lat, lon, bearing, distance):
"""Extrapolate latitude/longitude given a heading and distance
thanks to http://www.movable-type.co.uk/scripts/latlong.html .
"""
from math import sin, asin, cos, atan2, radians, degrees
lat1 = radians(lat)
lon1 = radians(lon)
brng = radians(bearing)
dr = distance / RADIUS_OF_EARTH
lat2 = asin(sin(lat1) * cos(dr) +
cos(lat1) * sin(dr) * cos(brng))
lon2 = lon1 + atan2(sin(brng) * sin(dr) * cos(lat1),
cos(dr) - sin(lat1) * sin(lat2))
return degrees(lat2), degrees(lon2)
def gps_distance(lat1, lon1, lat2, lon2):
"""Return distance between two points in meters,
coordinates are in degrees
thanks to http://www.movable-type.co.uk/scripts/latlong.html ."""
lat1 = math.radians(lat1)
lat2 = math.radians(lat2)
lon1 = math.radians(lon1)
lon2 = math.radians(lon2)
dLat = lat2 - lat1
dLon = lon2 - lon1
a = math.sin(0.5 * dLat)**2 + math.sin(0.5 * dLon)**2 * math.cos(lat1) * math.cos(lat2)
c = 2.0 * math.atan2(math.sqrt(a), math.sqrt(1.0 - a))
return RADIUS_OF_EARTH * c
def gps_bearing(lat1, lon1, lat2, lon2):
"""Return bearing between two points in degrees, in range 0-360
thanks to http://www.movable-type.co.uk/scripts/latlong.html ."""
lat1 = math.radians(lat1)
lat2 = math.radians(lat2)
lon1 = math.radians(lon1)
lon2 = math.radians(lon2)
dLon = lon2 - lon1
y = math.sin(dLon) * math.cos(lat2)
x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dLon)
bearing = math.degrees(math.atan2(y, x))
if bearing < 0:
bearing += 360.0
return bearing
def constrain(value, minv, maxv):
"""Constrain a value to a range."""
if value < minv:
value = minv
if value > maxv:
value = maxv
return value
def load_local_module(fname):
"""load a python module from within the ardupilot tree"""
fname = os.path.join(topdir(), fname)
if sys.version_info.major >= 3:
import importlib.util
spec = importlib.util.spec_from_file_location("local_module", fname)
ret = importlib.util.module_from_spec(spec)
spec.loader.exec_module(ret)
else:
import imp
ret = imp.load_source("local_module", fname)
return ret
def get_git_hash(short=False):
short_v = "--short=8 " if short else ""
githash = run_cmd(f'git rev-parse {short_v}HEAD', output=True, directory=reltopdir('.')).strip()
if sys.version_info.major >= 3:
githash = githash.decode('utf-8')
return githash
if __name__ == "__main__":
import doctest
doctest.testmod()
Reference in New Issue View Git Blame Copy Permalink