Merge pull request dronekit#199 from diydrones/tcr-gototest

Adds goto test.

Author: mrpollo

.travis.yml

@@ -1,6 +1,9 @@
 before_install: []
 env:
-  global: []
+  global:
+    - TEST_SPEEDUP=10
+    - TEST_RATE=200
+    - TEST_RETRY=1
 install:
   - 'a() { set -e; }'
   - "z() { E=$?; test $E -eq 0 && return 0; printf \"\\n\\033[1;31mThe command failed with exit code $?.\\033[0m\"; set -e; return $E; }"
@@ -9,7 +12,7 @@ install:
 script:
   - (a; sudo python setup.py install );z
   - (a; nosetests tests/web );z
-  - (a; cd tests; python -m sitl );z
+  - (a; cd tests; python -um sitl );z
 git:
   depth: 10
 language: objective-c

appveyor.yml

@@ -1,5 +1,8 @@
 environment:
-  global: {}
+  global:
+    TEST_SPEEDUP: 10
+    TEST_RATE: 200
+    TEST_RETRY: 4
 init: []
 cache:
   - "c:\\Users\\appveyor\\.pip-wheelhouse"
@@ -19,6 +22,8 @@ install:
   - cmd: "setlocal & SET PATH=%PYTHON%;c:\\Python27\\Scripts;%PATH% & pip2 install git+https://github.com/3drobotics/dronekit-sitl-runner.git & endlocal"
 build_script:
   - cmd: 'setlocal & python setup.py install & endlocal'
+  - cmd: "setlocal & SET PATH=%PYTHON%;c:\\Python27\\Scripts;%PATH% & nosetests tests\\web & endlocal"
+  - cmd: "setlocal & SET PATH=%PYTHON%;c:\\Python27\\Scripts;%PATH% & cd tests & python -um sitl & endlocal"
 clone_depth: 10
 test: 'off'
 branches:

circle.yml

@@ -1,12 +1,15 @@
 machine:
-  environment: {}
+  environment:
+    TEST_SPEEDUP: 10
+    TEST_RATE: 200
+    TEST_RETRY: 1
 dependencies:
   override:
     - python setup.py install:
         environment: {}
     - nosetests tests/web:
         environment: {}
-    - cd tests; python -m sitl:
+    - cd tests; python -um sitl:
         environment: {}
   pre:
     - pip2 install nose psutil:

tests/sitl/__main__.py

@@ -14,20 +14,6 @@
 import time
 import psutil
 
-def wait_timeout(proc, seconds):
-    """Wait for a process to finish, or raise exception after timeout"""
-    start = time.time()
-    end = start + seconds
-    interval = min(seconds / 1000.0, .25)
-
-    while True:
-        result = proc.poll()
-        if result is not None:
-            return result
-        if time.time() >= end:
-            raise RuntimeError("Process timed out")
-        time.sleep(interval)
-
 def kill(proc_pid):
     process = psutil.Process(proc_pid)
     for proc in process.children(recursive=True):
@@ -54,6 +40,67 @@ def cleanup_processes():
 
 testpath = os.path.dirname(__file__)
 
+from threading import Thread
+from Queue import Queue, Empty
+
+class NonBlockingStreamReader:
+
+    def __init__(self, stream):
+        '''
+        stream: the stream to read from.
+                Usually a process' stdout or stderr.
+        '''
+
+        self._s = stream
+        self._q = Queue()
+
+        def _populateQueue(stream, queue):
+            '''
+            Collect lines from 'stream' and put them in 'quque'.
+            '''
+
+            while True:
+                line = stream.readline()
+                if line:
+                    queue.put(line)
+                else:
+                    break
+
+        self._t = Thread(target = _populateQueue,
+                args = (self._s, self._q))
+        self._t.daemon = True
+        self._t.start() #start collecting lines from the stream
+
+    def readline(self, timeout = None):
+        try:
+            return self._q.get(block = timeout is not None,
+                    timeout = timeout)
+        except Empty:
+            return None
+
+class UnexpectedEndOfStream(Exception):
+    pass
+
+def wait_timeout(proc, seconds, verbose):
+    """Wait for a process to finish, or raise exception after timeout"""
+    start = time.time()
+    end = start + seconds
+    interval = min(seconds / 1000.0, .25)
+
+    stdout = NonBlockingStreamReader(proc.stdout)
+    while True:
+        # Read text in verbose mode. Also sleep for a bit.
+        nextline = stdout.readline(interval)
+        if nextline and verbose:
+            sys.stdout.write(nextline)
+            sys.stdout.flush()
+
+        # Poll for end of text.
+        result = proc.poll()
+        if result is not None:
+            return result
+        if time.time() >= end:
+            raise RuntimeError("Process timed out")
 
 def wrap_fd(pipeout):
     # Prepare to pass to child process
