Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Path: blob/master/Tools/autotest/pysim/util.py
Views: 1862
from __future__ import print_function12'''3AP_FLAKE8_CLEAN4'''56import atexit7import math8import os9import re10import shlex11import signal12import subprocess13import sys14import tempfile15import time161718import pexpect1920if sys.version_info[0] >= 3:21ENCODING = 'ascii'22else:23ENCODING = None2425RADIUS_OF_EARTH = 6378100.0 # in meters2627# List of open terminal windows for macosx28windowID = []293031def topdir():32"""Return top of git tree where autotest is running from."""33d = os.path.dirname(os.path.realpath(__file__))34assert os.path.basename(d) == 'pysim'35d = os.path.dirname(d)36assert os.path.basename(d) == 'autotest'37d = os.path.dirname(d)38assert os.path.basename(d) == 'Tools'39d = os.path.dirname(d)40return d414243def relcurdir(path):44"""Return a path relative to current dir"""45return os.path.relpath(path, os.getcwd())464748def reltopdir(path):49"""Returns the normalized ABSOLUTE path for 'path', where path is a path relative to topdir"""50return os.path.normpath(os.path.join(topdir(), path))515253def run_cmd(cmd, directory=".", show=True, output=False, checkfail=True):54"""Run a shell command."""55shell = False56if not isinstance(cmd, list):57cmd = [cmd]58shell = True59if show:60print("Running: (%s) in (%s)" % (cmd_as_shell(cmd), directory,))61if output:62return subprocess.Popen(cmd, shell=shell, stdout=subprocess.PIPE, cwd=directory).communicate()[0]63elif checkfail:64return subprocess.check_call(cmd, shell=shell, cwd=directory)65else:66return subprocess.call(cmd, shell=shell, cwd=directory)676869def rmfile(path):70"""Remove a file if it exists."""71try:72os.unlink(path)73except (OSError, FileNotFoundError):74pass757677def deltree(path):78"""Delete a tree of files."""79run_cmd('rm -rf %s' % path)808182def relwaf():83return "./modules/waf/waf-light"848586def waf_configure(board,87j=None,88debug=False,89math_check_indexes=False,90coverage=False,91ekf_single=False,92postype_single=False,93force_32bit=False,94extra_args=[],95extra_hwdef=None,96ubsan=False,97ubsan_abort=False,98num_aux_imus=0,99dronecan_tests=False,100extra_defines={}):101cmd_configure = [relwaf(), "configure", "--board", board]102if debug:103cmd_configure.append('--debug')104if coverage:105cmd_configure.append('--coverage')106if math_check_indexes:107cmd_configure.append('--enable-math-check-indexes')108if ekf_single:109cmd_configure.append('--ekf-single')110if postype_single:111cmd_configure.append('--postype-single')112if force_32bit:113cmd_configure.append('--force-32bit')114if ubsan:115cmd_configure.append('--ubsan')116if ubsan_abort:117cmd_configure.append('--ubsan-abort')118if num_aux_imus > 0:119cmd_configure.append('--num-aux-imus=%u' % num_aux_imus)120if dronecan_tests:121cmd_configure.append('--enable-dronecan-tests')122if extra_hwdef is not None:123cmd_configure.extend(['--extra-hwdef', extra_hwdef])124for nv in extra_defines.items():125cmd_configure.extend(['--define', "%s=%s" % nv])126if j is not None:127cmd_configure.extend(['-j', str(j)])128pieces = [shlex.split(x) for x in extra_args]129for piece in pieces:130cmd_configure.extend(piece)131run_cmd(cmd_configure, directory=topdir(), checkfail=True)132133134def waf_clean():135run_cmd([relwaf(), "clean"], directory=topdir(), checkfail=True)136137138def waf_build(target=None):139cmd = [relwaf(), "build"]140if target is not None:141cmd.append(target)142run_cmd(cmd, directory=topdir(), checkfail=True)143144145def build_SITL(146build_target,147board='sitl',148clean=True,149configure=True,150coverage=False,151debug=False,152ekf_single=False,153extra_configure_args=[],154extra_defines={},155j=None,156math_check_indexes=False,157postype_single=False,158force_32bit=False,159ubsan=False,160ubsan_abort=False,161num_aux_imus=0,162dronecan_tests=False,163):164165# first configure166if configure:167waf_configure(board,168j=j,169debug=debug,170math_check_indexes=math_check_indexes,171ekf_single=ekf_single,172postype_single=postype_single,173coverage=coverage,174force_32bit=force_32bit,175ubsan=ubsan,176ubsan_abort=ubsan_abort,177extra_defines=extra_defines,178num_aux_imus=num_aux_imus,179dronecan_tests=dronecan_tests,180extra_args=extra_configure_args,)181182# then clean183if clean:184waf_clean()185186# then build187cmd_make = [relwaf(), "build", "--target", build_target]188if j is not None:189cmd_make.extend(['-j', str(j)])190run_cmd(cmd_make, directory=topdir(), checkfail=True, show=True)191return True192193194def build_examples(board, j=None, debug=False, clean=False, configure=True, math_check_indexes=False, coverage=False,195ekf_single=False, postype_single=False, force_32bit=False, ubsan=False, ubsan_abort=False,196num_aux_imus=0, dronecan_tests=False,197extra_configure_args=[]):198# first configure199if configure:200waf_configure(board,201j=j,202debug=debug,203math_check_indexes=math_check_indexes,204ekf_single=ekf_single,205postype_single=postype_single,206coverage=coverage,207force_32bit=force_32bit,208ubsan=ubsan,209ubsan_abort=ubsan_abort,210extra_args=extra_configure_args,211dronecan_tests=dronecan_tests)212213# then clean214if clean:215waf_clean()216217# then build218cmd_make = [relwaf(), "examples"]219run_cmd(cmd_make, directory=topdir(), checkfail=True, show=True)220return True221222223def build_replay(board, j=None, debug=False, clean=False):224# first configure225waf_configure(board, j=j, debug=debug)226227# then clean228if clean:229waf_clean()230231# then build232cmd_make = [relwaf(), "replay"]233run_cmd(cmd_make, directory=topdir(), checkfail=True, show=True)234return True235236237def build_tests(board,238j=None,239debug=False,240clean=False,241configure=True,242math_check_indexes=False,243coverage=False,244ekf_single=False,245postype_single=False,246force_32bit=False,247ubsan=False,248ubsan_abort=False,249num_aux_imus=0,250dronecan_tests=False,251extra_configure_args=[]):252253# first configure254if configure:255waf_configure(board,256j=j,257debug=debug,258math_check_indexes=math_check_indexes,259ekf_single=ekf_single,260postype_single=postype_single,261coverage=coverage,262force_32bit=force_32bit,263ubsan=ubsan,264ubsan_abort=ubsan_abort,265num_aux_imus=num_aux_imus,266dronecan_tests=dronecan_tests,267extra_args=extra_configure_args,)268269# then clean270if clean:271waf_clean()272273# then build274run_cmd([relwaf(), "tests"], directory=topdir(), checkfail=True, show=True)275return True276277278# list of pexpect children to close on exit279close_list = []280281282def pexpect_autoclose(p):283"""Mark for autoclosing."""284global close_list285close_list.append(p)286287288def pexpect_close(p):289"""Close a pexpect child."""290global close_list291292ex = None293if p is None:294print("Nothing to close")295return296try:297p.kill(signal.SIGTERM)298except IOError as e:299print("Caught exception: %s" % str(e))300ex = e301pass302if ex is None:303# give the process some time to go away304for i in range(20):305if not p.isalive():306break307time.sleep(0.05)308try:309p.close()310except Exception:311pass312try:313p.close(force=True)314except Exception:315pass316if p in close_list:317close_list.remove(p)318319320def pexpect_close_all():321"""Close all pexpect children."""322global close_list323for p in close_list[:]:324pexpect_close(p)325326327def pexpect_drain(p):328"""Drain any pending input."""329try:330p.read_nonblocking(1000, timeout=0)331except Exception:332pass333334335def cmd_as_shell(cmd):336return (" ".join(['"%s"' % x for x in cmd]))337338339def make_safe_filename(text):340"""Return a version of text safe for use as a filename."""341r = re.compile("([^a-zA-Z0-9_.+-])")342text.replace('/', '-')343filename = r.sub(lambda m: str(hex(ord(str(m.group(1))))).upper(), text)344return filename345346347def valgrind_log_filepath(binary, model):348return make_safe_filename('%s-%s-valgrind.log' % (os.path.basename(binary), model,))349350351def kill_screen_gdb():352cmd = ["screen", "-X", "-S", "ardupilot-gdb", "quit"]353subprocess.Popen(cmd)354355356def kill_mac_terminal():357global windowID358for window in windowID:359cmd = ("osascript -e \'tell application \"Terminal\" to close "360"(window(get index of window id %s))\'" % window)361os.system(cmd)362363364class FakeMacOSXSpawn(object):365"""something that looks like a pspawn child so we can ignore attempts366to pause (and otherwise kill(1) SITL. MacOSX using osascript to367start/stop sitl368"""369def __init__(self):370pass371372def progress(self, message):373print(message)374375def kill(self, sig):376# self.progress("FakeMacOSXSpawn: ignoring kill(%s)" % str(sig))377pass378379def isalive(self):380self.progress("FakeMacOSXSpawn: assuming process is alive")381return True382383384class PSpawnStdPrettyPrinter(object):385'''a fake filehandle-like object which prefixes a string to all lines386before printing to stdout/stderr. To be used to pass to387pexpect.spawn's logfile argument388'''389def __init__(self, output=sys.stdout, prefix="stdout"):390self.output = output391self.prefix = prefix392self.buffer = ""393394def close(self):395self.print_prefixed_line(self.buffer)396397def write(self, data):398self.buffer += data399lines = self.buffer.split("\n")400self.buffer = lines[-1]401lines.pop()402for line in lines:403self.print_prefixed_line(line)404405def print_prefixed_line(self, line):406print("%s: %s" % (self.prefix, line), file=self.output)407408def flush(self):409pass410411412def start_SITL(binary,413valgrind=False,414callgrind=False,415gdb=False,416gdb_no_tui=False,417wipe=False,418home=None,419model=None,420speedup=1,421sim_rate_hz=None,422defaults_filepath=[],423unhide_parameters=False,424gdbserver=False,425breakpoints=[],426disable_breakpoints=False,427customisations=[],428lldb=False,429enable_fgview=False,430supplementary=False,431stdout_prefix=None):432433if model is None and not supplementary:434raise ValueError("model must not be None")435436"""Launch a SITL instance."""437cmd = []438if (callgrind or valgrind) and os.path.exists('/usr/bin/valgrind'):439# we specify a prefix for vgdb-pipe because on Vagrant virtual440# machines the pipes are created on the mountpoint for the441# shared directory with the host machine. mmap's,442# unsurprisingly, fail on files created on that mountpoint.443vgdb_prefix = os.path.join(tempfile.gettempdir(), "vgdb-pipe")444log_file = valgrind_log_filepath(binary=binary, model=model)445cmd.extend([446'valgrind',447# adding this option allows valgrind to cope with the overload448# of operator new449"--soname-synonyms=somalloc=nouserintercepts",450'--vgdb-prefix=%s' % vgdb_prefix,451'-q',452'--log-file=%s' % log_file])453if callgrind:454cmd.extend(["--tool=callgrind"])455if gdbserver:456cmd.extend(['gdbserver', 'localhost:3333'])457if gdb:458# attach gdb to the gdbserver:459f = open("/tmp/x.gdb", "w")460f.write("target extended-remote localhost:3333\nc\n")461for breakingpoint in breakpoints:462f.write("b %s\n" % (breakingpoint,))463if disable_breakpoints:464f.write("disable\n")465f.close()466run_cmd('screen -d -m -S ardupilot-gdbserver '467'bash -c "gdb -x /tmp/x.gdb"')468elif gdb:469f = open("/tmp/x.gdb", "w")470f.write("set pagination off\n")471for breakingpoint in breakpoints:472f.write("b %s\n" % (breakingpoint,))473if disable_breakpoints:474f.write("disable\n")475if not gdb_no_tui:476f.write("tui enable\n")477f.write("r\n")478f.close()479if sys.platform == "darwin" and os.getenv('DISPLAY'):480cmd.extend(['gdb', '-x', '/tmp/x.gdb', '--args'])481elif os.environ.get('DISPLAY'):482cmd.extend(['xterm', '-e', 'gdb', '-x', '/tmp/x.gdb', '--args'])483else:484cmd.extend(['screen',485'-L', '-Logfile', 'gdb.log',486'-d',487'-m',488'-S', 'ardupilot-gdb',489'gdb', '--cd', os.getcwd(), '-x', '/tmp/x.gdb', binary, '--args'])490elif lldb:491f = open("/tmp/x.lldb", "w")492for breakingpoint in breakpoints:493f.write("b %s\n" % (breakingpoint,))494if disable_breakpoints:495f.write("disable\n")496f.write("settings set target.process.stop-on-exec false\n")497f.write("process launch\n")498f.close()499if sys.platform == "darwin" and os.getenv('DISPLAY'):500cmd.extend(['lldb', '-s', '/tmp/x.lldb', '--'])501elif os.environ.get('DISPLAY'):502cmd.extend(['xterm', '-e', 'lldb', '-s', '/tmp/x.lldb', '--'])503else:504raise RuntimeError("DISPLAY was not set")505506cmd.append(binary)507508if defaults_filepath is None:509defaults_filepath = []510if not isinstance(defaults_filepath, list):511defaults_filepath = [defaults_filepath]512defaults = [reltopdir(path) for path in defaults_filepath]513514if not supplementary:515if wipe:516cmd.append('-w')517if home is not None:518cmd.extend(['--home', home])519cmd.extend(['--model', model])520if speedup is not None and speedup != 1:521ntf = tempfile.NamedTemporaryFile(mode="w", delete=False)522print(f"SIM_SPEEDUP {speedup}", file=ntf)523ntf.close()524# prepend it so that a caller can override the speedup in525# passed-in defaults:526defaults = [ntf.name] + defaults527if sim_rate_hz is not None:528cmd.extend(['--rate', str(sim_rate_hz)])529if unhide_parameters:530cmd.extend(['--unhide-groups'])531# somewhere for MAVProxy to connect to:532cmd.append('--serial1=tcp:2')533if enable_fgview:534cmd.append("--enable-fgview")535536if len(defaults):537cmd.extend(['--defaults', ",".join(defaults)])538539cmd.extend(customisations)540541if "--defaults" in customisations:542raise ValueError("--defaults must be passed in via defaults_filepath keyword argument, not as part of customisation list") # noqa543544pexpect_logfile_prefix = stdout_prefix545if pexpect_logfile_prefix is None:546pexpect_logfile_prefix = os.path.basename(binary)547pexpect_logfile = PSpawnStdPrettyPrinter(prefix=pexpect_logfile_prefix)548549if (gdb or lldb) and sys.platform == "darwin" and os.getenv('DISPLAY'):550global windowID551# on MacOS record the window IDs so we can close them later552atexit.register(kill_mac_terminal)553child = None554mydir = os.path.dirname(os.path.realpath(__file__))555autotest_dir = os.path.realpath(os.path.join(mydir, '..'))556runme = [os.path.join(autotest_dir, "run_in_terminal_window.sh"), 'mactest']557runme.extend(cmd)558print(cmd)559out = subprocess.Popen(runme, stdout=subprocess.PIPE).communicate()[0]560out = out.decode('utf-8')561p = re.compile('tab 1 of window id (.*)')562563tstart = time.time()564while time.time() - tstart < 5:565tabs = p.findall(out)566567if len(tabs) > 0:568break569570time.sleep(0.1)571# sleep for extra 2 seconds for application to start572time.sleep(2)573if len(tabs) > 0:574windowID.append(tabs[0])575else:576print("Cannot find %s process terminal" % binary)577child = FakeMacOSXSpawn()578elif gdb and not os.getenv('DISPLAY'):579subprocess.Popen(cmd)580atexit.register(kill_screen_gdb)581# we are expected to return a pexpect wrapped around the582# stdout of the ArduPilot binary. Not going to happen until583# AP gets a redirect-stdout-to-filehandle option. So, in the584# meantime, return a dummy:585return pexpect.spawn("true", ["true"],586logfile=pexpect_logfile,587encoding=ENCODING,588timeout=5)589else:590print("Running: %s" % cmd_as_shell(cmd))591592first = cmd[0]593rest = cmd[1:]594child = pexpect.spawn(first, rest, logfile=pexpect_logfile, encoding=ENCODING, timeout=5)595pexpect_autoclose(child)596if gdb or lldb:597# if we run GDB we do so in an xterm. "Waiting for598# connection" is never going to appear on xterm's output.599# ... so let's give it another magic second.600time.sleep(1)601# TODO: have a SITL-compiled ardupilot able to have its602# console on an output fd.603else:604child.expect('Waiting for ', timeout=300)605return child606607608def mavproxy_cmd():609"""return path to which mavproxy to use"""610return os.getenv('MAVPROXY_CMD', 'mavproxy.py')611612613def MAVProxy_version():614"""return the current version of mavproxy as a tuple e.g. (1,8,8)"""615command = "%s --version" % mavproxy_cmd()616output = subprocess.Popen(command, shell=True, stdout=subprocess.PIPE).communicate()[0]617output = output.decode('ascii')618match = re.search("MAVProxy Version: ([0-9]+)[.]([0-9]+)[.]([0-9]+)", output)619if match is None:620raise ValueError("Unable to determine MAVProxy version from (%s)" % output)621return int(match.group(1)), int(match.group(2)), int(match.group(3))622623624def start_MAVProxy_SITL(atype,625aircraft=None,626setup=False,627master=None,628options=[],629sitl_rcin_port=5501,630pexpect_timeout=60,631logfile=sys.stdout):632"""Launch mavproxy connected to a SITL instance."""633if master is None:634raise ValueError("Expected a master")635636local_mp_modules_dir = os.path.abspath(637os.path.join(__file__, '..', '..', '..', 'mavproxy_modules'))638env = dict(os.environ)639old = env.get('PYTHONPATH', None)640env['PYTHONPATH'] = local_mp_modules_dir641if old is not None:642env['PYTHONPATH'] += os.path.pathsep + old643644global close_list645cmd = []646cmd.append(mavproxy_cmd())647cmd.extend(['--master', master])648cmd.extend(['--sitl', "localhost:%u" % sitl_rcin_port])649if setup:650cmd.append('--setup')651if aircraft is None:652aircraft = 'test.%s' % atype653cmd.extend(['--aircraft', aircraft])654cmd.extend(options)655cmd.extend(['--default-modules', 'misc,wp,rally,fence,param,arm,mode,rc,cmdlong,output'])656657print("PYTHONPATH: %s" % str(env['PYTHONPATH']))658print("Running: %s" % cmd_as_shell(cmd))659660ret = pexpect.spawn(cmd[0], cmd[1:], logfile=logfile, encoding=ENCODING, timeout=pexpect_timeout, env=env)661ret.delaybeforesend = 0662pexpect_autoclose(ret)663return ret664665666def start_PPP_daemon(ips, sockaddr):667"""Start pppd for networking"""668669global close_list670cmd = "sudo pppd socket %s debug noauth nodetach %s" % (sockaddr, ips)671cmd = cmd.split()672print("Running: %s" % cmd_as_shell(cmd))673674ret = pexpect.spawn(cmd[0], cmd[1:], logfile=sys.stdout, encoding=ENCODING, timeout=30)675ret.delaybeforesend = 0676pexpect_autoclose(ret)677return ret678679680def expect_setup_callback(e, callback):681"""Setup a callback that is called once a second while waiting for682patterns."""683def _expect_callback(pattern, timeout=e.timeout):684tstart = time.time()685while time.time() < tstart + timeout:686try:687ret = e.expect_saved(pattern, timeout=1)688return ret689except pexpect.TIMEOUT:690e.expect_user_callback(e)691print("Timed out looking for %s" % pattern)692raise pexpect.TIMEOUT(timeout)693694e.expect_user_callback = callback695e.expect_saved = e.expect696e.expect = _expect_callback697698699def mkdir_p(directory):700"""Like mkdir -p ."""701if not directory:702return703if directory.endswith("/"):704mkdir_p(directory[:-1])705return706if os.path.isdir(directory):707return708mkdir_p(os.path.dirname(directory))709os.mkdir(directory)710711712def loadfile(fname):713"""Load a file as a string."""714f = open(fname, mode='r')715r = f.read()716f.close()717return r718719720def lock_file(fname):721"""Lock a file."""722import fcntl723f = open(fname, mode='w')724try:725fcntl.lockf(f, fcntl.LOCK_EX | fcntl.LOCK_NB)726except OSError:727return None728return f729730731def check_parent(parent_pid=None):732"""Check our parent process is still alive."""733if parent_pid is None:734try:735parent_pid = os.getppid()736except OSError:737pass738if parent_pid is None:739return740try:741os.kill(parent_pid, 0)742except OSError:743print("Parent had finished - exiting")744sys.exit(1)745746747def gps_newpos(lat, lon, bearing, distance):748"""Extrapolate latitude/longitude given a heading and distance749thanks to http://www.movable-type.co.uk/scripts/latlong.html .750"""751from math import sin, asin, cos, atan2, radians, degrees752753lat1 = radians(lat)754lon1 = radians(lon)755brng = radians(bearing)756dr = distance / RADIUS_OF_EARTH757758lat2 = asin(sin(lat1) * cos(dr) +759cos(lat1) * sin(dr) * cos(brng))760lon2 = lon1 + atan2(sin(brng) * sin(dr) * cos(lat1),761cos(dr) - sin(lat1) * sin(lat2))762return degrees(lat2), degrees(lon2)763764765def gps_distance(lat1, lon1, lat2, lon2):766"""Return distance between two points in meters,767coordinates are in degrees768thanks to http://www.movable-type.co.uk/scripts/latlong.html ."""769lat1 = math.radians(lat1)770lat2 = math.radians(lat2)771lon1 = math.radians(lon1)772lon2 = math.radians(lon2)773dLat = lat2 - lat1774dLon = lon2 - lon1775776a = math.sin(0.5 * dLat)**2 + math.sin(0.5 * dLon)**2 * math.cos(lat1) * math.cos(lat2)777c = 2.0 * math.atan2(math.sqrt(a), math.sqrt(1.0 - a))778return RADIUS_OF_EARTH * c779780781def gps_bearing(lat1, lon1, lat2, lon2):782"""Return bearing between two points in degrees, in range 0-360783thanks to http://www.movable-type.co.uk/scripts/latlong.html ."""784lat1 = math.radians(lat1)785lat2 = math.radians(lat2)786lon1 = math.radians(lon1)787lon2 = math.radians(lon2)788dLon = lon2 - lon1789y = math.sin(dLon) * math.cos(lat2)790x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dLon)791bearing = math.degrees(math.atan2(y, x))792if bearing < 0:793bearing += 360.0794return bearing795796797def constrain(value, minv, maxv):798"""Constrain a value to a range."""799if value < minv:800value = minv801if value > maxv:802value = maxv803return value804805806def load_local_module(fname):807"""load a python module from within the ardupilot tree"""808fname = os.path.join(topdir(), fname)809if sys.version_info.major >= 3:810import importlib.util811spec = importlib.util.spec_from_file_location("local_module", fname)812ret = importlib.util.module_from_spec(spec)813spec.loader.exec_module(ret)814else:815import imp816ret = imp.load_source("local_module", fname)817return ret818819820def get_git_hash(short=False):821short_v = "--short=8 " if short else ""822githash = run_cmd(f'git rev-parse {short_v}HEAD', output=True, directory=reltopdir('.')).strip()823if sys.version_info.major >= 3:824githash = githash.decode('utf-8')825return githash826827828if __name__ == "__main__":829import doctest830doctest.testmod()831832833