@@ -67,7 +114,15 @@ def wrap_fd(pipeout):
 
 def lets_run_a_test(name):
     sitl_args = ['dronekit-sitl', 'copter-3.3-rc5', '-I0', '-S', '--model', 'quad', '--home=-35.363261,149.165230,584,353']
+
+    speedup = os.environ.get('TEST_SPEEDUP', '1')
+    rate = os.environ.get('TEST_RATE', '200')
+    sitl_args += ['--speedup', str(speedup), '-r', str(rate)]
+
+    # Change CPU core affinity.
+    # TODO change affinity on osx/linux
     if sys.platform == 'win32':
+        # 0x14 = 0b1110 = all cores except cpu 1
         sitl = Popen(['start', '/affinity', '14', '/realtime', '/b', '/wait'] + sitl_args, shell=True, stdout=PIPE, stderr=PIPE)
     else:
         sitl = Popen(sitl_args, stdout=PIPE, stderr=PIPE)
@@ -104,36 +159,41 @@ def lets_run_a_test(name):
     sys.stdout.flush()
     sys.stderr.flush()
 
-    # APPVEYOR = SLOW
-    timeout = 15*60 if sys.platform == 'win32' else 5*60
+    mavproxy_verbose = os.environ.get('TEST_VERBOSE', '0') != '0'
+    timeout = 5*60
+
     try:
-        p = Popen([sys.executable, '-m', 'MAVProxy.mavproxy', '--logfile=' + tempfile.mkstemp()[1], '--master=tcp:127.0.0.1:5760'], cwd=testpath, env=newenv, stdin=PIPE, stdout=PIPE)#, stderr=PIPE)
+        p = Popen([sys.executable, '-m', 'MAVProxy.mavproxy', '--logfile=' + tempfile.mkstemp()[1], '--master=tcp:127.0.0.1:5760'],
+            cwd=testpath, env=newenv, stdin=PIPE, stdout=PIPE,
+            stderr=PIPE if not mavproxy_verbose else None)
         bg.append(p)
 
+        # TODO this sleep is only for us to waiting until
+        # all parameters to be received; would prefer to
+        # move this to testlib.py and happen asap
         while p.poll() == None:
             line = p.stdout.readline()
-            sys.stdout.write(line)
-            sys.stdout.flush()
+            if mavproxy_verbose:
+                sys.stdout.write(line)
+                sys.stdout.flush()
             if 'parameters' in line:
                 break
 
-        # TODO this sleep is only for us to waiting until
-        # all parameters to be received; would prefer to 
-        # move this to testlib.py and happen asap
         time.sleep(3)
-        p.stdin.write('module load droneapi.module.api\n')
+
+        # NOTE these are *very inappropriate settings*
+        # to make on a real vehicle. They are leveraged
+        # exclusively for simulation. Take heed!!!
         p.stdin.write('param set ARMING_CHECK 0\n')
+        p.stdin.write('param set FS_THR_ENABLE 0\n')
+        p.stdin.write('param set FS_GCS_ENABLE 0\n')
+        p.stdin.write('param set EKF_CHECK_THRESH 0\n')
+
+        p.stdin.write('module load droneapi.module.api\n')
         p.stdin.write('api start testlib.py\n')
         p.stdin.flush()
 
-        while True:
-            nextline = p.stdout.readline()
-            if nextline == '' and p.poll() != None:
-                break
-            sys.stdout.write(nextline)
-            sys.stdout.flush()
-
-        # wait_timeout(p, timeout)
+        wait_timeout(p, timeout, mavproxy_verbose)
     except RuntimeError:
         kill(p.pid)
         p.returncode = 143
@@ -148,17 +208,31 @@ def lets_run_a_test(name):
     bg.remove(sitl)
 
     if p.returncode != 0:
-        print('[runner] ...aborting with dronekit error code ' + str(p.returncode))
-        sys.stdout.flush()
-        sys.stderr.flush()
-        sys.exit(p.returncode)
-    
-    print('[runner] ...success.')
-    time.sleep(5)
+        print('[runner] ...failed with dronekit error code ' + str(p.returncode))
+    else:
+        print('[runner] ...success.')
 
-for i in os.listdir(testpath):
-    if i.startswith('test_') and i.endswith('.py'):
-        lets_run_a_test(i[:-3])
+    sys.stdout.flush()
+    sys.stderr.flush()
+    time.sleep(5)
+    return p.returncode
+
+retry = int(os.environ.get('TEST_RETRY', '1'))
+filelist = sys.argv[1:] if len(sys.argv) > 1 else os.listdir(testpath)
+for path in filelist:
+    assert os.path.isfile(os.path.join(testpath, path)), '"%s" is not a valid test file' % (path,)
+    if path.startswith('test_') and path.endswith('.py'):
+        name = path[:-3]
+        i = retry
+        while True:
+            ret = lets_run_a_test(name)
+            if ret == 0:
+                break
+            i = i - 1
+            if i == 0:
+                print('[runner] aborting after failed test.')
+                sys.exit(ret)
+            print('[runner] retrying %s %s more times' % (name, i, ))
 
 print('[runner] finished.')
 sys.stdout.flush()

tests/sitl/test_goto.py

@@ -0,0 +1,94 @@
+"""
+simple_goto.py: GUIDED mode "simple goto" example (Copter Only)
+
+The example demonstrates how to arm and takeoff in Copter and how to navigate to 
+points using Vehicle.commands.goto.
+
+Full documentation is provided at http://python.dronekit.io/examples/simple_goto.html
+"""
+
+import time
+from droneapi.lib import VehicleMode, Location
+from pymavlink import mavutil
+from testlib import assert_equals
+
+def test_goto(local_connect):
+    api = local_connect()
+    vehicle = api.get_vehicles()[0]
+
+    def arm_and_takeoff(aTargetAltitude):
+        """
+        Arms vehicle and fly to aTargetAltitude.
+        """
+
+        print "Basic pre-arm checks"
+        # Don't let the user try to fly autopilot is booting
+        if vehicle.mode.name == "INITIALISING":
+            print "Waiting for vehicle to initialise"
+            time.sleep(1)
+        while vehicle.gps_0.fix_type < 2:
+            print "Waiting for GPS...:", vehicle.gps_0.fix_type
+            time.sleep(1)
+            
+        print "Arming motors"
+        # Copter should arm in GUIDED mode
+        vehicle.mode    = VehicleMode("GUIDED")
+        vehicle.flush()
+
+        i = 60
+        while not api.exit and vehicle.mode.name != 'GUIDED' and i > 0:
+            print " Waiting for guided %s seconds..." % (i,)
+            time.sleep(1)
+            i = i - 1
+
+        vehicle.armed   = True
+        vehicle.flush()
+
+        i = 60
+        while not api.exit and not vehicle.armed and vehicle.mode.name == 'GUIDED' and i > 0:
+            print " Waiting for arming %s seconds..." % (i,)
+            time.sleep(1)
+            i = i - 1
+
+        # Failure will result in arming but immediately landing
+        assert vehicle.armed
+        assert_equals(vehicle.mode.name, 'GUIDED')
+
+        print "Taking off!"
+        vehicle.commands.takeoff(aTargetAltitude) # Take off to target altitude
+        vehicle.flush()
+
+        # Wait until the vehicle reaches a safe height before
+        # processing the goto (otherwise the command after
+        # Vehicle.commands.takeoff will execute immediately).
+        while not api.exit:
+            print " Altitude: ", vehicle.location.alt
+            # Test for altitude just below target, in case of undershoot.
+            if vehicle.location.alt >= aTargetAltitude * 0.95: 
+                print "Reached target altitude"
+                break;
+
+            assert_equals(vehicle.mode.name, 'GUIDED')
+            time.sleep(1)
+
+    arm_and_takeoff(10)
+
+    print "Going to first point..."
+    point1 = Location(-35.361354, 149.165218, 20, is_relative=True)
+    vehicle.commands.goto(point1)
+    vehicle.flush()
+
+    # sleep so we can see the change in map
+    time.sleep(3)
+
+    print "Going to second point..."
+    point2 = Location(-35.363244, 149.168801, 20, is_relative=True)
+    vehicle.commands.goto(point2)
+    vehicle.flush()
+
+    # sleep so we can see the change in map
+    time.sleep(3)
+
+    print "Returning to Launch"
+    vehicle.mode    = VehicleMode("RTL")
+    vehicle.flush()