From 89e9ce6ea3f1b64caefc8e664bc08c9ab7fa1013 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Mar 2020 11:00:23 +0100 Subject: [PATCH 01/51] mavsdk_tests: MAVSDK is required Without the library installed, we can't build and run the tests. Silently ignoring this just leads to confusion. --- test/mavsdk_tests/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index 196c5fcfe87c..71e1b6ea0e7c 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/test/mavsdk_tests/CMakeLists.txt @@ -6,7 +6,7 @@ set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -find_package(MAVSDK QUIET) +find_package(MAVSDK REQUIRED) if(MAVSDK_FOUND) add_executable(mavsdk_tests From 7453599eeee9bffca44f797f25152da6f1171c9f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Mar 2020 13:34:22 +0100 Subject: [PATCH 02/51] mavsdk_tests: PEP8 fixes --- test/mavsdk_tests/mavsdk_test_runner.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index c544192cf34c..d2566722cc0a 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -136,7 +136,8 @@ def __init__(self, model, workspace_dir, log_dir, speed_factor, debugger): if not debugger: pass elif debugger == "valgrind": - self.args = ["--track-origins=yes", "--leak-check=full", "-v", self.cmd] + self.args + self.args = ["--track-origins=yes", "--leak-check=full", "-v", + self.cmd] + self.args self.cmd = "valgrind" elif debugger == "callgrind": self.args = ["--tool=callgrind", "-v", self.cmd] + self.args @@ -145,7 +146,7 @@ def __init__(self, model, workspace_dir, log_dir, speed_factor, debugger): self.args = ["--args", self.cmd] + self.args self.cmd = "gdb" else: - print("Using custom debugger " , debugger) + print("Using custom debugger ", debugger) self.args = [self.cmd] + self.args self.cmd = debugger @@ -325,7 +326,8 @@ def run_test_group(args): def run_test(test, group, args): px4_runner = Px4Runner( - group['model'], os.getcwd(), args.log_dir, args.speed_factor, args.debugger) + group['model'], os.getcwd(), args.log_dir, args.speed_factor, + args.debugger) px4_runner.start(group) gzserver_runner = GzserverRunner( From c6c259a5a12c87c1dfcb528ddcf24e0362572d0e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Mar 2020 18:42:47 +0100 Subject: [PATCH 03/51] simulator: remove redundant semicolon --- src/modules/simulator/simulator_mavlink.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 267c67c2814b..61b01d2b249e 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -963,7 +963,7 @@ int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink { optical_flow_s flow = {}; flow.sensor_id = flow_mavlink->sensor_id; - flow.timestamp = hrt_absolute_time();; + flow.timestamp = hrt_absolute_time(); flow.time_since_last_sonar_update = 0; flow.frame_count_since_last_readout = 0; // ? flow.integration_timespan = flow_mavlink->integration_time_us; From d0eac74fa89de4d90b22429311657004b28226a1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Mar 2020 18:13:15 +0100 Subject: [PATCH 04/51] mavsdk_tests: add max speed factor This is required because some tests don't work at more than 1x. --- test/mavsdk_tests/mavsdk_test_runner.py | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index d2566722cc0a..70848dc7951b 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -18,19 +18,21 @@ "timeout_min": 20, }, { - "model": "iris_opt_flow", + "model": "iris_opt_flow_mockup", "test_filter": "[multicopter_offboard]", "timeout_min": 20, }, { - "model": "iris_opt_flow_mockup", + "model": "iris_opt_flow", "test_filter": "[multicopter_offboard]", "timeout_min": 20, + "max_speed_factor": 1, }, { "model": "iris_vision", "test_filter": "[multicopter_offboard]", "timeout_min": 20, + "max_speed_factor": 1, }, { "model": "standard_vtol", @@ -325,13 +327,18 @@ def run_test_group(args): def run_test(test, group, args): + + speed_factor = args.speed_factor + if "max_speed_factor" in group: + speed_factor = max(int(speed_factor), group["max_speed_factor"]) + px4_runner = Px4Runner( - group['model'], os.getcwd(), args.log_dir, args.speed_factor, + group['model'], os.getcwd(), args.log_dir, speed_factor, args.debugger) px4_runner.start(group) gzserver_runner = GzserverRunner( - group['model'], os.getcwd(), args.log_dir, args.speed_factor) + group['model'], os.getcwd(), args.log_dir, speed_factor) gzserver_runner.start(group) if args.gui: From 42bc412fea272109c14383ddae98716c9e8f7809 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 5 Mar 2020 10:50:55 +0100 Subject: [PATCH 05/51] mavsdk_tests: remove opt_flow test It doesn't actually seem to work. --- test/mavsdk_tests/mavsdk_test_runner.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 70848dc7951b..4dede4cfbcd5 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -22,12 +22,6 @@ "test_filter": "[multicopter_offboard]", "timeout_min": 20, }, - { - "model": "iris_opt_flow", - "test_filter": "[multicopter_offboard]", - "timeout_min": 20, - "max_speed_factor": 1, - }, { "model": "iris_vision", "test_filter": "[multicopter_offboard]", From 2a5093b923f5f5829bc28734c0c822cded0777f8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 5 Mar 2020 10:59:33 +0100 Subject: [PATCH 06/51] mavsdk_tests: use the min of the speed factors Facepalm. --- test/mavsdk_tests/mavsdk_test_runner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 4dede4cfbcd5..6686b84e61b9 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -324,7 +324,7 @@ def run_test(test, group, args): speed_factor = args.speed_factor if "max_speed_factor" in group: - speed_factor = max(int(speed_factor), group["max_speed_factor"]) + speed_factor = min(int(speed_factor), group["max_speed_factor"]) px4_runner = Px4Runner( group['model'], os.getcwd(), args.log_dir, speed_factor, From 7a803081f6d96452e023a30970e751b68cd7b17b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 5 Mar 2020 11:02:09 +0100 Subject: [PATCH 07/51] mavsdk_tests: remove plane tests We can put them back once it's supported. --- test/mavsdk_tests/mavsdk_test_runner.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 6686b84e61b9..1aea969263ce 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -33,11 +33,6 @@ "test_filter": "[vtol]", "timeout_min": 20, }, - # { - # "model": "plane", - # "test_filter": "[plane]", - # "timeout_min": 25, - # } ] From c1bc9ee0246716f004503c56fb4ad15b2fafa739 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 5 Mar 2020 11:18:43 +0100 Subject: [PATCH 08/51] mavsdk_tests: move config into json file --- test/mavsdk_tests/configs/sitl.json | 23 +++++++++++++++++++ test/mavsdk_tests/mavsdk_test_runner.py | 30 +++++-------------------- 2 files changed, 28 insertions(+), 25 deletions(-) create mode 100644 test/mavsdk_tests/configs/sitl.json diff --git a/test/mavsdk_tests/configs/sitl.json b/test/mavsdk_tests/configs/sitl.json new file mode 100644 index 000000000000..a6636141e5c4 --- /dev/null +++ b/test/mavsdk_tests/configs/sitl.json @@ -0,0 +1,23 @@ +[ + { + "model": "iris", + "test_filter": "[multicopter]", + "timeout_min": 20 + }, + { + "model": "iris_opt_flow_mockup", + "test_filter": "[multicopter_offboard]", + "timeout_min": 20 + }, + { + "model": "iris_vision", + "test_filter": "[multicopter_offboard]", + "timeout_min": 20, + "max_speed_factor": 1 + }, + { + "model": "standard_vtol", + "test_filter": "[vtol]", + "timeout_min": 20 + } +] diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 1aea969263ce..908651ebee50 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -4,6 +4,7 @@ import atexit import datetime import errno +import json import os import psutil import subprocess @@ -11,31 +12,6 @@ import signal -test_matrix = [ - { - "model": "iris", - "test_filter": "[multicopter]", - "timeout_min": 20, - }, - { - "model": "iris_opt_flow_mockup", - "test_filter": "[multicopter_offboard]", - "timeout_min": 20, - }, - { - "model": "iris_vision", - "test_filter": "[multicopter_offboard]", - "timeout_min": 20, - "max_speed_factor": 1, - }, - { - "model": "standard_vtol", - "test_filter": "[vtol]", - "timeout_min": 20, - }, -] - - class Runner: def __init__(self, log_dir): self.cmd = "" @@ -202,6 +178,7 @@ def main(): help="Specify which model to run") parser.add_argument("--debugger", default="", help="valgrind callgrind gdb lldb") + parser.add_argument("config_file", help="JSON config file to use") args = parser.parse_args() if not is_everything_ready(): @@ -281,6 +258,9 @@ def run(args): def run_test_group(args): overall_success = True + with open(args.config_file) as json_file: + test_matrix = json.load(json_file) + if args.model == 'all': models = test_matrix else: From 44f761a34c0a7a81138f049b17fe55eed08f0e8a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 5 Mar 2020 14:58:14 +0100 Subject: [PATCH 09/51] mavsdk_tests: without speedup the VTOL takes time --- test/mavsdk_tests/autopilot_tester.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index a43b7d758f03..2c78bec640d7 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -89,7 +89,7 @@ void AutopilotTester::wait_until_disarmed() void AutopilotTester::wait_until_hovering() { REQUIRE(poll_condition_with_timeout( - [this]() { return _telemetry->landed_state() == Telemetry::LandedState::IN_AIR; }, std::chrono::seconds(10))); + [this]() { return _telemetry->landed_state() == Telemetry::LandedState::IN_AIR; }, std::chrono::seconds(20))); } void AutopilotTester::prepare_square_mission(MissionOptions mission_options) From a289b25099a5d509749349ac2abeb2b28d6e9b53 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 5 Mar 2020 15:28:20 +0100 Subject: [PATCH 10/51] mavsdk_tests: add more config into JSON --- test/mavsdk_tests/configs/sitl.json | 52 ++++++----- test/mavsdk_tests/mavsdk_test_runner.py | 116 ++++++++++++++---------- test/mavsdk_tests/test_main.cpp | 2 +- 3 files changed, 97 insertions(+), 73 deletions(-) diff --git a/test/mavsdk_tests/configs/sitl.json b/test/mavsdk_tests/configs/sitl.json index a6636141e5c4..3406c57d98ca 100644 --- a/test/mavsdk_tests/configs/sitl.json +++ b/test/mavsdk_tests/configs/sitl.json @@ -1,23 +1,29 @@ -[ - { - "model": "iris", - "test_filter": "[multicopter]", - "timeout_min": 20 - }, - { - "model": "iris_opt_flow_mockup", - "test_filter": "[multicopter_offboard]", - "timeout_min": 20 - }, - { - "model": "iris_vision", - "test_filter": "[multicopter_offboard]", - "timeout_min": 20, - "max_speed_factor": 1 - }, - { - "model": "standard_vtol", - "test_filter": "[vtol]", - "timeout_min": 20 - } -] +{ + "mode": "sitl", + "simulator": "gazebo", + "mavlink_connection": "udp://:14540", + "tests": + [ + { + "model": "iris", + "test_filter": "[multicopter]", + "timeout_min": 20 + }, + { + "model": "iris_opt_flow_mockup", + "test_filter": "[multicopter_offboard]", + "timeout_min": 20 + }, + { + "model": "iris_vision", + "test_filter": "[multicopter_offboard]", + "timeout_min": 20, + "max_speed_factor": 1 + }, + { + "model": "standard_vtol", + "test_filter": "[vtol]", + "timeout_min": 20 + } + ] +} diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 908651ebee50..b138ac17966b 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -152,12 +152,13 @@ def __init__(self, workspace_dir, log_dir): class TestRunner(Runner): - def __init__(self, workspace_dir, log_dir, config, test): + def __init__(self, workspace_dir, log_dir, config, test, + mavlink_connection): super().__init__(log_dir) self.env = {"PATH": os.environ['PATH']} self.cmd = workspace_dir + \ "/build/px4_sitl_default/mavsdk_tests/mavsdk_tests" - self.args = [test] + self.args = ["--url", mavlink_connection, test] self.log_prefix = "test_runner" @@ -181,10 +182,18 @@ def main(): parser.add_argument("config_file", help="JSON config file to use") args = parser.parse_args() - if not is_everything_ready(): + with open(args.config_file) as json_file: + config = json.load(json_file) + + if config["mode"] != "sitl" and args.gui: + print("--gui is not compatible with the mode '{}'" + .format(config["mode"])) + sys.exit(1) + + if not is_everything_ready(config): sys.exit(1) - run(args) + run(args, config) def determine_tests(workspace_dir, filter): @@ -206,41 +215,46 @@ def is_running(process_name): return False -def is_everything_ready(): +def is_everything_ready(config): result = True - if is_running('px4'): - print("px4 process already running\n" - "run `killall px4` and try again") - result = False - if is_running('gzserver'): - print("gzserver process already running\n" - "run `killall gzserver` and try again") - result = False - if is_running('gzclient'): - print("gzclient process already running\n" - "run `killall gzclient` and try again") - result = False - if not os.path.isfile('build/px4_sitl_default/bin/px4'): - print("PX4 SITL is not built\n" - "run `DONT_RUN=1 " - "make px4_sitl gazebo mavsdk_tests`") - result = False + + if config['mode'] == 'sitl': + if is_running('px4'): + print("px4 process already running\n" + "run `killall px4` and try again") + result = False + if not os.path.isfile('build/px4_sitl_default/bin/px4'): + print("PX4 SITL is not built\n" + "run `DONT_RUN=1 " + "make px4_sitl gazebo mavsdk_tests`") + result = False + if config['simulator'] == 'gazebo': + if is_running('gzserver'): + print("gzserver process already running\n" + "run `killall gzserver` and try again") + result = False + if is_running('gzclient'): + print("gzclient process already running\n" + "run `killall gzclient` and try again") + result = False + if not os.path.isfile('build/px4_sitl_default/mavsdk_tests/mavsdk_tests'): print("Test runner is not built\n" "run `DONT_RUN=1 " "make px4_sitl gazebo mavsdk_tests`") result = False + return result -def run(args): +def run(args, config): overall_success = True for iteration in range(args.iterations): if args.iterations > 1: print("Test iteration: {}".format(iteration + 1, args.iterations)) - was_success = run_test_group(args) + was_success = run_test_group(args, config) if not was_success: overall_success = False @@ -255,11 +269,10 @@ def run(args): sys.exit(0 if overall_success else 1) -def run_test_group(args): +def run_test_group(args, config): overall_success = True - with open(args.config_file) as json_file: - test_matrix = json.load(json_file) + test_matrix = config["tests"] if args.model == 'all': models = test_matrix @@ -281,55 +294,60 @@ def run_test_group(args): for test in tests: print("Running test '{}'".format(test)) - was_success = run_test(test, group, args) + was_success = run_test(test, group, args, config) print("Test '{}': {}". format(test, "Success" if was_success else "Fail")) if not was_success: overall_success = False - if not was_success and args.fail_early: + if not was_success and args.abort_early: print("Aborting early") return False return overall_success -def run_test(test, group, args): +def run_test(test, group, args, config): speed_factor = args.speed_factor if "max_speed_factor" in group: speed_factor = min(int(speed_factor), group["max_speed_factor"]) - px4_runner = Px4Runner( - group['model'], os.getcwd(), args.log_dir, speed_factor, - args.debugger) - px4_runner.start(group) + if config['mode'] == 'sitl': + px4_runner = Px4Runner( + group['model'], os.getcwd(), args.log_dir, speed_factor, + args.debugger) + px4_runner.start(group) - gzserver_runner = GzserverRunner( - group['model'], os.getcwd(), args.log_dir, speed_factor) - gzserver_runner.start(group) + if config['simulator'] == 'gazebo': + gzserver_runner = GzserverRunner( + group['model'], os.getcwd(), args.log_dir, speed_factor) + gzserver_runner.start(group) - if args.gui: - gzclient_runner = GzclientRunner( - os.getcwd(), args.log_dir) - gzclient_runner.start(group) + if args.gui: + gzclient_runner = GzclientRunner( + os.getcwd(), args.log_dir) + gzclient_runner.start(group) - test_runner = TestRunner(os.getcwd(), args.log_dir, group, test) + test_runner = TestRunner(os.getcwd(), args.log_dir, group, test, + config['mavlink_connection']) test_runner.start(group) returncode = test_runner.wait(group['timeout_min']) is_success = (returncode == 0) - if args.gui: - returncode = gzclient_runner.stop() - print("gzclient exited with {}".format(returncode)) + if config['mode'] == 'sitl': + if config['simulator'] == 'gazebo': + if args.gui: + returncode = gzclient_runner.stop() + print("gzclient exited with {}".format(returncode)) - returncode = gzserver_runner.stop() - print("gzserver exited with {}".format(returncode)) + returncode = gzserver_runner.stop() + print("gzserver exited with {}".format(returncode)) - px4_runner.stop() - print("px4 exited with {}".format(returncode)) + px4_runner.stop() + print("px4 exited with {}".format(returncode)) return is_success diff --git a/test/mavsdk_tests/test_main.cpp b/test/mavsdk_tests/test_main.cpp index cabd43362639..65753cef115d 100644 --- a/test/mavsdk_tests/test_main.cpp +++ b/test/mavsdk_tests/test_main.cpp @@ -63,4 +63,4 @@ void remove_argv(int& argc, char** argv, int pos) argv[i] = argv[i+1]; } argv[--argc] = nullptr; -} \ No newline at end of file +} From 80fcfc7089ddecb2bd37c14c961cf56ac550940c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 5 Mar 2020 15:28:35 +0100 Subject: [PATCH 11/51] mavsdk_tests: improve naming of argument We abort when we already failed so this makes more sense. --- test/mavsdk_tests/mavsdk_test_runner.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index b138ac17966b..4ace64f92e4e 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -171,7 +171,7 @@ def main(): help="How fast to run the simulation") parser.add_argument("--iterations", type=int, default=1, help="How often to run all tests") - parser.add_argument("--fail-early", action='store_true', + parser.add_argument("--abort-early", action='store_true', help="Abort on first unsuccessful test") parser.add_argument("--gui", default=False, action='store_true', help="Display gzclient with simulation") @@ -259,7 +259,7 @@ def run(args, config): if not was_success: overall_success = False - if args.iterations > 1 and not was_success and args.fail_early: + if args.iterations > 1 and not was_success and args.abort_early: print("Aborting with a failure in test run {}/{}". format(iteration + 1, args.iterations)) break From eae94d6da57fc92e1f1a82eb55b9cef9d5ad3cd5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 5 Mar 2020 16:41:20 +0100 Subject: [PATCH 12/51] workflows: adapt to mavsdk_tests cli changes --- .github/workflows/sitl_tests.yml | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/.github/workflows/sitl_tests.yml b/.github/workflows/sitl_tests.yml index 820c067c4948..829cfd0c6788 100644 --- a/.github/workflows/sitl_tests.yml +++ b/.github/workflows/sitl_tests.yml @@ -33,16 +33,8 @@ jobs: run: DONT_RUN=1 make px4_sitl PX4_CMAKE_BUILD_TYPE=Coverage gazebo mavsdk_tests - name: ccache post-run run: ccache -s && ccache -z - - # Run mavsdk tests - - name: Run multicopter simulation test - run: test/mavsdk_tests/mavsdk_test_runner.py --model iris --speed-factor 20 --iterations 1 --fail-early - - name: Run multicopter with optical flow simulation test - run: test/mavsdk_tests/mavsdk_test_runner.py --model iris_opt_flow_mockup --speed-factor 1 --iterations 1 --fail-early - - name: Run multicopter with VIO sensor simulation test - run: test/mavsdk_tests/mavsdk_test_runner.py --model iris_vision --speed-factor 1 --iterations 1 --fail-early - - name: Run VTOL simulation test # - run: test/mavsdk_tests/mavsdk_test_runner.py --model standard_vtol --speed-factor 20 --iterations 1 --fail-early + - name: Run SITL tests + run: test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 20 --abort-early test/mavsdk_tests/configs/sitl.json # Report test coverage - name: disable the keychain credential helper From 6fccbad842cf40019e092aa316d099c8179bdd63 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Mar 2020 12:41:28 +0100 Subject: [PATCH 13/51] mavsdk_tests: consolidate level of abstraction This moves the CHECK/REQUIRE inside of AutopilotTester. --- test/mavsdk_tests/autopilot_tester.cpp | 23 ++++++++++++++----- test/mavsdk_tests/autopilot_tester.h | 17 +++++++++----- .../test_multicopter_offboard.cpp | 10 ++++---- 3 files changed, 32 insertions(+), 18 deletions(-) diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 2c78bec640d7..cd9f887ee09a 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -42,6 +42,17 @@ void AutopilotTester::wait_until_ready_local_position_only() }, std::chrono::seconds(20))); } +void AutopilotTester::store_home() +{ + request_ground_truth(); + _home = get_ground_truth_position(); +} + +void AutopilotTester::check_home_within(float acceptance_radius_m) +{ + CHECK(ground_truth_horizontal_position_close_to(_home, acceptance_radius_m)); +} + void AutopilotTester::set_takeoff_altitude(const float altitude_m) { CHECK(Action::Result::SUCCESS == _action->set_takeoff_altitude(altitude_m)); @@ -94,12 +105,12 @@ void AutopilotTester::wait_until_hovering() void AutopilotTester::prepare_square_mission(MissionOptions mission_options) { - const auto ct = _get_coordinate_transformation(); + const auto ct = get_coordinate_transformation(); std::vector> mission_items {}; - mission_items.push_back(_create_mission_item({mission_options.leg_length_m, 0.}, mission_options, ct)); - mission_items.push_back(_create_mission_item({mission_options.leg_length_m, mission_options.leg_length_m}, mission_options, ct)); - mission_items.push_back(_create_mission_item({0., mission_options.leg_length_m}, mission_options, ct)); + mission_items.push_back(create_mission_item({mission_options.leg_length_m, 0.}, mission_options, ct)); + mission_items.push_back(create_mission_item({mission_options.leg_length_m, mission_options.leg_length_m}, mission_options, ct)); + mission_items.push_back(create_mission_item({0., mission_options.leg_length_m}, mission_options, ct)); _mission->set_return_to_launch_after_mission(mission_options.rtl_at_end); @@ -132,7 +143,7 @@ void AutopilotTester::execute_mission() REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready); } -CoordinateTransformation AutopilotTester::_get_coordinate_transformation() +CoordinateTransformation AutopilotTester::get_coordinate_transformation() { const auto home = _telemetry->home_position(); REQUIRE(std::isfinite(home.latitude_deg)); @@ -140,7 +151,7 @@ CoordinateTransformation AutopilotTester::_get_coordinate_transformation() return CoordinateTransformation({home.latitude_deg, home.longitude_deg}); } -std::shared_ptr AutopilotTester::_create_mission_item( +std::shared_ptr AutopilotTester::create_mission_item( const CoordinateTransformation::LocalCoordinate& local_coordinate, const MissionOptions& mission_options, const CoordinateTransformation& ct) diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 9ce340d280fa..3b0b6d8bf7d6 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -27,6 +27,8 @@ class AutopilotTester { void connect(const std::string uri); void wait_until_ready(); void wait_until_ready_local_position_only(); + void store_home(); + void check_home_within(float acceptance_radius_m); void set_takeoff_altitude(const float altitude_m); void arm(); void takeoff(); @@ -41,25 +43,28 @@ class AutopilotTester { void offboard_goto(const Offboard::PositionNEDYaw& target, float acceptance_radius = 0.3f, std::chrono::seconds timeout_duration = std::chrono::seconds(60)); void offboard_land(); - bool estimated_position_close_to(const Offboard::PositionNEDYaw& target_position, float acceptance_radius_m); - bool estimated_horizontal_position_close_to(const Offboard::PositionNEDYaw& target_pos, float acceptance_radius_m); void request_ground_truth(); - Telemetry::GroundTruth get_ground_truth_position(); - bool ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth& target_pos, float acceptance_radius_m); private: - mavsdk::geometry::CoordinateTransformation _get_coordinate_transformation(); - std::shared_ptr _create_mission_item( + mavsdk::geometry::CoordinateTransformation get_coordinate_transformation(); + std::shared_ptr create_mission_item( const mavsdk::geometry::CoordinateTransformation::LocalCoordinate& local_coordinate, const MissionOptions& mission_options, const mavsdk::geometry::CoordinateTransformation& ct); + Telemetry::GroundTruth get_ground_truth_position(); + + bool ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth& target_pos, float acceptance_radius_m); + bool estimated_position_close_to(const Offboard::PositionNEDYaw& target_position, float acceptance_radius_m); + bool estimated_horizontal_position_close_to(const Offboard::PositionNEDYaw& target_pos, float acceptance_radius_m); mavsdk::Mavsdk _mavsdk{}; std::unique_ptr _telemetry{}; std::unique_ptr _action{}; std::unique_ptr _mission{}; std::unique_ptr _offboard{}; + + Telemetry::GroundTruth _home{NAN, NAN, NAN}; }; template diff --git a/test/mavsdk_tests/test_multicopter_offboard.cpp b/test/mavsdk_tests/test_multicopter_offboard.cpp index 89552c814117..18430b27278b 100644 --- a/test/mavsdk_tests/test_multicopter_offboard.cpp +++ b/test/mavsdk_tests/test_multicopter_offboard.cpp @@ -17,13 +17,12 @@ TEST_CASE("Takeoff and Land (Multicopter offboard)", "[multicopter_offboard]") Offboard::PositionNEDYaw takeoff_position {0.0f, 0.0f, -2.0f, 0.0f}; tester.connect(connection_url); tester.wait_until_ready_local_position_only(); - tester.request_ground_truth(); - Telemetry::GroundTruth home = tester.get_ground_truth_position(); + tester.store_home(); tester.arm(); tester.offboard_goto(takeoff_position, 0.5f); tester.offboard_land(); tester.wait_until_disarmed(); - REQUIRE(tester.ground_truth_horizontal_position_close_to(home, 0.5f)); + tester.check_home_within(0.5f); } TEST_CASE("Mission (Multicopter offboard )", "[multicopter_offboard]") @@ -35,8 +34,7 @@ TEST_CASE("Mission (Multicopter offboard )", "[multicopter_offboard]") Offboard::PositionNEDYaw setpoint_3 {5.0f, 0.0f, -4.0f, 90.0f}; tester.connect(connection_url); tester.wait_until_ready_local_position_only(); - tester.request_ground_truth(); - Telemetry::GroundTruth home = tester.get_ground_truth_position(); + tester.store_home(); tester.arm(); tester.offboard_goto(takeoff_position, 0.5f); tester.offboard_goto(setpoint_1, 1.0f); @@ -45,5 +43,5 @@ TEST_CASE("Mission (Multicopter offboard )", "[multicopter_offboard]") tester.offboard_goto(takeoff_position, 0.2f); tester.offboard_land(); tester.wait_until_disarmed(); - REQUIRE(tester.ground_truth_horizontal_position_close_to(home, 1.0f)); + tester.check_home_within(1.0f); } From 062c3c245be74d4b063b4d6d39cd62a1a9905d7b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Mar 2020 12:42:33 +0100 Subject: [PATCH 14/51] mavsdk_tests: add missing unit --- test/mavsdk_tests/autopilot_tester.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 3b0b6d8bf7d6..3762eb0a0cb3 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -40,7 +40,7 @@ class AutopilotTester { void prepare_square_mission(MissionOptions mission_options); void execute_mission(); void execute_rtl(); - void offboard_goto(const Offboard::PositionNEDYaw& target, float acceptance_radius = 0.3f, + void offboard_goto(const Offboard::PositionNEDYaw& target, float acceptance_radius_m = 0.3f, std::chrono::seconds timeout_duration = std::chrono::seconds(60)); void offboard_land(); void request_ground_truth(); From c1afdf40bf8de70b214d3a56995f6d01f0ead00f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Mar 2020 12:42:53 +0100 Subject: [PATCH 15/51] mavsdk_tests: remove unused/not working test We can add it again once fixedwing is supported. Until then, this is just confusing. --- test/mavsdk_tests/CMakeLists.txt | 1 - test/mavsdk_tests/test_mission_plane.cpp | 57 ------------------------ 2 files changed, 58 deletions(-) delete mode 100644 test/mavsdk_tests/test_mission_plane.cpp diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index 71e1b6ea0e7c..50ee31ce180f 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/test/mavsdk_tests/CMakeLists.txt @@ -14,7 +14,6 @@ if(MAVSDK_FOUND) autopilot_tester.cpp test_multicopter_offboard.cpp test_mission_multicopter.cpp - test_mission_plane.cpp test_mission_vtol.cpp ) diff --git a/test/mavsdk_tests/test_mission_plane.cpp b/test/mavsdk_tests/test_mission_plane.cpp deleted file mode 100644 index ee23c553f966..000000000000 --- a/test/mavsdk_tests/test_mission_plane.cpp +++ /dev/null @@ -1,57 +0,0 @@ -// -// Plane mission test. -// -// Author: Lorenz Meier , Julian Oes - -#include -#include -#include -#include -#include -#include "autopilot_tester.h" - - -TEST_CASE("Takeoff and land (plane)", "[plane]") -{ - AutopilotTester tester; - tester.connect(connection_url); - tester.wait_until_ready(); - tester.arm(); - tester.takeoff(); - tester.wait_until_hovering(); - tester.land(); - tester.wait_until_disarmed(); -} - -// TODO: Add land pattern - -// TEST_CASE("Fly square missions (plane)", "[plane]") -// { -// AutopilotTester tester; -// tester.connect(connection_url); -// tester.wait_until_ready(); - -// SECTION("Mission including RTL (plane)") { -// AutopilotTester::MissionOptions mission_options; -// mission_options.leg_length_m = 250.0; -// mission_options.relative_altitude_m = 40.0; -// mission_options.rtl_at_end = true; -// tester.prepare_square_mission(mission_options); -// tester.arm(); -// tester.execute_mission(); -// tester.wait_until_disarmed(); -// } - -// SECTION("Mission with manual RTL (plane)") { -// AutopilotTester::MissionOptions mission_options; -// mission_options.leg_length_m = 250.0; -// mission_options.relative_altitude_m = 40.0; -// mission_options.rtl_at_end = false; -// tester.prepare_square_mission(mission_options); -// tester.arm(); -// tester.execute_mission(); -// tester.wait_until_hovering(); -// tester.execute_rtl(); -// tester.wait_until_disarmed(); -// } -// } From 205c83689f5ed0263aefb83b54d0e64463a76cda Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Mar 2020 12:45:45 +0100 Subject: [PATCH 16/51] mavsdk_tests: use CHECK if we don't need to abort --- test/mavsdk_tests/autopilot_tester.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index cd9f887ee09a..012037f5a9bf 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -146,8 +146,8 @@ void AutopilotTester::execute_mission() CoordinateTransformation AutopilotTester::get_coordinate_transformation() { const auto home = _telemetry->home_position(); - REQUIRE(std::isfinite(home.latitude_deg)); - REQUIRE(std::isfinite(home.longitude_deg)); + CHECK(std::isfinite(home.latitude_deg)); + CHECK(std::isfinite(home.longitude_deg)); return CoordinateTransformation({home.latitude_deg, home.longitude_deg}); } @@ -172,7 +172,7 @@ void AutopilotTester::offboard_goto(const Offboard::PositionNEDYaw& target, floa { _offboard->set_position_ned(target); REQUIRE(_offboard->start() == Offboard::Result::SUCCESS); - REQUIRE(poll_condition_with_timeout( + CHECK(poll_condition_with_timeout( [=]() { return estimated_position_close_to(target, acceptance_radius_m); }, timeout_duration)); std::cout << "Target position reached" << std::endl; } @@ -204,7 +204,7 @@ bool AutopilotTester::estimated_horizontal_position_close_to(const Offboard::Pos void AutopilotTester::request_ground_truth() { - REQUIRE(_telemetry->set_rate_ground_truth(15) == Telemetry::Result::SUCCESS); + CHECK(_telemetry->set_rate_ground_truth(15) == Telemetry::Result::SUCCESS); std::this_thread::sleep_for(std::chrono::milliseconds(100)); } @@ -215,15 +215,15 @@ Telemetry::GroundTruth AutopilotTester::get_ground_truth_position() bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth& target_pos, float acceptance_radius_m) { - REQUIRE(std::isfinite(target_pos.latitude_deg)); - REQUIRE(std::isfinite(target_pos.longitude_deg)); + CHECK(std::isfinite(target_pos.latitude_deg)); + CHECK(std::isfinite(target_pos.longitude_deg)); using GlobalCoordinate = CoordinateTransformation::GlobalCoordinate; using LocalCoordinate = CoordinateTransformation::LocalCoordinate; CoordinateTransformation ct(GlobalCoordinate{target_pos.latitude_deg, target_pos.longitude_deg}); Telemetry::GroundTruth current_pos = _telemetry->ground_truth(); - REQUIRE(std::isfinite(current_pos.latitude_deg)); - REQUIRE(std::isfinite(current_pos.longitude_deg)); + CHECK(std::isfinite(current_pos.latitude_deg)); + CHECK(std::isfinite(current_pos.longitude_deg)); LocalCoordinate local_pos= ct.local_from_global(GlobalCoordinate{current_pos.latitude_deg, current_pos.longitude_deg}); return sq(local_pos.north_m) + sq(local_pos.east_m) < sq(acceptance_radius_m); From 3d3fd8b6185b554c9f08e2f1deb59a07e96d999b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Mar 2020 15:06:31 +0100 Subject: [PATCH 17/51] mavsdk_tests: try using tags as intended The tags should enable use to assemble the tests in various ways. This will probably require some iterations though. --- test/mavsdk_tests/configs/sitl.json | 8 ++++---- test/mavsdk_tests/test_multicopter_offboard.cpp | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/test/mavsdk_tests/configs/sitl.json b/test/mavsdk_tests/configs/sitl.json index 3406c57d98ca..d9ee544dfdd9 100644 --- a/test/mavsdk_tests/configs/sitl.json +++ b/test/mavsdk_tests/configs/sitl.json @@ -6,23 +6,23 @@ [ { "model": "iris", - "test_filter": "[multicopter]", + "test_filter": "[multicopter],[offboard]", "timeout_min": 20 }, { "model": "iris_opt_flow_mockup", - "test_filter": "[multicopter_offboard]", + "test_filter": "[multicopter][offboard][nogps]", "timeout_min": 20 }, { "model": "iris_vision", - "test_filter": "[multicopter_offboard]", + "test_filter": "[multicopter][offboard][nogps]", "timeout_min": 20, "max_speed_factor": 1 }, { "model": "standard_vtol", - "test_filter": "[vtol]", + "test_filter": "[multicopter][vtol]", "timeout_min": 20 } ] diff --git a/test/mavsdk_tests/test_multicopter_offboard.cpp b/test/mavsdk_tests/test_multicopter_offboard.cpp index 18430b27278b..a366f8a9b69e 100644 --- a/test/mavsdk_tests/test_multicopter_offboard.cpp +++ b/test/mavsdk_tests/test_multicopter_offboard.cpp @@ -11,7 +11,7 @@ #include "autopilot_tester.h" -TEST_CASE("Takeoff and Land (Multicopter offboard)", "[multicopter_offboard]") +TEST_CASE("Takeoff and Land (Multicopter offboard)", "[multicopter][offboard][nogps]") { AutopilotTester tester; Offboard::PositionNEDYaw takeoff_position {0.0f, 0.0f, -2.0f, 0.0f}; @@ -25,7 +25,7 @@ TEST_CASE("Takeoff and Land (Multicopter offboard)", "[multicopter_offboard]") tester.check_home_within(0.5f); } -TEST_CASE("Mission (Multicopter offboard )", "[multicopter_offboard]") +TEST_CASE("Mission (Multicopter offboard )", "[multicopter][offboard][nogps]") { AutopilotTester tester; Offboard::PositionNEDYaw takeoff_position {0.0f, 0.0f, -2.0f, 0.0f}; From 19f0cfd0d07a7294a77ff57a3d6f7ec37f963921 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Mar 2020 15:36:38 +0100 Subject: [PATCH 18/51] mavsdk_tests: write output to logs folder And create folder if it doesn't exist already. --- test/mavsdk_tests/mavsdk_test_runner.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 4ace64f92e4e..e989568b7aae 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -166,7 +166,7 @@ def main(): parser = argparse.ArgumentParser() parser.add_argument("--log-dir", - help="Directory for log files, stdout if not provided") + help="Directory for log files", default="logs") parser.add_argument("--speed-factor", default=1, help="How fast to run the simulation") parser.add_argument("--iterations", type=int, default=1, @@ -193,6 +193,8 @@ def main(): if not is_everything_ready(config): sys.exit(1) + os.makedirs(args.log_dir, exist_ok=True) + run(args, config) From 39284ad0b3f39ff5838a926843a7b494c4651b82 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Mar 2020 15:55:14 +0100 Subject: [PATCH 19/51] Tools: exit as straightaway with DONT_RUN set --- Tools/sitl_run.sh | 58 +++++++++++++++++++++++------------------------ 1 file changed, 28 insertions(+), 30 deletions(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 6214a96a1968..a7053382a6a2 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -2,6 +2,16 @@ set -e +if [ "$#" -lt 6 ]; then + echo usage: sitl_run.sh sitl_bin debugger program model src_path build_path + exit 1 +fi + +if [[ -n "$DONT_RUN" ]]; then + echo "Not running simulation (DONT_RUN is set)." + exit 0 +fi + sitl_bin="$1" debugger="$2" program="$3" @@ -41,12 +51,6 @@ if [ "$model" == "" ] || [ "$model" == "none" ]; then model="iris" fi -if [ "$#" -lt 6 ]; then - echo usage: sitl_run.sh sitl_bin debugger program model src_path build_path - echo "" - exit 1 -fi - # kill process names that might stil # be running from last time pkill -x gazebo || true @@ -73,22 +77,20 @@ if [ "$program" == "jmavsim" ] && [ ! -n "$no_sim" ]; then SIM_PID=`echo $!` elif [ "$program" == "gazebo" ] && [ ! -n "$no_sim" ]; then if [ -x "$(command -v gazebo)" ]; then - if [[ -z "$DONT_RUN" ]]; then - # Set the plugin path so Gazebo finds our model and sim - source "$src_path/Tools/setup_gazebo.bash" "${src_path}" "${build_path}" + # Set the plugin path so Gazebo finds our model and sim + source "$src_path/Tools/setup_gazebo.bash" "${src_path}" "${build_path}" gzserver "${src_path}/Tools/sitl_gazebo/worlds/${model}.world" & SIM_PID=`echo $!` - if [[ -n "$HEADLESS" ]]; then - echo "not running gazebo gui" - else - # gzserver needs to be running to avoid a race. Since the launch - # is putting it into the background we need to avoid it by backing off - sleep 3 - nice -n 20 gzclient --verbose & - GUI_PID=`echo $!` - fi + if [[ -n "$HEADLESS" ]]; then + echo "not running gazebo gui" + else + # gzserver needs to be running to avoid a race. Since the launch + # is putting it into the background we need to avoid it by backing off + sleep 3 + nice -n 20 gzclient --verbose & + GUI_PID=`echo $!` fi else echo "You need to have gazebo simulator installed!" @@ -112,9 +114,7 @@ echo SITL COMMAND: $sitl_command export PX4_SIM_MODEL=${model} -if [[ -n "$DONT_RUN" ]]; then - echo "Not running simulation (\$DONT_RUN is set)." -elif [ "$debugger" == "lldb" ]; then +if [ "$debugger" == "lldb" ]; then eval lldb -- $sitl_command elif [ "$debugger" == "gdb" ]; then eval gdb --args $sitl_command @@ -138,14 +138,12 @@ fi popd >/dev/null -if [[ -z "$DONT_RUN" ]]; then - if [ "$program" == "jmavsim" ]; then - pkill -9 -P $SIM_PID - kill -9 $SIM_PID - elif [ "$program" == "gazebo" ]; then - kill -9 $SIM_PID - if [[ ! -n "$HEADLESS" ]]; then - kill -9 $GUI_PID - fi +if [ "$program" == "jmavsim" ]; then + pkill -9 -P $SIM_PID + kill -9 $SIM_PID +elif [ "$program" == "gazebo" ]; then + kill -9 $SIM_PID + if [[ ! -n "$HEADLESS" ]]; then + kill -9 $GUI_PID fi fi From 48f62cae80d23dada217cd288760cc7a615cee97 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Mar 2020 18:48:14 +0100 Subject: [PATCH 20/51] mavsdk_tests: output less verbose unless needed --- test/mavsdk_tests/mavsdk_test_runner.py | 76 ++++++++++++++----------- 1 file changed, 43 insertions(+), 33 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index e989568b7aae..dac0e656e564 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -13,27 +13,31 @@ class Runner: - def __init__(self, log_dir): + def __init__(self, log_dir, verbose): self.cmd = "" self.cwd = None self.args = [] self.env = {} self.log_prefix = "" self.log_dir = log_dir + self.verbose = verbose def start(self, config): + if self.verbose: + print("Running: {}".format(" ".join([self.cmd] + self.args))) + + atexit.register(self.stop) + if self.log_dir: - f = open(self.log_dir + os.path.sep + - "log-{}-{}-{}-{}.log".format( - self.log_prefix, - config['model'], - config['test_filter'], - datetime.datetime.now().strftime("%Y-%m-%dT%H-%M-%SZ") - ), 'w') + f = open( + self.log_dir + os.path.sep + "log-{}-{}-{}-{}.log".format( + self.log_prefix, config['model'], config['test_filter'], + datetime.datetime.now().strftime("%Y-%m-%dT%H-%M-%SZ")), + 'w') else: f = None - print("Running: {}".format(" ".join([self.cmd] + self.args))) + # TODO: pipe stdout through when set to verbose self.process = subprocess.Popen( [self.cmd] + self.args, @@ -43,8 +47,6 @@ def start(self, config): stderr=f ) - atexit.register(self.stop) - def wait(self, timeout_min): try: return self.process.wait(timeout=timeout_min*60) @@ -62,14 +64,16 @@ def stop(self): if returncode is not None: return returncode - print("Sending SIGINT to {}".format(self.process.pid)) + if self.verbose: + print("Sending SIGINT to {}".format(self.process.pid)) self.process.send_signal(signal.SIGINT) try: return self.process.wait(timeout=1) except subprocess.TimeoutExpired: pass - print("Terminating {}".format(self.process.pid)) + if self.verbose: + print("Terminating {}".format(self.process.pid)) self.process.terminate() try: @@ -77,14 +81,20 @@ def stop(self): except subprocess.TimeoutExpired: pass - print("Killing {}".format(self.process.pid)) + if self.verbose: + print("Killing {}".format(self.process.pid)) self.process.kill() + + if self.verbose: + print("{} exited with {}".format( + self.command, self.process.returncode)) return self.process.returncode class Px4Runner(Runner): - def __init__(self, model, workspace_dir, log_dir, speed_factor, debugger): - super().__init__(log_dir) + def __init__(self, model, workspace_dir, log_dir, speed_factor, debugger, + verbose): + super().__init__(log_dir, verbose) self.cmd = workspace_dir + "/build/px4_sitl_default/bin/px4" self.cwd = workspace_dir + "/build/px4_sitl_default/tmp/rootfs" self.args = [ @@ -119,8 +129,8 @@ def __init__(self, model, workspace_dir, log_dir, speed_factor, debugger): class GzserverRunner(Runner): - def __init__(self, model, workspace_dir, log_dir, speed_factor): - super().__init__(log_dir) + def __init__(self, model, workspace_dir, log_dir, speed_factor, verbose): + super().__init__(log_dir, verbose) self.env = {"PATH": os.environ['PATH'], "HOME": os.environ['HOME'], "GAZEBO_PLUGIN_PATH": @@ -137,8 +147,8 @@ def __init__(self, model, workspace_dir, log_dir, speed_factor): class GzclientRunner(Runner): - def __init__(self, workspace_dir, log_dir): - super().__init__(log_dir) + def __init__(self, workspace_dir, log_dir, verbose): + super().__init__(log_dir, verbose) self.env = {"PATH": os.environ['PATH'], "HOME": os.environ['HOME'], # "GAZEBO_PLUGIN_PATH": @@ -153,8 +163,8 @@ def __init__(self, workspace_dir, log_dir): class TestRunner(Runner): def __init__(self, workspace_dir, log_dir, config, test, - mavlink_connection): - super().__init__(log_dir) + mavlink_connection, verbose): + super().__init__(log_dir, verbose) self.env = {"PATH": os.environ['PATH']} self.cmd = workspace_dir + \ "/build/px4_sitl_default/mavsdk_tests/mavsdk_tests" @@ -179,6 +189,8 @@ def main(): help="Specify which model to run") parser.add_argument("--debugger", default="", help="valgrind callgrind gdb lldb") + parser.add_argument("--verbose", default=False, action='store_true', + help="Enable more verbose output") parser.add_argument("config_file", help="JSON config file to use") args = parser.parse_args() @@ -193,6 +205,8 @@ def main(): if not is_everything_ready(config): sys.exit(1) + if args.verbose: + print("Creating directory: {}".format(args.log_dir)) os.makedirs(args.log_dir, exist_ok=True) run(args, config) @@ -319,21 +333,22 @@ def run_test(test, group, args, config): if config['mode'] == 'sitl': px4_runner = Px4Runner( group['model'], os.getcwd(), args.log_dir, speed_factor, - args.debugger) + args.debugger, args.verbose) px4_runner.start(group) if config['simulator'] == 'gazebo': gzserver_runner = GzserverRunner( - group['model'], os.getcwd(), args.log_dir, speed_factor) + group['model'], os.getcwd(), args.log_dir, speed_factor, + args.verbose) gzserver_runner.start(group) if args.gui: gzclient_runner = GzclientRunner( - os.getcwd(), args.log_dir) + os.getcwd(), args.log_dir, args.verbose) gzclient_runner.start(group) test_runner = TestRunner(os.getcwd(), args.log_dir, group, test, - config['mavlink_connection']) + config['mavlink_connection'], args.verbose) test_runner.start(group) returncode = test_runner.wait(group['timeout_min']) @@ -342,14 +357,9 @@ def run_test(test, group, args, config): if config['mode'] == 'sitl': if config['simulator'] == 'gazebo': if args.gui: - returncode = gzclient_runner.stop() - print("gzclient exited with {}".format(returncode)) - - returncode = gzserver_runner.stop() - print("gzserver exited with {}".format(returncode)) - + gzclient_runner.stop() + gzserver_runner.stop() px4_runner.stop() - print("px4 exited with {}".format(returncode)) return is_success From 92da35d4431afe16c9f6a21a130658c46324d7b1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 10 Mar 2020 15:01:04 +0100 Subject: [PATCH 21/51] mavsdk_tests: only print error on failure --- test/mavsdk_tests/mavsdk_test_runner.py | 42 ++++++++++++++++++------- 1 file changed, 31 insertions(+), 11 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index dac0e656e564..0f333026ca0d 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -20,31 +20,37 @@ def __init__(self, log_dir, verbose): self.env = {} self.log_prefix = "" self.log_dir = log_dir + self.log_filename = "" + self.log_fd = None self.verbose = verbose + def create_log_filename(self, model, test_filter): + return self.log_dir + os.path.sep + \ + "log-{}-{}-{}-{}.log".format( + self.log_prefix, + model, + test_filter, + datetime.datetime.now().strftime("%Y-%m-%dT%H-%M-%SZ")) + def start(self, config): if self.verbose: print("Running: {}".format(" ".join([self.cmd] + self.args))) atexit.register(self.stop) - if self.log_dir: - f = open( - self.log_dir + os.path.sep + "log-{}-{}-{}-{}.log".format( - self.log_prefix, config['model'], config['test_filter'], - datetime.datetime.now().strftime("%Y-%m-%dT%H-%M-%SZ")), - 'w') + if self.verbose: + self.log_fd = None else: - f = None - - # TODO: pipe stdout through when set to verbose + self.log_filename = self.create_log_filename( + config['model'], config['test_filter']) + self.log_fd = open(self.log_filename, 'w') self.process = subprocess.Popen( [self.cmd] + self.args, cwd=self.cwd, env=self.env, - stdout=f, - stderr=f + stdout=self.log_fd, + stderr=self.log_fd ) def wait(self, timeout_min): @@ -60,14 +66,18 @@ def wait(self, timeout_min): def stop(self): atexit.unregister(self.stop) + self.log_fd.flush() + returncode = self.process.poll() if returncode is not None: + self.log_fd.close() return returncode if self.verbose: print("Sending SIGINT to {}".format(self.process.pid)) self.process.send_signal(signal.SIGINT) try: + self.log_fd.close() return self.process.wait(timeout=1) except subprocess.TimeoutExpired: pass @@ -77,6 +87,7 @@ def stop(self): self.process.terminate() try: + self.log_fd.close() return self.process.wait(timeout=1) except subprocess.TimeoutExpired: pass @@ -88,8 +99,14 @@ def stop(self): if self.verbose: print("{} exited with {}".format( self.command, self.process.returncode)) + + self.log_fd.close() return self.process.returncode + def print_output(self): + with open(self.log_filename, 'r') as f: + print(f.read(), end="") + class Px4Runner(Runner): def __init__(self, model, workspace_dir, log_dir, speed_factor, debugger, @@ -361,6 +378,9 @@ def run_test(test, group, args, config): gzserver_runner.stop() px4_runner.stop() + if not is_success and not args.verbose: + test_runner.print_output() + return is_success From d83218e0e3602d6c6aae0977a344bdb2647dbf03 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 10 Mar 2020 17:06:10 +0100 Subject: [PATCH 22/51] mavsdk_tests: improve test output readability --- test/mavsdk_tests/mavsdk_test_runner.py | 81 ++++++++++++++++++++----- 1 file changed, 65 insertions(+), 16 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 0f333026ca0d..9214b594668a 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -12,6 +12,47 @@ import signal +def supports_color(): + """ + Returns True if the running system's terminal supports color, and False + otherwise. + + From https://stackoverflow.com/a/22254892/8548472 + """ + plat = sys.platform + supported_platform = plat != 'Pocket PC' and (plat != 'win32' or + 'ANSICON' in os.environ) + # isatty is not always implemented, #6223. + is_a_tty = hasattr(sys.stdout, 'isatty') and sys.stdout.isatty() + return supported_platform and is_a_tty + + +if supports_color(): + class color: + PURPLE = '\033[95m' + CYAN = '\033[96m' + DARKCYAN = '\033[36m' + BLUE = '\033[94m' + GREEN = '\033[92m' + YELLOW = '\033[93m' + RED = '\033[91m' + BOLD = '\033[1m' + UNDERLINE = '\033[4m' + END = '\033[0m' +else: + class color: + PURPLE = '' + CYAN = '' + DARKCYAN = '' + BLUE = '' + GREEN = '' + YELLOW = '' + RED = '' + BOLD = '' + UNDERLINE = '' + END = '' + + class Runner: def __init__(self, log_dir, verbose): self.cmd = "" @@ -226,11 +267,11 @@ def main(): print("Creating directory: {}".format(args.log_dir)) os.makedirs(args.log_dir, exist_ok=True) - run(args, config) + sys.exit(run(args, config)) -def determine_tests(workspace_dir, filter): - cmd = workspace_dir + "/build/px4_sitl_default/mavsdk_tests/mavsdk_tests" +def determine_tests(filter): + cmd = os.getcwd() + "/build/px4_sitl_default/mavsdk_tests/mavsdk_tests" args = ["--list-test-names-only", filter] p = subprocess.Popen( [cmd] + args, @@ -297,21 +338,24 @@ def run(args, config): format(iteration + 1, args.iterations)) break - print("Overall result: {}". - format("SUCCESS" if overall_success else "FAIL")) - sys.exit(0 if overall_success else 1) + if overall_success: + print(color.GREEN + "Overall result: success!" + color.END) + return 0 + else: + print(color.RED + "Overall result: failure!" + color.END) + return 1 def run_test_group(args, config): overall_success = True - test_matrix = config["tests"] + tests = config["tests"] if args.model == 'all': - models = test_matrix + models = tests else: found = False - for elem in test_matrix: + for elem in tests: if elem['model'] == args.model: models = [elem] found = True @@ -320,16 +364,21 @@ def run_test_group(args, config): models = [] for group in models: - print("Running test group for '{}' with filter '{}'" - .format(group['model'], group['test_filter'])) + print(color.BOLD + "==> Running tests for '{}' with filter '{}'" + .format(group['model'], group['test_filter']) + color.END) - tests = determine_tests(os.getcwd(), group['test_filter']) + tests = determine_tests(group['test_filter']) - for test in tests: - print("Running test '{}'".format(test)) + for i, test in enumerate(tests): + print("--> Test {} of {}: '{}' running ...". + format(i+1, len(tests), test)) was_success = run_test(test, group, args, config) - print("Test '{}': {}". - format(test, "Success" if was_success else "Fail")) + if was_success: + print(color.GREEN + "--- Test {} of {}: '{}' succeeded.". + format(i+1, len(tests), test) + color.END) + else: + print(color.RED + "--- Test {} of {}: '{}' failed.". + format(i+1, len(tests), test) + color.END) if not was_success: overall_success = False From b70575b46d59c839d5d065f9fefd60047a58edde Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 10 Mar 2020 17:06:37 +0100 Subject: [PATCH 23/51] mavsdk_tests: improve test name/description --- test/mavsdk_tests/test_mission_multicopter.cpp | 2 +- test/mavsdk_tests/test_multicopter_offboard.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/test/mavsdk_tests/test_mission_multicopter.cpp b/test/mavsdk_tests/test_mission_multicopter.cpp index 82b1a57a6fe3..435d24fc10b2 100644 --- a/test/mavsdk_tests/test_mission_multicopter.cpp +++ b/test/mavsdk_tests/test_mission_multicopter.cpp @@ -11,7 +11,7 @@ #include "autopilot_tester.h" -TEST_CASE("Takeoff and Land (Multicopter)", "[multicopter][vtol]") +TEST_CASE("Takeoff and Land", "[multicopter][vtol]") { AutopilotTester tester; tester.connect(connection_url); diff --git a/test/mavsdk_tests/test_multicopter_offboard.cpp b/test/mavsdk_tests/test_multicopter_offboard.cpp index a366f8a9b69e..678e9a61eb74 100644 --- a/test/mavsdk_tests/test_multicopter_offboard.cpp +++ b/test/mavsdk_tests/test_multicopter_offboard.cpp @@ -11,7 +11,7 @@ #include "autopilot_tester.h" -TEST_CASE("Takeoff and Land (Multicopter offboard)", "[multicopter][offboard][nogps]") +TEST_CASE("Offboard takeoff and land", "[multicopter][offboard][nogps]") { AutopilotTester tester; Offboard::PositionNEDYaw takeoff_position {0.0f, 0.0f, -2.0f, 0.0f}; @@ -25,7 +25,7 @@ TEST_CASE("Takeoff and Land (Multicopter offboard)", "[multicopter][offboard][no tester.check_home_within(0.5f); } -TEST_CASE("Mission (Multicopter offboard )", "[multicopter][offboard][nogps]") +TEST_CASE("Offboard position control", "[multicopter][offboard][nogps]") { AutopilotTester tester; Offboard::PositionNEDYaw takeoff_position {0.0f, 0.0f, -2.0f, 0.0f}; From 2cf483e4e3016f18849f492ca388235e87f6c07c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 11 Mar 2020 13:33:46 +0100 Subject: [PATCH 24/51] mavsdk_tests: improve output, allow log and stdout This makes it possible to write to logfiles and at the same time print everything to console in verbose mode. --- test/mavsdk_tests/mavsdk_test_runner.py | 157 ++++++++++++++++-------- 1 file changed, 106 insertions(+), 51 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 9214b594668a..6561524ea86b 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -7,15 +7,16 @@ import json import os import psutil +import re import subprocess import sys -import signal +import threading +import time +import queue def supports_color(): - """ - Returns True if the running system's terminal supports color, and False - otherwise. + """Returns True if the running system's terminal supports color. From https://stackoverflow.com/a/22254892/8548472 """ @@ -53,22 +54,32 @@ class color: END = '' +def remove_color(text): + """Remove ANSI and xterm256 color codes. + + From https://stackoverflow.com/a/30500866/8548472 + """ + return re.sub(r'\x1b(\[.*?[@-~]|\].*?(\x07|\x1b\\))', '', text) + + class Runner: def __init__(self, log_dir, verbose): + self.name = "" self.cmd = "" self.cwd = None self.args = [] self.env = {} - self.log_prefix = "" self.log_dir = log_dir self.log_filename = "" self.log_fd = None self.verbose = verbose + self.output_queue = queue.Queue() + self.start_time = time.time() def create_log_filename(self, model, test_filter): return self.log_dir + os.path.sep + \ "log-{}-{}-{}-{}.log".format( - self.log_prefix, + self.name, model, test_filter, datetime.datetime.now().strftime("%Y-%m-%dT%H-%M-%SZ")) @@ -79,21 +90,35 @@ def start(self, config): atexit.register(self.stop) - if self.verbose: - self.log_fd = None - else: - self.log_filename = self.create_log_filename( - config['model'], config['test_filter']) - self.log_fd = open(self.log_filename, 'w') + self.log_filename = self.create_log_filename( + config['model'], config['test_filter']) + self.log_fd = open(self.log_filename, 'w') self.process = subprocess.Popen( [self.cmd] + self.args, cwd=self.cwd, env=self.env, - stdout=self.log_fd, - stderr=self.log_fd + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + universal_newlines=True ) + self.stop_thread = threading.Event() + self.thread = threading.Thread(target=self.process_output) + self.thread.start() + + def process_output(self): + while not self.stop_thread.is_set(): + line = self.process.stdout.readline() + if line == "\n": + continue + self.output_queue.put(line) + self.log_fd.write(line) + self.log_fd.flush() + + def poll(self): + return self.process.poll() + def wait(self, timeout_min): try: return self.process.wait(timeout=timeout_min*60) @@ -104,55 +129,67 @@ def wait(self, timeout_min): print("stopped.") return errno.ETIMEDOUT + def get_output(self): + try: + output = self.output_queue.get(block=True, timeout=0.1) + if supports_color(): + return output + else: + return remove_color(output) + except queue.Empty: + return None + + def print_output(self): + output = self.get_output() + if not output: + return + print(color.END + + "[" + self.name.ljust(11) + "] " + + output + + color.END, end="") + def stop(self): atexit.unregister(self.stop) - self.log_fd.flush() + self.stop_thread.set() returncode = self.process.poll() - if returncode is not None: - self.log_fd.close() - return returncode - - if self.verbose: - print("Sending SIGINT to {}".format(self.process.pid)) - self.process.send_signal(signal.SIGINT) - try: - self.log_fd.close() - return self.process.wait(timeout=1) - except subprocess.TimeoutExpired: - pass + if returncode is None: - if self.verbose: - print("Terminating {}".format(self.process.pid)) - self.process.terminate() + if self.verbose: + print("Terminating {}".format(self.cmd)) + self.process.terminate() - try: - self.log_fd.close() - return self.process.wait(timeout=1) - except subprocess.TimeoutExpired: - pass + try: + returncode = self.process.wait(timeout=1) + except subprocess.TimeoutExpired: + pass - if self.verbose: - print("Killing {}".format(self.process.pid)) - self.process.kill() + if returncode is None: + if self.verbose: + print("Killing {}".format(self.cmd)) + self.process.kill() + returncode = self.process.poll() if self.verbose: print("{} exited with {}".format( - self.command, self.process.returncode)) + self.cmd, self.process.returncode)) + + self.thread.join() self.log_fd.close() + return self.process.returncode - def print_output(self): - with open(self.log_filename, 'r') as f: - print(f.read(), end="") + def time_elapsed_s(self): + return time.time() - self.start_time class Px4Runner(Runner): def __init__(self, model, workspace_dir, log_dir, speed_factor, debugger, verbose): super().__init__(log_dir, verbose) + self.name = "px4" self.cmd = workspace_dir + "/build/px4_sitl_default/bin/px4" self.cwd = workspace_dir + "/build/px4_sitl_default/tmp/rootfs" self.args = [ @@ -166,7 +203,7 @@ def __init__(self, model, workspace_dir, log_dir, speed_factor, debugger, self.env = {"PATH": os.environ['PATH'], "PX4_SIM_MODEL": model, "PX4_SIM_SPEED_FACTOR": str(speed_factor)} - self.log_prefix = "px4" + self.name = "px4" if not debugger: pass @@ -189,6 +226,7 @@ def __init__(self, model, workspace_dir, log_dir, speed_factor, debugger, class GzserverRunner(Runner): def __init__(self, model, workspace_dir, log_dir, speed_factor, verbose): super().__init__(log_dir, verbose) + self.name = "gzserver" self.env = {"PATH": os.environ['PATH'], "HOME": os.environ['HOME'], "GAZEBO_PLUGIN_PATH": @@ -201,12 +239,12 @@ def __init__(self, model, workspace_dir, log_dir, speed_factor, verbose): self.args = ["--verbose", workspace_dir + "/Tools/sitl_gazebo/worlds/" + model + ".world"] - self.log_prefix = "gzserver" class GzclientRunner(Runner): def __init__(self, workspace_dir, log_dir, verbose): super().__init__(log_dir, verbose) + self.name = "gzclient" self.env = {"PATH": os.environ['PATH'], "HOME": os.environ['HOME'], # "GAZEBO_PLUGIN_PATH": @@ -216,18 +254,17 @@ def __init__(self, workspace_dir, log_dir, verbose): "DISPLAY": os.environ['DISPLAY']} self.cmd = "gzclient" self.args = ["--verbose"] - self.log_prefix = "gzclient" class TestRunner(Runner): def __init__(self, workspace_dir, log_dir, config, test, mavlink_connection, verbose): super().__init__(log_dir, verbose) + self.name = "test_runner" self.env = {"PATH": os.environ['PATH']} self.cmd = workspace_dir + \ "/build/px4_sitl_default/mavsdk_tests/mavsdk_tests" self.args = ["--url", mavlink_connection, test] - self.log_prefix = "test_runner" def main(): @@ -417,8 +454,29 @@ def run_test(test, group, args, config): config['mavlink_connection'], args.verbose) test_runner.start(group) - returncode = test_runner.wait(group['timeout_min']) - is_success = (returncode == 0) + while test_runner.time_elapsed_s() < group['timeout_min']*60: + returncode = test_runner.poll() + if returncode is not None: + is_success = (returncode == 0) + break + + if args.verbose: + test_runner.print_output() + + if config['mode'] == 'sitl': + px4_runner.print_output() + + if config['simulator'] == 'gazebo': + gzserver_runner.print_output() + + if args.gui: + gzclient_runner.print_output() + else: + print(color.BOLD + "Test timeout of {} mins triggered!". + format(group['timeout_min']) + color.END) + is_success = False + + test_runner.stop() if config['mode'] == 'sitl': if config['simulator'] == 'gazebo': @@ -427,9 +485,6 @@ def run_test(test, group, args, config): gzserver_runner.stop() px4_runner.stop() - if not is_success and not args.verbose: - test_runner.print_output() - return is_success From c55c394227c9ed54f6c3064def4fcf7a08251a6e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 11 Mar 2020 14:00:51 +0100 Subject: [PATCH 25/51] mavsdk_tests: properly exit on Ctrl+C With threads we need to manually take care of it, otherwise it gets messy and we need to press Ctrl+C multiple times. --- test/mavsdk_tests/mavsdk_test_runner.py | 239 ++++++++++++------------ 1 file changed, 123 insertions(+), 116 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 6561524ea86b..e6d024f4ad3e 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -8,6 +8,7 @@ import os import psutil import re +import signal import subprocess import sys import threading @@ -304,7 +305,10 @@ def main(): print("Creating directory: {}".format(args.log_dir)) os.makedirs(args.log_dir, exist_ok=True) - sys.exit(run(args, config)) + tester = Tester() + signal.signal(signal.SIGINT, tester.sigint_handler) + + sys.exit(tester.run(args, config)) def determine_tests(filter): @@ -358,134 +362,137 @@ def is_everything_ready(config): return result -def run(args, config): - overall_success = True - - for iteration in range(args.iterations): - if args.iterations > 1: - print("Test iteration: {}".format(iteration + 1, args.iterations)) - - was_success = run_test_group(args, config) - - if not was_success: - overall_success = False - - if args.iterations > 1 and not was_success and args.abort_early: - print("Aborting with a failure in test run {}/{}". - format(iteration + 1, args.iterations)) - break - - if overall_success: - print(color.GREEN + "Overall result: success!" + color.END) - return 0 - else: - print(color.RED + "Overall result: failure!" + color.END) - return 1 - - -def run_test_group(args, config): - overall_success = True +class Tester: + def __init__(self): + self.active_runners = [] - tests = config["tests"] + def run(self, args, config): + overall_success = True - if args.model == 'all': - models = tests - else: - found = False - for elem in tests: - if elem['model'] == args.model: - models = [elem] - found = True - if not found: - print("Specified model is not defined") - models = [] + for iteration in range(args.iterations): + if args.iterations > 1: + print("Test iteration: {}". + format(iteration + 1, args.iterations)) - for group in models: - print(color.BOLD + "==> Running tests for '{}' with filter '{}'" - .format(group['model'], group['test_filter']) + color.END) - - tests = determine_tests(group['test_filter']) - - for i, test in enumerate(tests): - print("--> Test {} of {}: '{}' running ...". - format(i+1, len(tests), test)) - was_success = run_test(test, group, args, config) - if was_success: - print(color.GREEN + "--- Test {} of {}: '{}' succeeded.". - format(i+1, len(tests), test) + color.END) - else: - print(color.RED + "--- Test {} of {}: '{}' failed.". - format(i+1, len(tests), test) + color.END) + was_success = self.run_test_group(args, config) if not was_success: overall_success = False - if not was_success and args.abort_early: - print("Aborting early") - return False - - return overall_success + if args.iterations > 1 and not was_success and args.abort_early: + print("Aborting with a failure in test run {}/{}". + format(iteration + 1, args.iterations)) + break + if overall_success: + print(color.GREEN + "Overall result: success!" + color.END) + return 0 + else: + print(color.RED + "Overall result: failure!" + color.END) + return 1 -def run_test(test, group, args, config): - - speed_factor = args.speed_factor - if "max_speed_factor" in group: - speed_factor = min(int(speed_factor), group["max_speed_factor"]) + def run_test_group(self, args, config): + overall_success = True - if config['mode'] == 'sitl': - px4_runner = Px4Runner( - group['model'], os.getcwd(), args.log_dir, speed_factor, - args.debugger, args.verbose) - px4_runner.start(group) + tests = config["tests"] - if config['simulator'] == 'gazebo': - gzserver_runner = GzserverRunner( + if args.model == 'all': + models = tests + else: + found = False + for elem in tests: + if elem['model'] == args.model: + models = [elem] + found = True + if not found: + print("Specified model is not defined") + models = [] + + for group in models: + print(color.BOLD + "==> Running tests for '{}' with filter '{}'" + .format(group['model'], group['test_filter']) + color.END) + + tests = determine_tests(group['test_filter']) + + for i, test in enumerate(tests): + print("--> Test {} of {}: '{}' running ...". + format(i+1, len(tests), test)) + was_success = self.run_test(test, group, args, config) + if was_success: + print(color.GREEN + "--- Test {} of {}: '{}' succeeded.". + format(i+1, len(tests), test) + color.END) + else: + print(color.RED + "--- Test {} of {}: '{}' failed.". + format(i+1, len(tests), test) + color.END) + + if not was_success: + overall_success = False + + if not was_success and args.abort_early: + print("Aborting early") + return False + + return overall_success + + def run_test(self, test, group, args, config): + self.active_runners = [] + + speed_factor = args.speed_factor + if "max_speed_factor" in group: + speed_factor = min(int(speed_factor), group["max_speed_factor"]) + + if config['mode'] == 'sitl': + px4_runner = Px4Runner( group['model'], os.getcwd(), args.log_dir, speed_factor, - args.verbose) - gzserver_runner.start(group) - - if args.gui: - gzclient_runner = GzclientRunner( - os.getcwd(), args.log_dir, args.verbose) - gzclient_runner.start(group) - - test_runner = TestRunner(os.getcwd(), args.log_dir, group, test, - config['mavlink_connection'], args.verbose) - test_runner.start(group) - - while test_runner.time_elapsed_s() < group['timeout_min']*60: - returncode = test_runner.poll() - if returncode is not None: - is_success = (returncode == 0) - break - - if args.verbose: - test_runner.print_output() - - if config['mode'] == 'sitl': - px4_runner.print_output() + args.debugger, args.verbose) + px4_runner.start(group) + self.active_runners.append(px4_runner) + + if config['simulator'] == 'gazebo': + gzserver_runner = GzserverRunner( + group['model'], os.getcwd(), args.log_dir, speed_factor, + args.verbose) + gzserver_runner.start(group) + self.active_runners.append(gzserver_runner) + + if args.gui: + gzclient_runner = GzclientRunner( + os.getcwd(), args.log_dir, args.verbose) + gzclient_runner.start(group) + self.active_runners.append(gzclient_runner) + + test_runner = TestRunner(os.getcwd(), args.log_dir, group, test, + config['mavlink_connection'], args.verbose) + test_runner.start(group) + self.active_runners.append(test_runner) + + while test_runner.time_elapsed_s() < group['timeout_min']*60: + returncode = test_runner.poll() + if returncode is not None: + is_success = (returncode == 0) + break + + if args.verbose: + for runner in self.active_runners: + runner.print_output() - if config['simulator'] == 'gazebo': - gzserver_runner.print_output() - - if args.gui: - gzclient_runner.print_output() - else: - print(color.BOLD + "Test timeout of {} mins triggered!". - format(group['timeout_min']) + color.END) - is_success = False - - test_runner.stop() - - if config['mode'] == 'sitl': - if config['simulator'] == 'gazebo': - if args.gui: - gzclient_runner.stop() - gzserver_runner.stop() - px4_runner.stop() - - return is_success + else: + print(color.BOLD + "Test timeout of {} mins triggered!". + format(group['timeout_min']) + color.END) + is_success = False + + for runner in self.active_runners: + runner.stop() + + return is_success + + def sigint_handler(self, sig, frame): + print("Received SIGINT") + print("Stopping all processes ...") + for runner in self.active_runners: + runner.stop() + print("Stopping all processes done.") + sys.exit(-sig) if __name__ == '__main__': From 184734c2009778b09d1d90e315f3ab0ed345f936 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 11 Mar 2020 18:01:02 +0100 Subject: [PATCH 26/51] mavsdk_tests: initialize Tester class in ctor --- test/mavsdk_tests/mavsdk_test_runner.py | 108 ++++++++++++++++-------- 1 file changed, 73 insertions(+), 35 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index e6d024f4ad3e..a4025a6a03a8 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -205,23 +205,24 @@ def __init__(self, model, workspace_dir, log_dir, speed_factor, debugger, "PX4_SIM_MODEL": model, "PX4_SIM_SPEED_FACTOR": str(speed_factor)} self.name = "px4" + self.debugger = debugger - if not debugger: + if not self.debugger: pass - elif debugger == "valgrind": + elif self.debugger == "valgrind": self.args = ["--track-origins=yes", "--leak-check=full", "-v", self.cmd] + self.args self.cmd = "valgrind" - elif debugger == "callgrind": + elif self.debugger == "callgrind": self.args = ["--tool=callgrind", "-v", self.cmd] + self.args self.cmd = "valgrind" - elif debugger == "gdb": + elif self.debugger == "gdb": self.args = ["--args", self.cmd] + self.args self.cmd = "gdb" else: - print("Using custom debugger ", debugger) + print("Using custom debugger " + self.debugger) self.args = [self.cmd] + self.args - self.cmd = debugger + self.cmd = self.debugger class GzserverRunner(Runner): @@ -305,10 +306,20 @@ def main(): print("Creating directory: {}".format(args.log_dir)) os.makedirs(args.log_dir, exist_ok=True) - tester = Tester() + tester = Tester( + config, + args.iterations, + args.abort_early, + args.speed_factor, + args.model, + args.debugger, + args.log_dir, + args.gui, + args.verbose + ) signal.signal(signal.SIGINT, tester.sigint_handler) - sys.exit(tester.run(args, config)) + sys.exit(tester.run()) def determine_tests(filter): @@ -363,25 +374,43 @@ def is_everything_ready(config): class Tester: - def __init__(self): + def __init__(self, + config, + iterations, + abort_early, + speed_factor, + model, + debugger, + log_dir, + gui, + verbose): self.active_runners = [] + self.iterations = iterations + self.abort_early = abort_early + self.config = config + self.speed_factor = speed_factor + self.model = model + self.debugger = debugger + self.log_dir = log_dir + self.gui = gui + self.verbose = verbose - def run(self, args, config): + def run(self): overall_success = True - for iteration in range(args.iterations): - if args.iterations > 1: + for iteration in range(self.iterations): + if self.iterations > 1: print("Test iteration: {}". - format(iteration + 1, args.iterations)) + format(iteration + 1, self.iterations)) - was_success = self.run_test_group(args, config) + was_success = self.run_test_group() if not was_success: overall_success = False - if args.iterations > 1 and not was_success and args.abort_early: + if self.iterations > 1 and not was_success and self.abort_early: print("Aborting with a failure in test run {}/{}". - format(iteration + 1, args.iterations)) + format(iteration + 1, self.iterations)) break if overall_success: @@ -391,17 +420,17 @@ def run(self, args, config): print(color.RED + "Overall result: failure!" + color.END) return 1 - def run_test_group(self, args, config): + def run_test_group(self): overall_success = True - tests = config["tests"] + tests = self.config["tests"] - if args.model == 'all': + if self.model == 'all': models = tests else: found = False for elem in tests: - if elem['model'] == args.model: + if elem['model'] == self.model: models = [elem] found = True if not found: @@ -417,7 +446,7 @@ def run_test_group(self, args, config): for i, test in enumerate(tests): print("--> Test {} of {}: '{}' running ...". format(i+1, len(tests), test)) - was_success = self.run_test(test, group, args, config) + was_success = self.run_test(test, group) if was_success: print(color.GREEN + "--- Test {} of {}: '{}' succeeded.". format(i+1, len(tests), test) + color.END) @@ -428,41 +457,50 @@ def run_test_group(self, args, config): if not was_success: overall_success = False - if not was_success and args.abort_early: + if not was_success and self.abort_early: print("Aborting early") return False return overall_success - def run_test(self, test, group, args, config): + def run_test(self, test, group): self.active_runners = [] - speed_factor = args.speed_factor + speed_factor = self.speed_factor if "max_speed_factor" in group: speed_factor = min(int(speed_factor), group["max_speed_factor"]) - if config['mode'] == 'sitl': + if self.config['mode'] == 'sitl': px4_runner = Px4Runner( - group['model'], os.getcwd(), args.log_dir, speed_factor, - args.debugger, args.verbose) + group['model'], os.getcwd(), self.log_dir, speed_factor, + self.debugger, self.verbose) px4_runner.start(group) self.active_runners.append(px4_runner) - if config['simulator'] == 'gazebo': + if self.config['simulator'] == 'gazebo': gzserver_runner = GzserverRunner( - group['model'], os.getcwd(), args.log_dir, speed_factor, - args.verbose) + group['model'], + os.getcwd(), + self.log_dir, + self.speed_factor, + self.verbose) gzserver_runner.start(group) self.active_runners.append(gzserver_runner) - if args.gui: + if self.gui: gzclient_runner = GzclientRunner( - os.getcwd(), args.log_dir, args.verbose) + os.getcwd(), self.log_dir, self.verbose) gzclient_runner.start(group) self.active_runners.append(gzclient_runner) - test_runner = TestRunner(os.getcwd(), args.log_dir, group, test, - config['mavlink_connection'], args.verbose) + test_runner = TestRunner( + os.getcwd(), + self.log_dir, + group, + test, + self.config['mavlink_connection'], + self.verbose) + test_runner.start(group) self.active_runners.append(test_runner) @@ -472,7 +510,7 @@ def run_test(self, test, group, args, config): is_success = (returncode == 0) break - if args.verbose: + if self.verbose: for runner in self.active_runners: runner.print_output() From 07c30ccf3864ecdfa8dcbc2464769823baa9cd5c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 11 Mar 2020 18:17:04 +0100 Subject: [PATCH 27/51] mavsdk_tests: split up Python script Otherwise, the script will get a tangled mess. --- test/mavsdk_tests/logger_helper.py | 52 +++++ test/mavsdk_tests/mavsdk_test_runner.py | 266 +----------------------- test/mavsdk_tests/process_helper.py | 217 +++++++++++++++++++ 3 files changed, 275 insertions(+), 260 deletions(-) create mode 100644 test/mavsdk_tests/logger_helper.py create mode 100644 test/mavsdk_tests/process_helper.py diff --git a/test/mavsdk_tests/logger_helper.py b/test/mavsdk_tests/logger_helper.py new file mode 100644 index 000000000000..92babc1defb4 --- /dev/null +++ b/test/mavsdk_tests/logger_helper.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 + +import re +import sys +import os + + +def supports_color(): + """Returns True if the running system's terminal supports color. + + From https://stackoverflow.com/a/22254892/8548472 + """ + plat = sys.platform + supported_platform = plat != 'Pocket PC' and (plat != 'win32' or + 'ANSICON' in os.environ) + # isatty is not always implemented, #6223. + is_a_tty = hasattr(sys.stdout, 'isatty') and sys.stdout.isatty() + return supported_platform and is_a_tty + + +if supports_color(): + class color: + PURPLE = '\033[95m' + CYAN = '\033[96m' + DARKCYAN = '\033[36m' + BLUE = '\033[94m' + GREEN = '\033[92m' + YELLOW = '\033[93m' + RED = '\033[91m' + BOLD = '\033[1m' + UNDERLINE = '\033[4m' + END = '\033[0m' +else: + class color: + PURPLE = '' + CYAN = '' + DARKCYAN = '' + BLUE = '' + GREEN = '' + YELLOW = '' + RED = '' + BOLD = '' + UNDERLINE = '' + END = '' + + +def remove_color(text): + """Remove ANSI and xterm256 color codes. + + From https://stackoverflow.com/a/30500866/8548472 + """ + return re.sub(r'\x1b(\[.*?[@-~]|\].*?(\x07|\x1b\\))', '', text) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index a4025a6a03a8..56ae289cdce6 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -1,272 +1,18 @@ #!/usr/bin/env python3 import argparse -import atexit -import datetime -import errno import json import os import psutil -import re import signal import subprocess import sys -import threading -import time -import queue - - -def supports_color(): - """Returns True if the running system's terminal supports color. - - From https://stackoverflow.com/a/22254892/8548472 - """ - plat = sys.platform - supported_platform = plat != 'Pocket PC' and (plat != 'win32' or - 'ANSICON' in os.environ) - # isatty is not always implemented, #6223. - is_a_tty = hasattr(sys.stdout, 'isatty') and sys.stdout.isatty() - return supported_platform and is_a_tty - - -if supports_color(): - class color: - PURPLE = '\033[95m' - CYAN = '\033[96m' - DARKCYAN = '\033[36m' - BLUE = '\033[94m' - GREEN = '\033[92m' - YELLOW = '\033[93m' - RED = '\033[91m' - BOLD = '\033[1m' - UNDERLINE = '\033[4m' - END = '\033[0m' -else: - class color: - PURPLE = '' - CYAN = '' - DARKCYAN = '' - BLUE = '' - GREEN = '' - YELLOW = '' - RED = '' - BOLD = '' - UNDERLINE = '' - END = '' - - -def remove_color(text): - """Remove ANSI and xterm256 color codes. - - From https://stackoverflow.com/a/30500866/8548472 - """ - return re.sub(r'\x1b(\[.*?[@-~]|\].*?(\x07|\x1b\\))', '', text) - - -class Runner: - def __init__(self, log_dir, verbose): - self.name = "" - self.cmd = "" - self.cwd = None - self.args = [] - self.env = {} - self.log_dir = log_dir - self.log_filename = "" - self.log_fd = None - self.verbose = verbose - self.output_queue = queue.Queue() - self.start_time = time.time() - - def create_log_filename(self, model, test_filter): - return self.log_dir + os.path.sep + \ - "log-{}-{}-{}-{}.log".format( - self.name, - model, - test_filter, - datetime.datetime.now().strftime("%Y-%m-%dT%H-%M-%SZ")) - - def start(self, config): - if self.verbose: - print("Running: {}".format(" ".join([self.cmd] + self.args))) - - atexit.register(self.stop) - - self.log_filename = self.create_log_filename( - config['model'], config['test_filter']) - self.log_fd = open(self.log_filename, 'w') - - self.process = subprocess.Popen( - [self.cmd] + self.args, - cwd=self.cwd, - env=self.env, - stdout=subprocess.PIPE, - stderr=subprocess.STDOUT, - universal_newlines=True - ) - - self.stop_thread = threading.Event() - self.thread = threading.Thread(target=self.process_output) - self.thread.start() - - def process_output(self): - while not self.stop_thread.is_set(): - line = self.process.stdout.readline() - if line == "\n": - continue - self.output_queue.put(line) - self.log_fd.write(line) - self.log_fd.flush() - - def poll(self): - return self.process.poll() - - def wait(self, timeout_min): - try: - return self.process.wait(timeout=timeout_min*60) - except subprocess.TimeoutExpired: - print("Timeout of {} min{} reached, stopping...". - format(timeout_min, "s" if timeout_min > 1 else "")) - self.stop() - print("stopped.") - return errno.ETIMEDOUT - - def get_output(self): - try: - output = self.output_queue.get(block=True, timeout=0.1) - if supports_color(): - return output - else: - return remove_color(output) - except queue.Empty: - return None - - def print_output(self): - output = self.get_output() - if not output: - return - print(color.END + - "[" + self.name.ljust(11) + "] " + - output + - color.END, end="") - - def stop(self): - atexit.unregister(self.stop) - - self.stop_thread.set() - - returncode = self.process.poll() - if returncode is None: - - if self.verbose: - print("Terminating {}".format(self.cmd)) - self.process.terminate() - - try: - returncode = self.process.wait(timeout=1) - except subprocess.TimeoutExpired: - pass - - if returncode is None: - if self.verbose: - print("Killing {}".format(self.cmd)) - self.process.kill() - returncode = self.process.poll() - - if self.verbose: - print("{} exited with {}".format( - self.cmd, self.process.returncode)) - - self.thread.join() - - self.log_fd.close() - - return self.process.returncode - - def time_elapsed_s(self): - return time.time() - self.start_time - - -class Px4Runner(Runner): - def __init__(self, model, workspace_dir, log_dir, speed_factor, debugger, - verbose): - super().__init__(log_dir, verbose) - self.name = "px4" - self.cmd = workspace_dir + "/build/px4_sitl_default/bin/px4" - self.cwd = workspace_dir + "/build/px4_sitl_default/tmp/rootfs" - self.args = [ - workspace_dir + "/ROMFS/px4fmu_common", - "-s", - "etc/init.d-posix/rcS", - "-t", - workspace_dir + "/test_data", - "-d" - ] - self.env = {"PATH": os.environ['PATH'], - "PX4_SIM_MODEL": model, - "PX4_SIM_SPEED_FACTOR": str(speed_factor)} - self.name = "px4" - self.debugger = debugger - - if not self.debugger: - pass - elif self.debugger == "valgrind": - self.args = ["--track-origins=yes", "--leak-check=full", "-v", - self.cmd] + self.args - self.cmd = "valgrind" - elif self.debugger == "callgrind": - self.args = ["--tool=callgrind", "-v", self.cmd] + self.args - self.cmd = "valgrind" - elif self.debugger == "gdb": - self.args = ["--args", self.cmd] + self.args - self.cmd = "gdb" - else: - print("Using custom debugger " + self.debugger) - self.args = [self.cmd] + self.args - self.cmd = self.debugger - - -class GzserverRunner(Runner): - def __init__(self, model, workspace_dir, log_dir, speed_factor, verbose): - super().__init__(log_dir, verbose) - self.name = "gzserver" - self.env = {"PATH": os.environ['PATH'], - "HOME": os.environ['HOME'], - "GAZEBO_PLUGIN_PATH": - workspace_dir + "/build/px4_sitl_default/build_gazebo", - "GAZEBO_MODEL_PATH": - workspace_dir + "/Tools/sitl_gazebo/models", - "PX4_SIM_SPEED_FACTOR": str(speed_factor), - "DISPLAY": os.environ['DISPLAY']} - self.cmd = "gzserver" - self.args = ["--verbose", - workspace_dir + "/Tools/sitl_gazebo/worlds/" + - model + ".world"] - - -class GzclientRunner(Runner): - def __init__(self, workspace_dir, log_dir, verbose): - super().__init__(log_dir, verbose) - self.name = "gzclient" - self.env = {"PATH": os.environ['PATH'], - "HOME": os.environ['HOME'], - # "GAZEBO_PLUGIN_PATH": - # workspace_dir + "/build/px4_sitl_default/build_gazebo", - "GAZEBO_MODEL_PATH": - workspace_dir + "/Tools/sitl_gazebo/models", - "DISPLAY": os.environ['DISPLAY']} - self.cmd = "gzclient" - self.args = ["--verbose"] - - -class TestRunner(Runner): - def __init__(self, workspace_dir, log_dir, config, test, - mavlink_connection, verbose): - super().__init__(log_dir, verbose) - self.name = "test_runner" - self.env = {"PATH": os.environ['PATH']} - self.cmd = workspace_dir + \ - "/build/px4_sitl_default/mavsdk_tests/mavsdk_tests" - self.args = ["--url", mavlink_connection, test] +from logger_helper import color +from process_helper import \ + Px4Runner, \ + GzserverRunner, \ + GzclientRunner, \ + TestRunner def main(): diff --git a/test/mavsdk_tests/process_helper.py b/test/mavsdk_tests/process_helper.py new file mode 100644 index 000000000000..75dd46b68f02 --- /dev/null +++ b/test/mavsdk_tests/process_helper.py @@ -0,0 +1,217 @@ +#!/usr/bin/env python3 + +import queue +import time +import os +import datetime +import atexit +import subprocess +import threading +import errno +from logger_helper import color + + +class Runner: + def __init__(self, log_dir, verbose): + self.name = "" + self.cmd = "" + self.cwd = None + self.args = [] + self.env = {} + self.log_dir = log_dir + self.log_filename = "" + self.log_fd = None + self.verbose = verbose + self.output_queue = queue.Queue() + self.start_time = time.time() + + def create_log_filename(self, model, test_filter): + return self.log_dir + os.path.sep + \ + "log-{}-{}-{}-{}.log".format( + self.name, + model, + test_filter, + datetime.datetime.now().strftime("%Y-%m-%dT%H-%M-%SZ")) + + def start(self, config): + if self.verbose: + print("Running: {}".format(" ".join([self.cmd] + self.args))) + + atexit.register(self.stop) + + self.log_filename = self.create_log_filename( + config['model'], config['test_filter']) + self.log_fd = open(self.log_filename, 'w') + + self.process = subprocess.Popen( + [self.cmd] + self.args, + cwd=self.cwd, + env=self.env, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + universal_newlines=True + ) + + self.stop_thread = threading.Event() + self.thread = threading.Thread(target=self.process_output) + self.thread.start() + + def process_output(self): + while not self.stop_thread.is_set(): + line = self.process.stdout.readline() + if line == "\n": + continue + self.output_queue.put(line) + self.log_fd.write(line) + self.log_fd.flush() + + def poll(self): + return self.process.poll() + + def wait(self, timeout_min): + try: + return self.process.wait(timeout=timeout_min*60) + except subprocess.TimeoutExpired: + print("Timeout of {} min{} reached, stopping...". + format(timeout_min, "s" if timeout_min > 1 else "")) + self.stop() + print("stopped.") + return errno.ETIMEDOUT + + def get_output(self): + try: + output = self.output_queue.get(block=True, timeout=0.1) + if supports_color(): + return output + else: + return remove_color(output) + except queue.Empty: + return None + + def print_output(self): + output = self.get_output() + if not output: + return + print(color.END + + "[" + self.name.ljust(11) + "] " + + output + + color.END, end="") + + def stop(self): + atexit.unregister(self.stop) + + self.stop_thread.set() + + returncode = self.process.poll() + if returncode is None: + + if self.verbose: + print("Terminating {}".format(self.cmd)) + self.process.terminate() + + try: + returncode = self.process.wait(timeout=1) + except subprocess.TimeoutExpired: + pass + + if returncode is None: + if self.verbose: + print("Killing {}".format(self.cmd)) + self.process.kill() + returncode = self.process.poll() + + if self.verbose: + print("{} exited with {}".format( + self.cmd, self.process.returncode)) + + self.thread.join() + + self.log_fd.close() + + return self.process.returncode + + def time_elapsed_s(self): + return time.time() - self.start_time + + +class Px4Runner(Runner): + def __init__(self, model, workspace_dir, log_dir, speed_factor, debugger, + verbose): + super().__init__(log_dir, verbose) + self.name = "px4" + self.cmd = workspace_dir + "/build/px4_sitl_default/bin/px4" + self.cwd = workspace_dir + "/build/px4_sitl_default/tmp/rootfs" + self.args = [ + workspace_dir + "/ROMFS/px4fmu_common", + "-s", + "etc/init.d-posix/rcS", + "-t", + workspace_dir + "/test_data", + "-d" + ] + self.env = {"PATH": os.environ['PATH'], + "PX4_SIM_MODEL": model, + "PX4_SIM_SPEED_FACTOR": str(speed_factor)} + self.name = "px4" + self.debugger = debugger + + if not self.debugger: + pass + elif self.debugger == "valgrind": + self.args = ["--track-origins=yes", "--leak-check=full", "-v", + self.cmd] + self.args + self.cmd = "valgrind" + elif self.debugger == "callgrind": + self.args = ["--tool=callgrind", "-v", self.cmd] + self.args + self.cmd = "valgrind" + elif self.debugger == "gdb": + self.args = ["--args", self.cmd] + self.args + self.cmd = "gdb" + else: + print("Using custom debugger " + self.debugger) + self.args = [self.cmd] + self.args + self.cmd = self.debugger + + +class GzserverRunner(Runner): + def __init__(self, model, workspace_dir, log_dir, speed_factor, verbose): + super().__init__(log_dir, verbose) + self.name = "gzserver" + self.env = {"PATH": os.environ['PATH'], + "HOME": os.environ['HOME'], + "GAZEBO_PLUGIN_PATH": + workspace_dir + "/build/px4_sitl_default/build_gazebo", + "GAZEBO_MODEL_PATH": + workspace_dir + "/Tools/sitl_gazebo/models", + "PX4_SIM_SPEED_FACTOR": str(speed_factor), + "DISPLAY": os.environ['DISPLAY']} + self.cmd = "gzserver" + self.args = ["--verbose", + workspace_dir + "/Tools/sitl_gazebo/worlds/" + + model + ".world"] + + +class GzclientRunner(Runner): + def __init__(self, workspace_dir, log_dir, verbose): + super().__init__(log_dir, verbose) + self.name = "gzclient" + self.env = {"PATH": os.environ['PATH'], + "HOME": os.environ['HOME'], + # "GAZEBO_PLUGIN_PATH": + # workspace_dir + "/build/px4_sitl_default/build_gazebo", + "GAZEBO_MODEL_PATH": + workspace_dir + "/Tools/sitl_gazebo/models", + "DISPLAY": os.environ['DISPLAY']} + self.cmd = "gzclient" + self.args = ["--verbose"] + + +class TestRunner(Runner): + def __init__(self, workspace_dir, log_dir, config, test, + mavlink_connection, verbose): + super().__init__(log_dir, verbose) + self.name = "test_runner" + self.env = {"PATH": os.environ['PATH']} + self.cmd = workspace_dir + \ + "/build/px4_sitl_default/mavsdk_tests/mavsdk_tests" + self.args = ["--url", mavlink_connection, test] From 48f0339b73100ad9bd57c184f0838ab5f8fd9d5c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 12 Mar 2020 08:43:20 +0100 Subject: [PATCH 28/51] mavsdk_tests: import naming, easier coloring --- test/mavsdk_tests/logger_helper.py | 78 +++++++++++++------------ test/mavsdk_tests/mavsdk_test_runner.py | 46 ++++++++------- test/mavsdk_tests/process_helper.py | 13 ++--- 3 files changed, 70 insertions(+), 67 deletions(-) diff --git a/test/mavsdk_tests/logger_helper.py b/test/mavsdk_tests/logger_helper.py index 92babc1defb4..c24573dabf68 100644 --- a/test/mavsdk_tests/logger_helper.py +++ b/test/mavsdk_tests/logger_helper.py @@ -3,50 +3,52 @@ import re import sys import os +from enum import Enum +from functools import lru_cache -def supports_color(): - """Returns True if the running system's terminal supports color. +class color(Enum): + PURPLE = '\033[95m' + CYAN = '\033[96m' + DARKCYAN = '\033[36m' + BLUE = '\033[94m' + GREEN = '\033[92m' + YELLOW = '\033[93m' + RED = '\033[91m' + BOLD = '\033[1m' + UNDERLINE = '\033[4m' + RESET = '\033[0m' - From https://stackoverflow.com/a/22254892/8548472 - """ - plat = sys.platform - supported_platform = plat != 'Pocket PC' and (plat != 'win32' or - 'ANSICON' in os.environ) - # isatty is not always implemented, #6223. - is_a_tty = hasattr(sys.stdout, 'isatty') and sys.stdout.isatty() - return supported_platform and is_a_tty + +def colorize(text: str, c: color) -> str: + if _supports_color(): + return c.value + text + color.RESET.value + else: + return text -if supports_color(): - class color: - PURPLE = '\033[95m' - CYAN = '\033[96m' - DARKCYAN = '\033[36m' - BLUE = '\033[94m' - GREEN = '\033[92m' - YELLOW = '\033[93m' - RED = '\033[91m' - BOLD = '\033[1m' - UNDERLINE = '\033[4m' - END = '\033[0m' -else: - class color: - PURPLE = '' - CYAN = '' - DARKCYAN = '' - BLUE = '' - GREEN = '' - YELLOW = '' - RED = '' - BOLD = '' - UNDERLINE = '' - END = '' - - -def remove_color(text): +def maybe_strip_color(text: str) -> str: """Remove ANSI and xterm256 color codes. From https://stackoverflow.com/a/30500866/8548472 """ - return re.sub(r'\x1b(\[.*?[@-~]|\].*?(\x07|\x1b\\))', '', text) + if not _supports_color(): + return re.sub(r'\x1b(\[.*?[@-~]|\].*?(\x07|\x1b\\))', '', text) + else: + return text + + +@lru_cache +def _supports_color() -> bool: + """Returns True if the running system's terminal supports color. + + From https://stackoverflow.com/a/22254892/8548472 + """ + supported_platform = \ + (sys.platform != 'Pocket PC') and \ + (sys.platform != 'win32' or 'ANSICON' in os.environ) + + # isatty is not always implemented. + is_a_tty = hasattr(sys.stdout, 'isatty') and sys.stdout.isatty() + + return supported_platform and is_a_tty diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 56ae289cdce6..29be306d0a2b 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -7,12 +7,8 @@ import signal import subprocess import sys -from logger_helper import color -from process_helper import \ - Px4Runner, \ - GzserverRunner, \ - GzclientRunner, \ - TestRunner +from logger_helper import color, colorize +import process_helper as ph def main(): @@ -160,10 +156,12 @@ def run(self): break if overall_success: - print(color.GREEN + "Overall result: success!" + color.END) + print(colorize(colorize( + "Overall result: success!", color.GREEN), color.BOLD)) return 0 else: - print(color.RED + "Overall result: failure!" + color.END) + print(colorize(colorize( + "Overall result: failure!", color.RED), color.BOLD)) return 1 def run_test_group(self): @@ -184,8 +182,10 @@ def run_test_group(self): models = [] for group in models: - print(color.BOLD + "==> Running tests for '{}' with filter '{}'" - .format(group['model'], group['test_filter']) + color.END) + print(colorize( + "==> Running tests for '{}' with filter '{}'" + .format(group['model'], group['test_filter']), + color.BOLD)) tests = determine_tests(group['test_filter']) @@ -194,11 +194,15 @@ def run_test_group(self): format(i+1, len(tests), test)) was_success = self.run_test(test, group) if was_success: - print(color.GREEN + "--- Test {} of {}: '{}' succeeded.". - format(i+1, len(tests), test) + color.END) + print(colorize( + "--- Test {} of {}: '{}' succeeded." + .format(i+1, len(tests), test), + color.GREEN)) else: - print(color.RED + "--- Test {} of {}: '{}' failed.". - format(i+1, len(tests), test) + color.END) + print(colorize( + "--- Test {} of {}: '{}' failed." + .format(i+1, len(tests), test), + color.RED)) if not was_success: overall_success = False @@ -217,14 +221,14 @@ def run_test(self, test, group): speed_factor = min(int(speed_factor), group["max_speed_factor"]) if self.config['mode'] == 'sitl': - px4_runner = Px4Runner( + px4_runner = ph.Px4Runner( group['model'], os.getcwd(), self.log_dir, speed_factor, self.debugger, self.verbose) px4_runner.start(group) self.active_runners.append(px4_runner) if self.config['simulator'] == 'gazebo': - gzserver_runner = GzserverRunner( + gzserver_runner = ph.GzserverRunner( group['model'], os.getcwd(), self.log_dir, @@ -234,12 +238,12 @@ def run_test(self, test, group): self.active_runners.append(gzserver_runner) if self.gui: - gzclient_runner = GzclientRunner( + gzclient_runner = ph.GzclientRunner( os.getcwd(), self.log_dir, self.verbose) gzclient_runner.start(group) self.active_runners.append(gzclient_runner) - test_runner = TestRunner( + test_runner = ph.TestRunner( os.getcwd(), self.log_dir, group, @@ -261,8 +265,10 @@ def run_test(self, test, group): runner.print_output() else: - print(color.BOLD + "Test timeout of {} mins triggered!". - format(group['timeout_min']) + color.END) + print(colorize( + "Test timeout of {} mins triggered!". + format(group['timeout_min']), + color.BOLD)) is_success = False for runner in self.active_runners: diff --git a/test/mavsdk_tests/process_helper.py b/test/mavsdk_tests/process_helper.py index 75dd46b68f02..3d2a318d7242 100644 --- a/test/mavsdk_tests/process_helper.py +++ b/test/mavsdk_tests/process_helper.py @@ -8,7 +8,7 @@ import subprocess import threading import errno -from logger_helper import color +from logger_helper import color, maybe_strip_color, colorize class Runner: @@ -81,10 +81,7 @@ def wait(self, timeout_min): def get_output(self): try: output = self.output_queue.get(block=True, timeout=0.1) - if supports_color(): - return output - else: - return remove_color(output) + return maybe_strip_color(output) except queue.Empty: return None @@ -92,10 +89,8 @@ def print_output(self): output = self.get_output() if not output: return - print(color.END + - "[" + self.name.ljust(11) + "] " + - output + - color.END, end="") + print(colorize("[" + self.name.ljust(11) + "] " + output, color.RESET), + end="") def stop(self): atexit.unregister(self.stop) From 10d0b6c7aa71f07278a36844f80a3c2430d2ce70 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 13 Mar 2020 09:18:10 +0100 Subject: [PATCH 29/51] mavsdk_tests: further argument cleanup, some types --- test/mavsdk_tests/mavsdk_test_runner.py | 29 +++++++++----- test/mavsdk_tests/process_helper.py | 50 +++++++++++++++---------- 2 files changed, 49 insertions(+), 30 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 29be306d0a2b..68438b9c24ea 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -3,7 +3,7 @@ import argparse import json import os -import psutil +import psutil # type: ignore import signal import subprocess import sys @@ -142,7 +142,7 @@ def run(self): for iteration in range(self.iterations): if self.iterations > 1: - print("Test iteration: {}". + print("Test iteration: {} / {}". format(iteration + 1, self.iterations)) was_success = self.run_test_group() @@ -203,6 +203,7 @@ def run_test_group(self): "--- Test {} of {}: '{}' failed." .format(i+1, len(tests), test), color.RED)) + # TODO: now print the test output if not was_success: overall_success = False @@ -222,25 +223,32 @@ def run_test(self, test, group): if self.config['mode'] == 'sitl': px4_runner = ph.Px4Runner( - group['model'], os.getcwd(), self.log_dir, speed_factor, - self.debugger, self.verbose) - px4_runner.start(group) + os.getcwd(), + self.log_dir, + group, + speed_factor, + self.debugger, + self.verbose) + px4_runner.start() self.active_runners.append(px4_runner) if self.config['simulator'] == 'gazebo': gzserver_runner = ph.GzserverRunner( - group['model'], os.getcwd(), self.log_dir, + group, self.speed_factor, self.verbose) - gzserver_runner.start(group) + gzserver_runner.start() self.active_runners.append(gzserver_runner) if self.gui: gzclient_runner = ph.GzclientRunner( - os.getcwd(), self.log_dir, self.verbose) - gzclient_runner.start(group) + os.getcwd(), + self.log_dir, + group, + self.verbose) + gzclient_runner.start() self.active_runners.append(gzclient_runner) test_runner = ph.TestRunner( @@ -251,7 +259,7 @@ def run_test(self, test, group): self.config['mavlink_connection'], self.verbose) - test_runner.start(group) + test_runner.start() self.active_runners.append(test_runner) while test_runner.time_elapsed_s() < group['timeout_min']*60: @@ -282,6 +290,7 @@ def sigint_handler(self, sig, frame): for runner in self.active_runners: runner.stop() print("Stopping all processes done.") + # TODO: print interim results sys.exit(-sig) diff --git a/test/mavsdk_tests/process_helper.py b/test/mavsdk_tests/process_helper.py index 3d2a318d7242..d6f261bbb503 100644 --- a/test/mavsdk_tests/process_helper.py +++ b/test/mavsdk_tests/process_helper.py @@ -9,40 +9,46 @@ import threading import errno from logger_helper import color, maybe_strip_color, colorize +from typing import Dict, List class Runner: - def __init__(self, log_dir, verbose): + def __init__(self, log_dir: str, config: dict, verbose: bool): self.name = "" self.cmd = "" - self.cwd = None - self.args = [] - self.env = {} + self.cwd = "" + self.args: List[str] + self.env: Dict[str, str] self.log_dir = log_dir + self.config = config self.log_filename = "" self.log_fd = None self.verbose = verbose - self.output_queue = queue.Queue() + self.output_queue: queue.Queue = queue.Queue() self.start_time = time.time() - def create_log_filename(self, model, test_filter): + def create_log_filename(self, model: str, test_filter: str) -> str: return self.log_dir + os.path.sep + \ "log-{}-{}-{}-{}.log".format( self.name, model, test_filter, datetime.datetime.now().strftime("%Y-%m-%dT%H-%M-%SZ")) + # TODO: improve logfilename, create folder, create merged log. - def start(self, config): + def start(self): if self.verbose: print("Running: {}".format(" ".join([self.cmd] + self.args))) atexit.register(self.stop) self.log_filename = self.create_log_filename( - config['model'], config['test_filter']) + self.config['model'], self.config['test_filter']) self.log_fd = open(self.log_filename, 'w') + print("self.cmd: {}".format(self.cmd)) + print("self.args: {}".format(self.args)) + print("self.cwd: {}".format(self.cwd)) self.process = subprocess.Popen( [self.cmd] + self.args, cwd=self.cwd, @@ -130,9 +136,10 @@ def time_elapsed_s(self): class Px4Runner(Runner): - def __init__(self, model, workspace_dir, log_dir, speed_factor, debugger, - verbose): - super().__init__(log_dir, verbose) + def __init__(self, workspace_dir: str, log_dir: str, + config: Dict[str, str], speed_factor: float, + debugger: str, verbose: bool): + super().__init__(log_dir, config, verbose) self.name = "px4" self.cmd = workspace_dir + "/build/px4_sitl_default/bin/px4" self.cwd = workspace_dir + "/build/px4_sitl_default/tmp/rootfs" @@ -144,10 +151,9 @@ def __init__(self, model, workspace_dir, log_dir, speed_factor, debugger, workspace_dir + "/test_data", "-d" ] - self.env = {"PATH": os.environ['PATH'], - "PX4_SIM_MODEL": model, + self.env = {"PATH": str(os.environ['PATH']), + "PX4_SIM_MODEL": str(config['model']), "PX4_SIM_SPEED_FACTOR": str(speed_factor)} - self.name = "px4" self.debugger = debugger if not self.debugger: @@ -169,9 +175,11 @@ def __init__(self, model, workspace_dir, log_dir, speed_factor, debugger, class GzserverRunner(Runner): - def __init__(self, model, workspace_dir, log_dir, speed_factor, verbose): - super().__init__(log_dir, verbose) + def __init__(self, workspace_dir, log_dir, config, + speed_factor, verbose): + super().__init__(log_dir, config, verbose) self.name = "gzserver" + self.cwd = workspace_dir self.env = {"PATH": os.environ['PATH'], "HOME": os.environ['HOME'], "GAZEBO_PLUGIN_PATH": @@ -183,13 +191,14 @@ def __init__(self, model, workspace_dir, log_dir, speed_factor, verbose): self.cmd = "gzserver" self.args = ["--verbose", workspace_dir + "/Tools/sitl_gazebo/worlds/" + - model + ".world"] + config['model'] + ".world"] class GzclientRunner(Runner): - def __init__(self, workspace_dir, log_dir, verbose): - super().__init__(log_dir, verbose) + def __init__(self, workspace_dir, log_dir, config, verbose): + super().__init__(log_dir, config, verbose) self.name = "gzclient" + self.cwd = workspace_dir self.env = {"PATH": os.environ['PATH'], "HOME": os.environ['HOME'], # "GAZEBO_PLUGIN_PATH": @@ -204,8 +213,9 @@ def __init__(self, workspace_dir, log_dir, verbose): class TestRunner(Runner): def __init__(self, workspace_dir, log_dir, config, test, mavlink_connection, verbose): - super().__init__(log_dir, verbose) + super().__init__(log_dir, config, verbose) self.name = "test_runner" + self.cwd = workspace_dir self.env = {"PATH": os.environ['PATH']} self.cmd = workspace_dir + \ "/build/px4_sitl_default/mavsdk_tests/mavsdk_tests" From 0a444afa08bb2cf906c1e5d3693c5810bc4f55a2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 13 Mar 2020 10:02:58 +0100 Subject: [PATCH 30/51] mavsdk_tests: satisfy mypy type checks --- test/mavsdk_tests/logger_helper.py | 2 +- test/mavsdk_tests/mavsdk_test_runner.py | 1 + test/mavsdk_tests/process_helper.py | 51 ++++++++++++++++--------- 3 files changed, 36 insertions(+), 18 deletions(-) diff --git a/test/mavsdk_tests/logger_helper.py b/test/mavsdk_tests/logger_helper.py index c24573dabf68..a5766ef14718 100644 --- a/test/mavsdk_tests/logger_helper.py +++ b/test/mavsdk_tests/logger_helper.py @@ -22,7 +22,7 @@ class color(Enum): def colorize(text: str, c: color) -> str: if _supports_color(): - return c.value + text + color.RESET.value + return str(c.value) + text + color.RESET.value else: return text diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 68438b9c24ea..f5810077da25 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -72,6 +72,7 @@ def determine_tests(filter): stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.STDOUT) + assert p.stdout is not None tests = str(p.stdout.read().decode("utf-8")).strip().split('\n') return tests diff --git a/test/mavsdk_tests/process_helper.py b/test/mavsdk_tests/process_helper.py index d6f261bbb503..d95f0021ab53 100644 --- a/test/mavsdk_tests/process_helper.py +++ b/test/mavsdk_tests/process_helper.py @@ -9,11 +9,14 @@ import threading import errno from logger_helper import color, maybe_strip_color, colorize -from typing import Dict, List +from typing import Dict, List, TextIO, Optional class Runner: - def __init__(self, log_dir: str, config: dict, verbose: bool): + def __init__(self, + log_dir: str, + config: Dict[str, str], + verbose: bool): self.name = "" self.cmd = "" self.cwd = "" @@ -22,9 +25,9 @@ def __init__(self, log_dir: str, config: dict, verbose: bool): self.log_dir = log_dir self.config = config self.log_filename = "" - self.log_fd = None + self.log_fd: TextIO self.verbose = verbose - self.output_queue: queue.Queue = queue.Queue() + self.output_queue: queue.Queue[str] = queue.Queue() self.start_time = time.time() def create_log_filename(self, model: str, test_filter: str) -> str: @@ -36,7 +39,7 @@ def create_log_filename(self, model: str, test_filter: str) -> str: datetime.datetime.now().strftime("%Y-%m-%dT%H-%M-%SZ")) # TODO: improve logfilename, create folder, create merged log. - def start(self): + def start(self) -> None: if self.verbose: print("Running: {}".format(" ".join([self.cmd] + self.args))) @@ -62,7 +65,8 @@ def start(self): self.thread = threading.Thread(target=self.process_output) self.thread.start() - def process_output(self): + def process_output(self) -> None: + assert self.process.stdout is not None while not self.stop_thread.is_set(): line = self.process.stdout.readline() if line == "\n": @@ -71,10 +75,10 @@ def process_output(self): self.log_fd.write(line) self.log_fd.flush() - def poll(self): + def poll(self) -> Optional[int]: return self.process.poll() - def wait(self, timeout_min): + def wait(self, timeout_min: float) -> Optional[int]: try: return self.process.wait(timeout=timeout_min*60) except subprocess.TimeoutExpired: @@ -84,21 +88,21 @@ def wait(self, timeout_min): print("stopped.") return errno.ETIMEDOUT - def get_output(self): + def get_output(self) -> Optional[str]: try: output = self.output_queue.get(block=True, timeout=0.1) return maybe_strip_color(output) except queue.Empty: return None - def print_output(self): + def print_output(self) -> None: output = self.get_output() if not output: return print(colorize("[" + self.name.ljust(11) + "] " + output, color.RESET), end="") - def stop(self): + def stop(self) -> int: atexit.unregister(self.stop) self.stop_thread.set() @@ -131,7 +135,7 @@ def stop(self): return self.process.returncode - def time_elapsed_s(self): + def time_elapsed_s(self) -> float: return time.time() - self.start_time @@ -175,8 +179,12 @@ def __init__(self, workspace_dir: str, log_dir: str, class GzserverRunner(Runner): - def __init__(self, workspace_dir, log_dir, config, - speed_factor, verbose): + def __init__(self, + workspace_dir: str, + log_dir: str, + config: Dict[str, str], + speed_factor: float, + verbose: bool): super().__init__(log_dir, config, verbose) self.name = "gzserver" self.cwd = workspace_dir @@ -195,7 +203,11 @@ def __init__(self, workspace_dir, log_dir, config, class GzclientRunner(Runner): - def __init__(self, workspace_dir, log_dir, config, verbose): + def __init__(self, + workspace_dir: str, + log_dir: str, + config: Dict[str, str], + verbose: bool): super().__init__(log_dir, config, verbose) self.name = "gzclient" self.cwd = workspace_dir @@ -211,8 +223,13 @@ def __init__(self, workspace_dir, log_dir, config, verbose): class TestRunner(Runner): - def __init__(self, workspace_dir, log_dir, config, test, - mavlink_connection, verbose): + def __init__(self, + workspace_dir: str, + log_dir: str, + config: Dict[str, str], + test: str, + mavlink_connection: str, + verbose: bool): super().__init__(log_dir, config, verbose) self.name = "test_runner" self.cwd = workspace_dir From 7f8dac59f00bbbf56e266395340a7503e628b734 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 13 Mar 2020 18:53:11 +0100 Subject: [PATCH 31/51] mavsdk_tests: print overall results --- test/mavsdk_tests/logger_helper.py | 1 + test/mavsdk_tests/mavsdk_test_runner.py | 274 ++++++++++++++++-------- test/mavsdk_tests/process_helper.py | 3 - 3 files changed, 182 insertions(+), 96 deletions(-) diff --git a/test/mavsdk_tests/logger_helper.py b/test/mavsdk_tests/logger_helper.py index a5766ef14718..b63a0df0c051 100644 --- a/test/mavsdk_tests/logger_helper.py +++ b/test/mavsdk_tests/logger_helper.py @@ -15,6 +15,7 @@ class color(Enum): GREEN = '\033[92m' YELLOW = '\033[93m' RED = '\033[91m' + GRAY = '\033[90m' BOLD = '\033[1m' UNDERLINE = '\033[4m' RESET = '\033[0m' diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index f5810077da25..b81e2e856931 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -9,9 +9,11 @@ import sys from logger_helper import color, colorize import process_helper as ph +from typing import Any, Dict, List, NoReturn, Optional +from types import FrameType -def main(): +def main() -> NoReturn: parser = argparse.ArgumentParser() parser.add_argument("--log-dir", @@ -61,30 +63,17 @@ def main(): ) signal.signal(signal.SIGINT, tester.sigint_handler) - sys.exit(tester.run()) + sys.exit(0 if tester.run() else 1) -def determine_tests(filter): - cmd = os.getcwd() + "/build/px4_sitl_default/mavsdk_tests/mavsdk_tests" - args = ["--list-test-names-only", filter] - p = subprocess.Popen( - [cmd] + args, - stdin=subprocess.PIPE, - stdout=subprocess.PIPE, - stderr=subprocess.STDOUT) - assert p.stdout is not None - tests = str(p.stdout.read().decode("utf-8")).strip().split('\n') - return tests - - -def is_running(process_name): +def is_running(process_name: str) -> bool: for proc in psutil.process_iter(attrs=['name']): if proc.info['name'] == process_name: return True return False -def is_everything_ready(config): +def is_everything_ready(config: Dict[str, str]) -> bool: result = True if config['mode'] == 'sitl': @@ -118,115 +107,158 @@ def is_everything_ready(config): class Tester: def __init__(self, - config, - iterations, - abort_early, - speed_factor, - model, - debugger, - log_dir, - gui, - verbose): - self.active_runners = [] + config: Dict[str, Any], + iterations: int, + abort_early: bool, + speed_factor: float, + model: str, + debugger: str, + log_dir: str, + gui: bool, + verbose: bool): + self.config = config + self.active_runners: List[ph.Runner] self.iterations = iterations self.abort_early = abort_early - self.config = config + self.tests = self.determine_tests(config['tests'], model) self.speed_factor = speed_factor - self.model = model self.debugger = debugger self.log_dir = log_dir self.gui = gui self.verbose = verbose - def run(self): - overall_success = True - + @classmethod + def determine_tests(cls, tests: List[Dict[str, Any]], model: str) \ + -> List[Dict[str, Any]]: + for test in tests: + test['selected'] = (model == 'all') or model == test['model'] + test['cases'] = dict.fromkeys( + cls.determine_test_cases(test['test_filter'])) + + return tests + + @staticmethod + def determine_test_cases(filter: str) -> List[str]: + cmd = os.getcwd() + "/build/px4_sitl_default/mavsdk_tests/mavsdk_tests" + args = ["--list-test-names-only", filter] + p = subprocess.Popen( + [cmd] + args, + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT) + assert p.stdout is not None + cases = str(p.stdout.read().decode("utf-8")).strip().split('\n') + return cases + + @staticmethod + def plural_s(n_items: int) -> str: + if n_items > 1: + return "s" + else: + return "" + + def run(self) -> bool: + self.show_plans() + self.prepare_for_results() + self.run_tests() + self.show_detailed_results() + self.show_overall_result() + return self.was_overall_pass() + + def show_plans(self) -> None: + n_models = sum(map( + lambda test: 1 if test['selected'] else 0, + self.tests)) + n_cases = sum(map( + lambda test: len(test['cases']) if test['selected'] else 0, + self.tests)) + + print() + print(colorize( + "About to run {} test case{} for {} selected model{} " + "({} iteration{}):". + format(n_cases, self.plural_s(n_cases), + n_models, self.plural_s(n_models), + self.iterations, self.plural_s(self.iterations)), + color.BOLD)) + + for test in self.tests: + if not test['selected']: + print(colorize(colorize( + " - {} (not selected)".format(test['model']), + color.BOLD), color.GRAY)) + continue + print(colorize(" - {}:".format(test['model']), color.BOLD)) + for case in test['cases']: + print(" - '{}'".format(case)) + print() + + def prepare_for_results(self) -> None: + # prepare dict entry for results + for test in self.tests: + if not test['selected']: + continue + for key in test['cases'].keys(): + test['cases'][key] = {'results': []} + + def run_tests(self) -> None: for iteration in range(self.iterations): if self.iterations > 1: - print("Test iteration: {} / {}". - format(iteration + 1, self.iterations)) - - was_success = self.run_test_group() - - if not was_success: - overall_success = False - - if self.iterations > 1 and not was_success and self.abort_early: - print("Aborting with a failure in test run {}/{}". + print("%%% Test iteration: {} / {}". format(iteration + 1, self.iterations)) - break - if overall_success: - print(colorize(colorize( - "Overall result: success!", color.GREEN), color.BOLD)) - return 0 - else: - print(colorize(colorize( - "Overall result: failure!", color.RED), color.BOLD)) - return 1 + success = self.run_iteration() + if not success: + if self.abort_early: + break - def run_test_group(self): - overall_success = True + def run_iteration(self) -> bool: + iteration_success = True - tests = self.config["tests"] + for test in self.tests: + if not test['selected']: + continue - if self.model == 'all': - models = tests - else: - found = False - for elem in tests: - if elem['model'] == self.model: - models = [elem] - found = True - if not found: - print("Specified model is not defined") - models = [] - - for group in models: print(colorize( - "==> Running tests for '{}' with filter '{}'" - .format(group['model'], group['test_filter']), + "==> Running tests for {}".format(test['model']), color.BOLD)) - tests = determine_tests(group['test_filter']) - - for i, test in enumerate(tests): - print("--> Test {} of {}: '{}' running ...". - format(i+1, len(tests), test)) - was_success = self.run_test(test, group) + for i, case in enumerate(test['cases']): + print("--> Test case {} of {}: '{}' running ...". + format(i+1, len(test['cases']), case)) + was_success = self.run_test_case(test, case) if was_success: print(colorize( - "--- Test {} of {}: '{}' succeeded." - .format(i+1, len(tests), test), + "--- Test case {} of {}: '{}' succeeded." + .format(i+1, len(test['cases']), case), color.GREEN)) else: print(colorize( - "--- Test {} of {}: '{}' failed." - .format(i+1, len(tests), test), + "--- Test case {} of {}: '{}' failed." + .format(i+1, len(test['cases']), case), color.RED)) - # TODO: now print the test output if not was_success: - overall_success = False + iteration_success = False if not was_success and self.abort_early: print("Aborting early") return False - return overall_success + return iteration_success - def run_test(self, test, group): + def run_test_case(self, test: Dict[str, Any], case: str) -> bool: self.active_runners = [] speed_factor = self.speed_factor - if "max_speed_factor" in group: - speed_factor = min(int(speed_factor), group["max_speed_factor"]) + if "max_speed_factor" in test: + speed_factor = min(float(speed_factor), test["max_speed_factor"]) if self.config['mode'] == 'sitl': px4_runner = ph.Px4Runner( os.getcwd(), self.log_dir, - group, + test, speed_factor, self.debugger, self.verbose) @@ -237,7 +269,7 @@ def run_test(self, test, group): gzserver_runner = ph.GzserverRunner( os.getcwd(), self.log_dir, - group, + test, self.speed_factor, self.verbose) gzserver_runner.start() @@ -247,7 +279,7 @@ def run_test(self, test, group): gzclient_runner = ph.GzclientRunner( os.getcwd(), self.log_dir, - group, + test, self.verbose) gzclient_runner.start() self.active_runners.append(gzclient_runner) @@ -255,15 +287,15 @@ def run_test(self, test, group): test_runner = ph.TestRunner( os.getcwd(), self.log_dir, - group, test, + case, self.config['mavlink_connection'], self.verbose) test_runner.start() self.active_runners.append(test_runner) - while test_runner.time_elapsed_s() < group['timeout_min']*60: + while test_runner.time_elapsed_s() < test['timeout_min']*60: returncode = test_runner.poll() if returncode is not None: is_success = (returncode == 0) @@ -276,22 +308,78 @@ def run_test(self, test, group): else: print(colorize( "Test timeout of {} mins triggered!". - format(group['timeout_min']), + format(test['timeout_min']), color.BOLD)) is_success = False for runner in self.active_runners: runner.stop() + test['cases'][case]['results'].append(is_success) return is_success - def sigint_handler(self, sig, frame): + def show_detailed_results(self) -> None: + + print() + print(colorize("Results:", color.BOLD)) + + for test in self.tests: + if not test['selected']: + print(colorize(colorize(" - {} (not selected)". + format(test['model']), color.BOLD), color.GRAY)) + continue + else: + print(colorize(" - {}:".format(test['model']), color.BOLD)) + + for name, case in test['cases'].items(): + print(" - '{}': ".format(name), end="") + + n_succeeded = case['results'].count(True) + n_failed = case['results'].count(False) + n_cancelled = self.iterations - len(case['results']) + + notes = [ + self.note_if_any( + "succeeded", n_succeeded, self.iterations), + self.note_if_any( + "failed", n_failed, self.iterations), + self.note_if_any( + "cancelled", n_cancelled, self.iterations)] + notes_without_none = list(filter(None, notes)) + print(", ".join(notes_without_none)) + + def was_overall_pass(self) -> bool: + for test in self.tests: + if not test['selected']: + continue + + for case in test['cases'].values(): + if case['results'].count(False) > 0: + return False + return True + + def show_overall_result(self) -> None: + print("Overall result: {}".format( + "PASS" if self.was_overall_pass() else "FAIL")) + + @staticmethod + def note_if_any(text: str, n: int, total: int) -> Optional[str]: + assert not n < 0 + if n == 0: + return None + elif n == 1 and total == 1: + return text + else: + return "{} {}".format(n, text) + + def sigint_handler(self, sig: signal.Signals, frame: FrameType) \ + -> NoReturn: print("Received SIGINT") print("Stopping all processes ...") for runner in self.active_runners: runner.stop() print("Stopping all processes done.") - # TODO: print interim results + self.show_detailed_results() sys.exit(-sig) diff --git a/test/mavsdk_tests/process_helper.py b/test/mavsdk_tests/process_helper.py index d95f0021ab53..ac128efd3f4a 100644 --- a/test/mavsdk_tests/process_helper.py +++ b/test/mavsdk_tests/process_helper.py @@ -49,9 +49,6 @@ def start(self) -> None: self.config['model'], self.config['test_filter']) self.log_fd = open(self.log_filename, 'w') - print("self.cmd: {}".format(self.cmd)) - print("self.args: {}".format(self.args)) - print("self.cwd: {}".format(self.cwd)) self.process = subprocess.Popen( [self.cmd] + self.args, cwd=self.cwd, From d439552c29dfe7a8005ecd77aab959dcb0f6770f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 16 Mar 2020 16:05:12 +0100 Subject: [PATCH 32/51] mavsdk_tests: improve log file folder structure --- test/mavsdk_tests/mavsdk_test_runner.py | 78 ++++++++++++++++++------- test/mavsdk_tests/process_helper.py | 55 ++++++++--------- 2 files changed, 87 insertions(+), 46 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index b81e2e856931..0ef57e25f164 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -1,7 +1,9 @@ #!/usr/bin/env python3 import argparse +import datetime import json +import math import os import psutil # type: ignore import signal @@ -126,6 +128,7 @@ def __init__(self, self.log_dir = log_dir self.gui = gui self.verbose = verbose + self.start_time = datetime.datetime.now() @classmethod def determine_tests(cls, tests: List[Dict[str, Any]], model: str) \ @@ -194,7 +197,6 @@ def show_plans(self) -> None: print() def prepare_for_results(self) -> None: - # prepare dict entry for results for test in self.tests: if not test['selected']: continue @@ -207,14 +209,13 @@ def run_tests(self) -> None: print("%%% Test iteration: {} / {}". format(iteration + 1, self.iterations)) - success = self.run_iteration() + success = self.run_iteration(iteration) if not success: if self.abort_early: break - def run_iteration(self) -> bool: + def run_iteration(self, iteration: int) -> bool: iteration_success = True - for test in self.tests: if not test['selected']: continue @@ -226,7 +227,15 @@ def run_iteration(self) -> bool: for i, case in enumerate(test['cases']): print("--> Test case {} of {}: '{}' running ...". format(i+1, len(test['cases']), case)) - was_success = self.run_test_case(test, case) + + log_dir = self.get_log_dir(iteration, test['model'], case) + if self.verbose: + print("creating log directory: {}" + .format(log_dir)) + os.makedirs(log_dir, exist_ok=True) + + was_success = self.run_test_case(test, case, log_dir) + if was_success: print(colorize( "--- Test case {} of {}: '{}' succeeded." @@ -247,7 +256,23 @@ def run_iteration(self) -> bool: return iteration_success - def run_test_case(self, test: Dict[str, Any], case: str) -> bool: + def get_log_dir(self, iteration: int, model: str, case: str) -> str: + date_and_time = self.start_time.strftime("%Y-%m-%dT%H-%M-%SZ") + foldername = os.path.join(self.log_dir, date_and_time) + + if self.iterations > 1: + # Use as many zeros in foldername as required. + digits = math.floor(math.log10(self.iterations)) + 1 + foldername = os.path.join(foldername, + str(iteration + 1).zfill(digits)) + + foldername = os.path.join(foldername, model) + foldername = os.path.join(foldername, case.replace(" ", "_")) + + return foldername + + def run_test_case(self, test: Dict[str, Any], + case: str, logfile_path: str) -> bool: self.active_runners = [] speed_factor = self.speed_factor @@ -257,8 +282,9 @@ def run_test_case(self, test: Dict[str, Any], case: str) -> bool: if self.config['mode'] == 'sitl': px4_runner = ph.Px4Runner( os.getcwd(), - self.log_dir, - test, + logfile_path, + test['model'], + case, speed_factor, self.debugger, self.verbose) @@ -268,8 +294,9 @@ def run_test_case(self, test: Dict[str, Any], case: str) -> bool: if self.config['simulator'] == 'gazebo': gzserver_runner = ph.GzserverRunner( os.getcwd(), - self.log_dir, - test, + logfile_path, + test['model'], + case, self.speed_factor, self.verbose) gzserver_runner.start() @@ -278,16 +305,17 @@ def run_test_case(self, test: Dict[str, Any], case: str) -> bool: if self.gui: gzclient_runner = ph.GzclientRunner( os.getcwd(), - self.log_dir, - test, + logfile_path, + test['model'], + case, self.verbose) gzclient_runner.start() self.active_runners.append(gzclient_runner) test_runner = ph.TestRunner( os.getcwd(), - self.log_dir, - test, + logfile_path, + test['model'], case, self.config['mavlink_connection'], self.verbose) @@ -315,18 +343,26 @@ def run_test_case(self, test: Dict[str, Any], case: str) -> bool: for runner in self.active_runners: runner.stop() - test['cases'][case]['results'].append(is_success) + result = {'success': is_success, + 'logfiles': [runner.get_log_filename() + for runner in self.active_runners]} + test['cases'][case]['results'].append(result) + + if not is_success: + # TODO: print error and logfiles + pass + return is_success def show_detailed_results(self) -> None: - print() print(colorize("Results:", color.BOLD)) for test in self.tests: if not test['selected']: - print(colorize(colorize(" - {} (not selected)". - format(test['model']), color.BOLD), color.GRAY)) + print(colorize(colorize( + " - {} (not selected)". + format(test['model']), color.BOLD), color.GRAY)) continue else: print(colorize(" - {}:".format(test['model']), color.BOLD)) @@ -334,8 +370,10 @@ def show_detailed_results(self) -> None: for name, case in test['cases'].items(): print(" - '{}': ".format(name), end="") - n_succeeded = case['results'].count(True) - n_failed = case['results'].count(False) + n_succeeded = [result['success'] + for result in case['results']].count(True) + n_failed = [result['success'] + for result in case['results']].count(False) n_cancelled = self.iterations - len(case['results']) notes = [ diff --git a/test/mavsdk_tests/process_helper.py b/test/mavsdk_tests/process_helper.py index ac128efd3f4a..0e37a0a5edac 100644 --- a/test/mavsdk_tests/process_helper.py +++ b/test/mavsdk_tests/process_helper.py @@ -3,7 +3,6 @@ import queue import time import os -import datetime import atexit import subprocess import threading @@ -15,29 +14,30 @@ class Runner: def __init__(self, log_dir: str, - config: Dict[str, str], + model: str, + case: str, verbose: bool): self.name = "" self.cmd = "" self.cwd = "" self.args: List[str] self.env: Dict[str, str] - self.log_dir = log_dir - self.config = config + self.model = model + self.case = case self.log_filename = "" self.log_fd: TextIO self.verbose = verbose self.output_queue: queue.Queue[str] = queue.Queue() self.start_time = time.time() + self.log_dir = log_dir + self.log_filename = "" - def create_log_filename(self, model: str, test_filter: str) -> str: - return self.log_dir + os.path.sep + \ - "log-{}-{}-{}-{}.log".format( - self.name, - model, - test_filter, - datetime.datetime.now().strftime("%Y-%m-%dT%H-%M-%SZ")) - # TODO: improve logfilename, create folder, create merged log. + def set_log_filename(self) -> None: + self.log_filename = self.log_dir + os.path.sep + \ + "log-{}.log".format(self.name) + + def get_log_filename(self) -> str: + return self.log_filename def start(self) -> None: if self.verbose: @@ -45,8 +45,9 @@ def start(self) -> None: atexit.register(self.stop) - self.log_filename = self.create_log_filename( - self.config['model'], self.config['test_filter']) + self.set_log_filename() + if self.verbose: + print("Logging to {}".format(self.log_filename)) self.log_fd = open(self.log_filename, 'w') self.process = subprocess.Popen( @@ -138,9 +139,9 @@ def time_elapsed_s(self) -> float: class Px4Runner(Runner): def __init__(self, workspace_dir: str, log_dir: str, - config: Dict[str, str], speed_factor: float, + model: str, case: str, speed_factor: float, debugger: str, verbose: bool): - super().__init__(log_dir, config, verbose) + super().__init__(log_dir, model, case, verbose) self.name = "px4" self.cmd = workspace_dir + "/build/px4_sitl_default/bin/px4" self.cwd = workspace_dir + "/build/px4_sitl_default/tmp/rootfs" @@ -153,7 +154,7 @@ def __init__(self, workspace_dir: str, log_dir: str, "-d" ] self.env = {"PATH": str(os.environ['PATH']), - "PX4_SIM_MODEL": str(config['model']), + "PX4_SIM_MODEL": self.model, "PX4_SIM_SPEED_FACTOR": str(speed_factor)} self.debugger = debugger @@ -179,10 +180,11 @@ class GzserverRunner(Runner): def __init__(self, workspace_dir: str, log_dir: str, - config: Dict[str, str], + model: str, + case: str, speed_factor: float, verbose: bool): - super().__init__(log_dir, config, verbose) + super().__init__(log_dir, model, case, verbose) self.name = "gzserver" self.cwd = workspace_dir self.env = {"PATH": os.environ['PATH'], @@ -196,16 +198,17 @@ def __init__(self, self.cmd = "gzserver" self.args = ["--verbose", workspace_dir + "/Tools/sitl_gazebo/worlds/" + - config['model'] + ".world"] + self.model + ".world"] class GzclientRunner(Runner): def __init__(self, workspace_dir: str, log_dir: str, - config: Dict[str, str], + model: str, + case: str, verbose: bool): - super().__init__(log_dir, config, verbose) + super().__init__(log_dir, model, case, verbose) self.name = "gzclient" self.cwd = workspace_dir self.env = {"PATH": os.environ['PATH'], @@ -223,14 +226,14 @@ class TestRunner(Runner): def __init__(self, workspace_dir: str, log_dir: str, - config: Dict[str, str], - test: str, + model: str, + case: str, mavlink_connection: str, verbose: bool): - super().__init__(log_dir, config, verbose) + super().__init__(log_dir, model, case, verbose) self.name = "test_runner" self.cwd = workspace_dir self.env = {"PATH": os.environ['PATH']} self.cmd = workspace_dir + \ "/build/px4_sitl_default/mavsdk_tests/mavsdk_tests" - self.args = ["--url", mavlink_connection, test] + self.args = ["--url", mavlink_connection, case] From 47ad85c894b79a9c028e56b0d2235304e4d61134 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 16 Mar 2020 19:26:05 +0100 Subject: [PATCH 33/51] mavsdk_tests: add combined log --- test/mavsdk_tests/mavsdk_test_runner.py | 58 +++++++++++++++++++------ test/mavsdk_tests/process_helper.py | 16 ++----- 2 files changed, 48 insertions(+), 26 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 0ef57e25f164..25c1b9e9511d 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -11,7 +11,7 @@ import sys from logger_helper import color, colorize import process_helper as ph -from typing import Any, Dict, List, NoReturn, Optional +from typing import Any, Dict, List, NoReturn, TextIO, Optional from types import FrameType @@ -129,6 +129,7 @@ def __init__(self, self.gui = gui self.verbose = verbose self.start_time = datetime.datetime.now() + self.combined_log_fd = TextIO @classmethod def determine_tests(cls, tests: List[Dict[str, Any]], model: str) \ @@ -272,7 +273,7 @@ def get_log_dir(self, iteration: int, model: str, case: str) -> str: return foldername def run_test_case(self, test: Dict[str, Any], - case: str, logfile_path: str) -> bool: + case: str, log_dir: str) -> bool: self.active_runners = [] speed_factor = self.speed_factor @@ -282,56 +283,68 @@ def run_test_case(self, test: Dict[str, Any], if self.config['mode'] == 'sitl': px4_runner = ph.Px4Runner( os.getcwd(), - logfile_path, + log_dir, test['model'], case, speed_factor, self.debugger, self.verbose) - px4_runner.start() self.active_runners.append(px4_runner) if self.config['simulator'] == 'gazebo': gzserver_runner = ph.GzserverRunner( os.getcwd(), - logfile_path, + log_dir, test['model'], case, self.speed_factor, self.verbose) - gzserver_runner.start() self.active_runners.append(gzserver_runner) if self.gui: gzclient_runner = ph.GzclientRunner( os.getcwd(), - logfile_path, + log_dir, test['model'], case, self.verbose) - gzclient_runner.start() self.active_runners.append(gzclient_runner) test_runner = ph.TestRunner( os.getcwd(), - logfile_path, + log_dir, test['model'], case, self.config['mavlink_connection'], self.verbose) - test_runner.start() self.active_runners.append(test_runner) + for runner in self.active_runners: + runner.set_log_filename( + self.determine_logfile_path(log_dir, runner.name)) + runner.start() + + max_name = max(len(runner.name) for runner in self.active_runners) + + self.start_combined_log( + self.determine_logfile_path(log_dir, 'combined')) + while test_runner.time_elapsed_s() < test['timeout_min']*60: returncode = test_runner.poll() if returncode is not None: is_success = (returncode == 0) break - if self.verbose: - for runner in self.active_runners: - runner.print_output() + for runner in self.active_runners: + output = runner.get_output() + if not output: + continue + + output = self.add_name_prefix(max_name, runner.name, output) + self.add_to_combined_log(output) + if self.verbose: + print(output) else: print(colorize( @@ -342,6 +355,7 @@ def run_test_case(self, test: Dict[str, Any], for runner in self.active_runners: runner.stop() + self.stop_combined_log() result = {'success': is_success, 'logfiles': [runner.get_log_filename() @@ -354,6 +368,23 @@ def run_test_case(self, test: Dict[str, Any], return is_success + def start_combined_log(self, filename: str) -> None: + self.log_fd = open(filename, 'w') + + def stop_combined_log(self) -> None: + self.log_fd.close() + + def add_to_combined_log(self, output: str) -> None: + self.log_fd.write(output) + + @staticmethod + def add_name_prefix(width: int, name: str, text: str) -> str: + return colorize("[" + name.ljust(width) + "] " + text, color.RESET) + + @staticmethod + def determine_logfile_path(log_dir: str, desc: str) -> str: + return os.path.join(log_dir, "log-{}.log".format(desc)) + def show_detailed_results(self) -> None: print() print(colorize("Results:", color.BOLD)) @@ -416,6 +447,7 @@ def sigint_handler(self, sig: signal.Signals, frame: FrameType) \ print("Stopping all processes ...") for runner in self.active_runners: runner.stop() + self.stop_combined_log() print("Stopping all processes done.") self.show_detailed_results() sys.exit(-sig) diff --git a/test/mavsdk_tests/process_helper.py b/test/mavsdk_tests/process_helper.py index 0e37a0a5edac..a7dfa7054796 100644 --- a/test/mavsdk_tests/process_helper.py +++ b/test/mavsdk_tests/process_helper.py @@ -32,9 +32,8 @@ def __init__(self, self.log_dir = log_dir self.log_filename = "" - def set_log_filename(self) -> None: - self.log_filename = self.log_dir + os.path.sep + \ - "log-{}.log".format(self.name) + def set_log_filename(self, log_filename: str) -> None: + self.log_filename = log_filename def get_log_filename(self) -> str: return self.log_filename @@ -45,7 +44,6 @@ def start(self) -> None: atexit.register(self.stop) - self.set_log_filename() if self.verbose: print("Logging to {}".format(self.log_filename)) self.log_fd = open(self.log_filename, 'w') @@ -88,18 +86,10 @@ def wait(self, timeout_min: float) -> Optional[int]: def get_output(self) -> Optional[str]: try: - output = self.output_queue.get(block=True, timeout=0.1) - return maybe_strip_color(output) + return self.output_queue.get(block=True, timeout=0.1) except queue.Empty: return None - def print_output(self) -> None: - output = self.get_output() - if not output: - return - print(colorize("[" + self.name.ljust(11) + "] " + output, color.RESET), - end="") - def stop(self) -> int: atexit.unregister(self.stop) From 8307d4b36a487bc8b7b5322a90890f48c069ea6f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 17 Mar 2020 12:22:36 +0100 Subject: [PATCH 34/51] mavsdk_tests: improve and fix colors/result --- test/mavsdk_tests/mavsdk_test_runner.py | 48 +++++++++++++------------ 1 file changed, 26 insertions(+), 22 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 25c1b9e9511d..0f95de3da923 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -207,8 +207,8 @@ def prepare_for_results(self) -> None: def run_tests(self) -> None: for iteration in range(self.iterations): if self.iterations > 1: - print("%%% Test iteration: {} / {}". - format(iteration + 1, self.iterations)) + print(colorize("%%% Test iteration: {} / {}". + format(iteration + 1, self.iterations), color.BOLD)) success = self.run_iteration(iteration) if not success: @@ -237,16 +237,13 @@ def run_iteration(self, iteration: int) -> bool: was_success = self.run_test_case(test, case, log_dir) - if was_success: - print(colorize( - "--- Test case {} of {}: '{}' succeeded." - .format(i+1, len(test['cases']), case), - color.GREEN)) - else: - print(colorize( - "--- Test case {} of {}: '{}' failed." - .format(i+1, len(test['cases']), case), - color.RED)) + print("--- Test case {} of {}: '{}' {}." + .format(i+1, + len(test['cases']), + case, + colorize("succeeded", color.GREEN) + if was_success + else colorize("failed", color.RED))) if not was_success: iteration_success = False @@ -409,11 +406,14 @@ def show_detailed_results(self) -> None: notes = [ self.note_if_any( - "succeeded", n_succeeded, self.iterations), + colorize("{}succeeded", color.GREEN), + n_succeeded, self.iterations), self.note_if_any( - "failed", n_failed, self.iterations), + colorize("{}failed", color.RED), + n_failed, self.iterations), self.note_if_any( - "cancelled", n_cancelled, self.iterations)] + colorize("{}cancelled", color.GRAY), + n_cancelled, self.iterations)] notes_without_none = list(filter(None, notes)) print(", ".join(notes_without_none)) @@ -423,23 +423,27 @@ def was_overall_pass(self) -> bool: continue for case in test['cases'].values(): - if case['results'].count(False) > 0: - return False + for result in case['results']: + if result['success'] is not True: + return False return True def show_overall_result(self) -> None: - print("Overall result: {}".format( - "PASS" if self.was_overall_pass() else "FAIL")) + print(colorize( + "Overall result: {}".format( + colorize("PASS", color.GREEN) + if self.was_overall_pass() + else colorize("FAIL", color.RED)), color.BOLD)) @staticmethod - def note_if_any(text: str, n: int, total: int) -> Optional[str]: + def note_if_any(text_to_format: str, n: int, total: int) -> Optional[str]: assert not n < 0 if n == 0: return None elif n == 1 and total == 1: - return text + return text_to_format.format("") else: - return "{} {}".format(n, text) + return text_to_format.format(n) def sigint_handler(self, sig: signal.Signals, frame: FrameType) \ -> NoReturn: From 8f0aca4e4f80b8334c7bd7ead3f961d7a4e14463 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 17 Mar 2020 12:22:58 +0100 Subject: [PATCH 35/51] mavsdk_tests: print error on failure --- test/mavsdk_tests/mavsdk_test_runner.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 0f95de3da923..d1e4f5623c03 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -323,9 +323,9 @@ def run_test_case(self, test: Dict[str, Any], runner.start() max_name = max(len(runner.name) for runner in self.active_runners) + logfile_path = self.determine_logfile_path(log_dir, 'combined') - self.start_combined_log( - self.determine_logfile_path(log_dir, 'combined')) + self.start_combined_log(logfile_path) while test_runner.time_elapsed_s() < test['timeout_min']*60: returncode = test_runner.poll() @@ -360,9 +360,7 @@ def run_test_case(self, test: Dict[str, Any], test['cases'][case]['results'].append(result) if not is_success: - # TODO: print error and logfiles - pass - + print(self.get_combined_log(logfile_path)) return is_success def start_combined_log(self, filename: str) -> None: @@ -374,6 +372,10 @@ def stop_combined_log(self) -> None: def add_to_combined_log(self, output: str) -> None: self.log_fd.write(output) + def get_combined_log(self, filename: str) -> str: + with open(filename, 'r') as f: + return f.read() + @staticmethod def add_name_prefix(width: int, name: str, text: str) -> str: return colorize("[" + name.ljust(width) + "] " + text, color.RESET) From 385974606939859883c5b8dde9cae4e09a957b09 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 17 Mar 2020 12:33:01 +0100 Subject: [PATCH 36/51] mavsdk_tests: print where to find logfiles --- test/mavsdk_tests/mavsdk_test_runner.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index d1e4f5623c03..2b21133f62ff 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -361,6 +361,10 @@ def run_test_case(self, test: Dict[str, Any], if not is_success: print(self.get_combined_log(logfile_path)) + print("Logfiles: ") + print(" - {}".format(logfile_path)) + for runner in self.active_runners: + print(" - {}".format(runner.get_log_filename())) return is_success def start_combined_log(self, filename: str) -> None: From a0119964b69fdc3414f8a9009d1a2dbbf7847ec0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 17 Mar 2020 12:33:18 +0100 Subject: [PATCH 37/51] mavsdk_tests: fix missing space after number --- test/mavsdk_tests/mavsdk_test_runner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 2b21133f62ff..6e615dda4403 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -449,7 +449,7 @@ def note_if_any(text_to_format: str, n: int, total: int) -> Optional[str]: elif n == 1 and total == 1: return text_to_format.format("") else: - return text_to_format.format(n) + return text_to_format.format(str(n) + " ") def sigint_handler(self, sig: signal.Signals, frame: FrameType) \ -> NoReturn: From d892d7041ed1f90f53910114b005433bbf13b1b0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 17 Mar 2020 15:51:41 +0100 Subject: [PATCH 38/51] mavsdk_tests: add filter for cases --- test/mavsdk_tests/mavsdk_test_runner.py | 209 +++++++++++++++--------- 1 file changed, 133 insertions(+), 76 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 6e615dda4403..03ad2e837a1c 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -29,7 +29,9 @@ def main() -> NoReturn: parser.add_argument("--gui", default=False, action='store_true', help="Display gzclient with simulation") parser.add_argument("--model", type=str, default='all', - help="Specify which model to run") + help="Only run tests for one model") + parser.add_argument("--case", type=str, default='all', + help="Only run tests for one case") parser.add_argument("--debugger", default="", help="valgrind callgrind gdb lldb") parser.add_argument("--verbose", default=False, action='store_true', @@ -58,6 +60,7 @@ def main() -> NoReturn: args.abort_early, args.speed_factor, args.model, + args.case, args.debugger, args.log_dir, args.gui, @@ -114,6 +117,7 @@ def __init__(self, abort_early: bool, speed_factor: float, model: str, + case: str, debugger: str, log_dir: str, gui: bool, @@ -122,7 +126,7 @@ def __init__(self, self.active_runners: List[ph.Runner] self.iterations = iterations self.abort_early = abort_early - self.tests = self.determine_tests(config['tests'], model) + self.tests = self.determine_tests(config['tests'], model, case) self.speed_factor = speed_factor self.debugger = debugger self.log_dir = log_dir @@ -132,17 +136,23 @@ def __init__(self, self.combined_log_fd = TextIO @classmethod - def determine_tests(cls, tests: List[Dict[str, Any]], model: str) \ - -> List[Dict[str, Any]]: + def determine_tests(cls, + tests: List[Dict[str, Any]], + model: str, + case: str) -> List[Dict[str, Any]]: for test in tests: - test['selected'] = (model == 'all') or model == test['model'] + test['selected'] = (model == 'all' or model == test['model']) test['cases'] = dict.fromkeys( - cls.determine_test_cases(test['test_filter'])) + cls.query_test_cases(test['test_filter'])) + for key in test['cases'].keys(): + test['cases'][key] = { + 'selected': (test['selected'] and + (case == 'all' or case == key))} return tests @staticmethod - def determine_test_cases(filter: str) -> List[str]: + def query_test_cases(filter: str) -> List[str]: cmd = os.getcwd() + "/build/px4_sitl_default/mavsdk_tests/mavsdk_tests" args = ["--list-test-names-only", filter] p = subprocess.Popen( @@ -170,19 +180,12 @@ def run(self) -> bool: return self.was_overall_pass() def show_plans(self) -> None: - n_models = sum(map( - lambda test: 1 if test['selected'] else 0, - self.tests)) - n_cases = sum(map( - lambda test: len(test['cases']) if test['selected'] else 0, - self.tests)) - print() print(colorize( "About to run {} test case{} for {} selected model{} " "({} iteration{}):". - format(n_cases, self.plural_s(n_cases), - n_models, self.plural_s(n_models), + format(self.num_cases(), self.plural_s(self.num_cases()), + self.num_models(), self.plural_s(self.num_models()), self.iterations, self.plural_s(self.iterations)), color.BOLD)) @@ -193,16 +196,44 @@ def show_plans(self) -> None: color.BOLD), color.GRAY)) continue print(colorize(" - {}:".format(test['model']), color.BOLD)) - for case in test['cases']: - print(" - '{}'".format(case)) + for key, case in test['cases'].items(): + if case['selected']: + print(" - '{}'".format(key)) + else: + print(colorize(" - '{}' (not selected)".format(key), + color.GRAY)) print() + def num_models(self) -> int: + return sum(map( + lambda test: 1 if test['selected'] else 0, + self.tests)) + + def num_cases(self) -> int: + n_cases = 0 + for test in self.tests: + if not test['selected']: + continue + n_cases += self.num_cases_for_test(test) + + return n_cases + + def num_cases_for_test(self, test: Dict[str, Any]) -> int: + n_cases = 0 + for case in test['cases'].values(): + if not case['selected']: + continue + n_cases += 1 + return n_cases + def prepare_for_results(self) -> None: for test in self.tests: if not test['selected']: continue - for key in test['cases'].keys(): - test['cases'][key] = {'results': []} + for key, value in test['cases'].items(): + if not value['selected']: + continue + value['results'] = [] def run_tests(self) -> None: for iteration in range(self.iterations): @@ -225,22 +256,28 @@ def run_iteration(self, iteration: int) -> bool: "==> Running tests for {}".format(test['model']), color.BOLD)) - for i, case in enumerate(test['cases']): + test_i = 0 + for key, case_value in test['cases'].items(): + if not case_value['selected']: + continue + print("--> Test case {} of {}: '{}' running ...". - format(i+1, len(test['cases']), case)) + format(test_i+1, + self.num_cases_for_test(test), + key)) - log_dir = self.get_log_dir(iteration, test['model'], case) + log_dir = self.get_log_dir(iteration, test['model'], key) if self.verbose: print("creating log directory: {}" .format(log_dir)) os.makedirs(log_dir, exist_ok=True) - was_success = self.run_test_case(test, case, log_dir) + was_success = self.run_test_case(test, key, log_dir) print("--- Test case {} of {}: '{}' {}." - .format(i+1, - len(test['cases']), - case, + .format(test_i+1, + self.num_cases_for_test(test), + key, colorize("succeeded", color.GREEN) if was_success else colorize("failed", color.RED))) @@ -252,6 +289,8 @@ def run_iteration(self, iteration: int) -> bool: print("Aborting early") return False + test_i += 1 + return iteration_success def get_log_dir(self, iteration: int, model: str, case: str) -> str: @@ -269,21 +308,65 @@ def get_log_dir(self, iteration: int, model: str, case: str) -> str: return foldername - def run_test_case(self, test: Dict[str, Any], - case: str, log_dir: str) -> bool: - self.active_runners = [] - + def get_max_speed_factor(self, test: Dict[str, Any]) -> float: speed_factor = self.speed_factor if "max_speed_factor" in test: speed_factor = min(float(speed_factor), test["max_speed_factor"]) + return speed_factor + + def run_test_case(self, test: Dict[str, Any], + case: str, log_dir: str) -> bool: + + self.start_runners(log_dir, test, case) + + logfile_path = self.determine_logfile_path(log_dir, 'combined') + self.start_combined_log(logfile_path) + + test_timeout_s = test['timeout_min']*60 + while self.active_runners[-1].time_elapsed_s() < test_timeout_s: + returncode = self.active_runners[-1].poll() + if returncode is not None: + is_success = (returncode == 0) + break + + self.collect_runner_output() + + else: + print(colorize( + "Test timeout of {} mins triggered!". + format(test['timeout_min']), + color.BOLD)) + is_success = False + + self.stop_runners() + self.stop_combined_log() + + result = {'success': is_success, + 'logfiles': [runner.get_log_filename() + for runner in self.active_runners]} + test['cases'][case]['results'].append(result) + if not is_success: + if not self.verbose: + print(self.get_combined_log(logfile_path)) + print("Logfiles: ") + print(" - {}".format(logfile_path)) + for runner in self.active_runners: + print(" - {}".format(runner.get_log_filename())) + return is_success + + def start_runners(self, + log_dir: str, + test: Dict[str, Any], + case: str) -> None: + self.active_runners = [] if self.config['mode'] == 'sitl': px4_runner = ph.Px4Runner( os.getcwd(), log_dir, test['model'], case, - speed_factor, + self.get_max_speed_factor(test), self.debugger, self.verbose) self.active_runners.append(px4_runner) @@ -294,7 +377,7 @@ def run_test_case(self, test: Dict[str, Any], log_dir, test['model'], case, - self.speed_factor, + self.get_max_speed_factor(test), self.verbose) self.active_runners.append(gzserver_runner) @@ -314,7 +397,6 @@ def run_test_case(self, test: Dict[str, Any], case, self.config['mavlink_connection'], self.verbose) - self.active_runners.append(test_runner) for runner in self.active_runners: @@ -322,50 +404,22 @@ def run_test_case(self, test: Dict[str, Any], self.determine_logfile_path(log_dir, runner.name)) runner.start() - max_name = max(len(runner.name) for runner in self.active_runners) - logfile_path = self.determine_logfile_path(log_dir, 'combined') - - self.start_combined_log(logfile_path) - - while test_runner.time_elapsed_s() < test['timeout_min']*60: - returncode = test_runner.poll() - if returncode is not None: - is_success = (returncode == 0) - break - - for runner in self.active_runners: - output = runner.get_output() - if not output: - continue - - output = self.add_name_prefix(max_name, runner.name, output) - self.add_to_combined_log(output) - if self.verbose: - print(output) - - else: - print(colorize( - "Test timeout of {} mins triggered!". - format(test['timeout_min']), - color.BOLD)) - is_success = False - + def stop_runners(self) -> None: for runner in self.active_runners: runner.stop() - self.stop_combined_log() - result = {'success': is_success, - 'logfiles': [runner.get_log_filename() - for runner in self.active_runners]} - test['cases'][case]['results'].append(result) + def collect_runner_output(self) -> None: + max_name = max(len(runner.name) for runner in self.active_runners) - if not is_success: - print(self.get_combined_log(logfile_path)) - print("Logfiles: ") - print(" - {}".format(logfile_path)) - for runner in self.active_runners: - print(" - {}".format(runner.get_log_filename())) - return is_success + for runner in self.active_runners: + output = runner.get_output() + if not output: + continue + + output = self.add_name_prefix(max_name, runner.name, output) + self.add_to_combined_log(output) + if self.verbose: + print(output, end="") def start_combined_log(self, filename: str) -> None: self.log_fd = open(filename, 'w') @@ -402,6 +456,8 @@ def show_detailed_results(self) -> None: print(colorize(" - {}:".format(test['model']), color.BOLD)) for name, case in test['cases'].items(): + if not case['selected']: + continue print(" - '{}': ".format(name), end="") n_succeeded = [result['success'] @@ -429,6 +485,8 @@ def was_overall_pass(self) -> bool: continue for case in test['cases'].values(): + if not case['selected']: + continue for result in case['results']: if result['success'] is not True: return False @@ -455,8 +513,7 @@ def sigint_handler(self, sig: signal.Signals, frame: FrameType) \ -> NoReturn: print("Received SIGINT") print("Stopping all processes ...") - for runner in self.active_runners: - runner.stop() + self.stop_runners() self.stop_combined_log() print("Stopping all processes done.") self.show_detailed_results() From 0f3454bdfe4a99ce84926cb87917b2d891e1e7b8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 17 Mar 2020 16:15:12 +0100 Subject: [PATCH 39/51] Tools: check style of mavsdk_tests files --- Tools/astyle/files_to_check_code_style.sh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index 3dc1582aec37..7acf30112982 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -7,7 +7,7 @@ if [ $# -gt 0 ]; then PATTERN="$1" fi -exec find boards msg src platforms \ +exec find boards msg src platforms test \ -path msg/templates/urtps -prune -o \ -path platforms/nuttx/NuttX -prune -o \ -path platforms/qurt/dspal -prune -o \ @@ -18,4 +18,5 @@ exec find boards msg src platforms \ -path src/lib/systemlib/uthash -prune -o \ -path src/modules/micrortps_bridge/micro-CDR -prune -o \ -path src/modules/micrortps_bridge/microRTPS_client -prune -o \ + -path test/mavsdk_tests/catch2 -prune -o \ -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN From e817992fb20f4438775015c4505a67bb7022ba85 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 17 Mar 2020 16:15:34 +0100 Subject: [PATCH 40/51] mavsdk_tests: add header and fix style --- test/mavsdk_tests/autopilot_tester.cpp | 269 ++++++++++-------- test/mavsdk_tests/autopilot_tester.h | 151 ++++++---- test/mavsdk_tests/test_main.cpp | 120 +++++--- .../mavsdk_tests/test_mission_multicopter.cpp | 94 +++--- test/mavsdk_tests/test_mission_vtol.cpp | 54 +++- .../test_multicopter_offboard.cpp | 90 ++++-- 6 files changed, 484 insertions(+), 294 deletions(-) diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 012037f5a9bf..5b39c9fbf76a 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -1,3 +1,36 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + #include "autopilot_tester.h" #include #include @@ -6,225 +39,229 @@ std::string connection_url {"udp://"}; void AutopilotTester::connect(const std::string uri) { - ConnectionResult ret = _mavsdk.add_any_connection(uri); - REQUIRE(ret == ConnectionResult::SUCCESS); + ConnectionResult ret = _mavsdk.add_any_connection(uri); + REQUIRE(ret == ConnectionResult::SUCCESS); - std::cout << "Waiting for system connect" << std::endl; - REQUIRE(poll_condition_with_timeout( - [this]() { return _mavsdk.is_connected(); }, std::chrono::seconds(25))); + std::cout << "Waiting for system connect" << std::endl; + REQUIRE(poll_condition_with_timeout( + [this]() { return _mavsdk.is_connected(); }, std::chrono::seconds(25))); - auto& system = _mavsdk.system(); + auto &system = _mavsdk.system(); - _telemetry.reset(new Telemetry(system)); - _action.reset(new Action(system)); - _mission.reset(new Mission(system)); - _offboard.reset(new Offboard(system)); + _telemetry.reset(new Telemetry(system)); + _action.reset(new Action(system)); + _mission.reset(new Mission(system)); + _offboard.reset(new Offboard(system)); } void AutopilotTester::wait_until_ready() { - std::cout << "Waiting for system to be ready" << std::endl; - CHECK(poll_condition_with_timeout( - [this]() { return _telemetry->health_all_ok(); }, std::chrono::seconds(20))); + std::cout << "Waiting for system to be ready" << std::endl; + CHECK(poll_condition_with_timeout( + [this]() { return _telemetry->health_all_ok(); }, std::chrono::seconds(20))); } void AutopilotTester::wait_until_ready_local_position_only() { - std::cout << "Waiting for system to be ready" << std::endl; - CHECK(poll_condition_with_timeout( - [this]() { - return - (_telemetry->health().gyrometer_calibration_ok && - _telemetry->health().accelerometer_calibration_ok && - _telemetry->health().magnetometer_calibration_ok && - _telemetry->health().level_calibration_ok && - _telemetry->health().local_position_ok); + std::cout << "Waiting for system to be ready" << std::endl; + CHECK(poll_condition_with_timeout( + [this]() { + return + (_telemetry->health().gyrometer_calibration_ok && + _telemetry->health().accelerometer_calibration_ok && + _telemetry->health().magnetometer_calibration_ok && + _telemetry->health().level_calibration_ok && + _telemetry->health().local_position_ok); }, std::chrono::seconds(20))); } void AutopilotTester::store_home() { - request_ground_truth(); - _home = get_ground_truth_position(); + request_ground_truth(); + _home = get_ground_truth_position(); } void AutopilotTester::check_home_within(float acceptance_radius_m) { - CHECK(ground_truth_horizontal_position_close_to(_home, acceptance_radius_m)); + CHECK(ground_truth_horizontal_position_close_to(_home, acceptance_radius_m)); } void AutopilotTester::set_takeoff_altitude(const float altitude_m) { - CHECK(Action::Result::SUCCESS == _action->set_takeoff_altitude(altitude_m)); - const auto result = _action->get_takeoff_altitude(); - CHECK(result.first == Action::Result::SUCCESS); - CHECK(result.second == Approx(altitude_m)); + CHECK(Action::Result::SUCCESS == _action->set_takeoff_altitude(altitude_m)); + const auto result = _action->get_takeoff_altitude(); + CHECK(result.first == Action::Result::SUCCESS); + CHECK(result.second == Approx(altitude_m)); } void AutopilotTester::arm() { - const auto result = _action->arm(); - REQUIRE(result == Action::Result::SUCCESS); + const auto result = _action->arm(); + REQUIRE(result == Action::Result::SUCCESS); } void AutopilotTester::takeoff() { - const auto result = _action->takeoff(); - REQUIRE(result == Action::Result::SUCCESS); + const auto result = _action->takeoff(); + REQUIRE(result == Action::Result::SUCCESS); } void AutopilotTester::land() { - const auto result = _action->land(); - REQUIRE(result == Action::Result::SUCCESS); + const auto result = _action->land(); + REQUIRE(result == Action::Result::SUCCESS); } void AutopilotTester::transition_to_fixedwing() { - const auto result = _action->transition_to_fixedwing(); - REQUIRE(result == Action::Result::SUCCESS); + const auto result = _action->transition_to_fixedwing(); + REQUIRE(result == Action::Result::SUCCESS); } void AutopilotTester::transition_to_multicopter() { - const auto result = _action->transition_to_multicopter(); - REQUIRE(result == Action::Result::SUCCESS); + const auto result = _action->transition_to_multicopter(); + REQUIRE(result == Action::Result::SUCCESS); } void AutopilotTester::wait_until_disarmed() { - REQUIRE(poll_condition_with_timeout( - [this]() { return !_telemetry->armed(); }, std::chrono::seconds(60))); + REQUIRE(poll_condition_with_timeout( + [this]() { return !_telemetry->armed(); }, std::chrono::seconds(60))); } void AutopilotTester::wait_until_hovering() { - REQUIRE(poll_condition_with_timeout( - [this]() { return _telemetry->landed_state() == Telemetry::LandedState::IN_AIR; }, std::chrono::seconds(20))); + REQUIRE(poll_condition_with_timeout( + [this]() { return _telemetry->landed_state() == Telemetry::LandedState::IN_AIR; }, std::chrono::seconds(20))); } void AutopilotTester::prepare_square_mission(MissionOptions mission_options) { - const auto ct = get_coordinate_transformation(); + const auto ct = get_coordinate_transformation(); - std::vector> mission_items {}; - mission_items.push_back(create_mission_item({mission_options.leg_length_m, 0.}, mission_options, ct)); - mission_items.push_back(create_mission_item({mission_options.leg_length_m, mission_options.leg_length_m}, mission_options, ct)); - mission_items.push_back(create_mission_item({0., mission_options.leg_length_m}, mission_options, ct)); + std::vector> mission_items {}; + mission_items.push_back(create_mission_item({mission_options.leg_length_m, 0.}, mission_options, ct)); + mission_items.push_back(create_mission_item({mission_options.leg_length_m, mission_options.leg_length_m}, + mission_options, ct)); + mission_items.push_back(create_mission_item({0., mission_options.leg_length_m}, mission_options, ct)); - _mission->set_return_to_launch_after_mission(mission_options.rtl_at_end); + _mission->set_return_to_launch_after_mission(mission_options.rtl_at_end); - std::promise prom; - auto fut = prom.get_future(); + std::promise prom; + auto fut = prom.get_future(); - _mission->upload_mission_async(mission_items, [&prom](Mission::Result result) { - REQUIRE(Mission::Result::SUCCESS == result); - prom.set_value(); - }); + _mission->upload_mission_async(mission_items, [&prom](Mission::Result result) { + REQUIRE(Mission::Result::SUCCESS == result); + prom.set_value(); + }); - REQUIRE(fut.wait_for(std::chrono::seconds(2)) == std::future_status::ready); + REQUIRE(fut.wait_for(std::chrono::seconds(2)) == std::future_status::ready); } void AutopilotTester::execute_mission() { - std::promise prom; - auto fut = prom.get_future(); + std::promise prom; + auto fut = prom.get_future(); - _mission->start_mission_async([&prom](Mission::Result result) { - REQUIRE(Mission::Result::SUCCESS == result); - prom.set_value(); - }); + _mission->start_mission_async([&prom](Mission::Result result) { + REQUIRE(Mission::Result::SUCCESS == result); + prom.set_value(); + }); - // TODO: Adapt time limit based on mission size, flight speed, sim speed factor, etc. + // TODO: Adapt time limit based on mission size, flight speed, sim speed factor, etc. - REQUIRE(poll_condition_with_timeout( - [this]() { return _mission->mission_finished(); }, std::chrono::seconds(60))); + REQUIRE(poll_condition_with_timeout( + [this]() { return _mission->mission_finished(); }, std::chrono::seconds(60))); - REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready); + REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready); } CoordinateTransformation AutopilotTester::get_coordinate_transformation() { - const auto home = _telemetry->home_position(); - CHECK(std::isfinite(home.latitude_deg)); - CHECK(std::isfinite(home.longitude_deg)); - return CoordinateTransformation({home.latitude_deg, home.longitude_deg}); + const auto home = _telemetry->home_position(); + CHECK(std::isfinite(home.latitude_deg)); + CHECK(std::isfinite(home.longitude_deg)); + return CoordinateTransformation({home.latitude_deg, home.longitude_deg}); } std::shared_ptr AutopilotTester::create_mission_item( - const CoordinateTransformation::LocalCoordinate& local_coordinate, - const MissionOptions& mission_options, - const CoordinateTransformation& ct) + const CoordinateTransformation::LocalCoordinate &local_coordinate, + const MissionOptions &mission_options, + const CoordinateTransformation &ct) { - auto mission_item = std::make_shared(); - const auto pos_north = ct.global_from_local(local_coordinate); - mission_item->set_position(pos_north.latitude_deg, pos_north.longitude_deg); - mission_item->set_relative_altitude(mission_options.relative_altitude_m); - return mission_item; + auto mission_item = std::make_shared(); + const auto pos_north = ct.global_from_local(local_coordinate); + mission_item->set_position(pos_north.latitude_deg, pos_north.longitude_deg); + mission_item->set_relative_altitude(mission_options.relative_altitude_m); + return mission_item; } void AutopilotTester::execute_rtl() { - REQUIRE(Action::Result::SUCCESS == _action->return_to_launch()); + REQUIRE(Action::Result::SUCCESS == _action->return_to_launch()); } -void AutopilotTester::offboard_goto(const Offboard::PositionNEDYaw& target, float acceptance_radius_m, std::chrono::seconds timeout_duration) +void AutopilotTester::offboard_goto(const Offboard::PositionNEDYaw &target, float acceptance_radius_m, + std::chrono::seconds timeout_duration) { - _offboard->set_position_ned(target); - REQUIRE(_offboard->start() == Offboard::Result::SUCCESS); - CHECK(poll_condition_with_timeout( - [=]() { return estimated_position_close_to(target, acceptance_radius_m); }, timeout_duration)); - std::cout << "Target position reached" << std::endl; + _offboard->set_position_ned(target); + REQUIRE(_offboard->start() == Offboard::Result::SUCCESS); + CHECK(poll_condition_with_timeout( + [ = ]() { return estimated_position_close_to(target, acceptance_radius_m); }, timeout_duration)); + std::cout << "Target position reached" << std::endl; } void AutopilotTester::offboard_land() { - Offboard::VelocityNEDYaw land_velocity; - land_velocity.north_m_s = 0.0f; - land_velocity.east_m_s = 0.0f; - land_velocity.down_m_s = 1.0f; - land_velocity.yaw_deg = 0.0f; - _offboard->set_velocity_ned(land_velocity); + Offboard::VelocityNEDYaw land_velocity; + land_velocity.north_m_s = 0.0f; + land_velocity.east_m_s = 0.0f; + land_velocity.down_m_s = 1.0f; + land_velocity.yaw_deg = 0.0f; + _offboard->set_velocity_ned(land_velocity); } -bool AutopilotTester::estimated_position_close_to(const Offboard::PositionNEDYaw& target_pos, float acceptance_radius_m) +bool AutopilotTester::estimated_position_close_to(const Offboard::PositionNEDYaw &target_pos, float acceptance_radius_m) { - Telemetry::PositionNED est_pos = _telemetry->position_velocity_ned().position; - return sq(est_pos.north_m - target_pos.north_m) + - sq(est_pos.east_m - target_pos.east_m) + - sq(est_pos.down_m - target_pos.down_m) < sq(acceptance_radius_m); + Telemetry::PositionNED est_pos = _telemetry->position_velocity_ned().position; + return sq(est_pos.north_m - target_pos.north_m) + + sq(est_pos.east_m - target_pos.east_m) + + sq(est_pos.down_m - target_pos.down_m) < sq(acceptance_radius_m); } -bool AutopilotTester::estimated_horizontal_position_close_to(const Offboard::PositionNEDYaw& target_pos, float acceptance_radius_m) +bool AutopilotTester::estimated_horizontal_position_close_to(const Offboard::PositionNEDYaw &target_pos, + float acceptance_radius_m) { - Telemetry::PositionNED est_pos = _telemetry->position_velocity_ned().position; - return sq(est_pos.north_m - target_pos.north_m) + - sq(est_pos.east_m - target_pos.east_m) < sq(acceptance_radius_m); + Telemetry::PositionNED est_pos = _telemetry->position_velocity_ned().position; + return sq(est_pos.north_m - target_pos.north_m) + + sq(est_pos.east_m - target_pos.east_m) < sq(acceptance_radius_m); } void AutopilotTester::request_ground_truth() { - CHECK(_telemetry->set_rate_ground_truth(15) == Telemetry::Result::SUCCESS); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + CHECK(_telemetry->set_rate_ground_truth(15) == Telemetry::Result::SUCCESS); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } Telemetry::GroundTruth AutopilotTester::get_ground_truth_position() { - return _telemetry->ground_truth(); + return _telemetry->ground_truth(); } -bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth& target_pos, float acceptance_radius_m) +bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos, + float acceptance_radius_m) { - CHECK(std::isfinite(target_pos.latitude_deg)); - CHECK(std::isfinite(target_pos.longitude_deg)); - using GlobalCoordinate = CoordinateTransformation::GlobalCoordinate; - using LocalCoordinate = CoordinateTransformation::LocalCoordinate; - CoordinateTransformation ct(GlobalCoordinate{target_pos.latitude_deg, target_pos.longitude_deg}); - - Telemetry::GroundTruth current_pos = _telemetry->ground_truth(); - CHECK(std::isfinite(current_pos.latitude_deg)); - CHECK(std::isfinite(current_pos.longitude_deg)); - LocalCoordinate local_pos= ct.local_from_global(GlobalCoordinate{current_pos.latitude_deg, current_pos.longitude_deg}); - - return sq(local_pos.north_m) + sq(local_pos.east_m) < sq(acceptance_radius_m); + CHECK(std::isfinite(target_pos.latitude_deg)); + CHECK(std::isfinite(target_pos.longitude_deg)); + using GlobalCoordinate = CoordinateTransformation::GlobalCoordinate; + using LocalCoordinate = CoordinateTransformation::LocalCoordinate; + CoordinateTransformation ct(GlobalCoordinate{target_pos.latitude_deg, target_pos.longitude_deg}); + + Telemetry::GroundTruth current_pos = _telemetry->ground_truth(); + CHECK(std::isfinite(current_pos.latitude_deg)); + CHECK(std::isfinite(current_pos.longitude_deg)); + LocalCoordinate local_pos = ct.local_from_global(GlobalCoordinate{current_pos.latitude_deg, current_pos.longitude_deg}); + + return sq(local_pos.north_m) + sq(local_pos.east_m) < sq(acceptance_radius_m); } diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 3762eb0a0cb3..d520071e9e34 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -1,3 +1,36 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + #pragma once #include @@ -16,72 +49,76 @@ extern std::string connection_url; using namespace mavsdk; using namespace mavsdk::geometry; -class AutopilotTester { +class AutopilotTester +{ public: - struct MissionOptions { - double leg_length_m {20.0}; - double relative_altitude_m {10.0}; - bool rtl_at_end {false}; - }; - - void connect(const std::string uri); - void wait_until_ready(); - void wait_until_ready_local_position_only(); - void store_home(); - void check_home_within(float acceptance_radius_m); - void set_takeoff_altitude(const float altitude_m); - void arm(); - void takeoff(); - void land(); - void transition_to_fixedwing(); - void transition_to_multicopter(); - void wait_until_disarmed(); - void wait_until_hovering(); - void prepare_square_mission(MissionOptions mission_options); - void execute_mission(); - void execute_rtl(); - void offboard_goto(const Offboard::PositionNEDYaw& target, float acceptance_radius_m = 0.3f, - std::chrono::seconds timeout_duration = std::chrono::seconds(60)); - void offboard_land(); - void request_ground_truth(); + struct MissionOptions { + double leg_length_m {20.0}; + double relative_altitude_m {10.0}; + bool rtl_at_end {false}; + }; + + void connect(const std::string uri); + void wait_until_ready(); + void wait_until_ready_local_position_only(); + void store_home(); + void check_home_within(float acceptance_radius_m); + void set_takeoff_altitude(const float altitude_m); + void arm(); + void takeoff(); + void land(); + void transition_to_fixedwing(); + void transition_to_multicopter(); + void wait_until_disarmed(); + void wait_until_hovering(); + void prepare_square_mission(MissionOptions mission_options); + void execute_mission(); + void execute_rtl(); + void offboard_goto(const Offboard::PositionNEDYaw &target, float acceptance_radius_m = 0.3f, + std::chrono::seconds timeout_duration = std::chrono::seconds(60)); + void offboard_land(); + void request_ground_truth(); private: - mavsdk::geometry::CoordinateTransformation get_coordinate_transformation(); - std::shared_ptr create_mission_item( - const mavsdk::geometry::CoordinateTransformation::LocalCoordinate& local_coordinate, - const MissionOptions& mission_options, - const mavsdk::geometry::CoordinateTransformation& ct); - Telemetry::GroundTruth get_ground_truth_position(); - - bool ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth& target_pos, float acceptance_radius_m); - bool estimated_position_close_to(const Offboard::PositionNEDYaw& target_position, float acceptance_radius_m); - bool estimated_horizontal_position_close_to(const Offboard::PositionNEDYaw& target_pos, float acceptance_radius_m); - - mavsdk::Mavsdk _mavsdk{}; - std::unique_ptr _telemetry{}; - std::unique_ptr _action{}; - std::unique_ptr _mission{}; - std::unique_ptr _offboard{}; - - Telemetry::GroundTruth _home{NAN, NAN, NAN}; + mavsdk::geometry::CoordinateTransformation get_coordinate_transformation(); + std::shared_ptr create_mission_item( + const mavsdk::geometry::CoordinateTransformation::LocalCoordinate &local_coordinate, + const MissionOptions &mission_options, + const mavsdk::geometry::CoordinateTransformation &ct); + Telemetry::GroundTruth get_ground_truth_position(); + + bool ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos, float acceptance_radius_m); + bool estimated_position_close_to(const Offboard::PositionNEDYaw &target_position, float acceptance_radius_m); + bool estimated_horizontal_position_close_to(const Offboard::PositionNEDYaw &target_pos, float acceptance_radius_m); + + mavsdk::Mavsdk _mavsdk{}; + std::unique_ptr _telemetry{}; + std::unique_ptr _action{}; + std::unique_ptr _mission{}; + std::unique_ptr _offboard{}; + + Telemetry::GroundTruth _home{NAN, NAN, NAN}; }; template bool poll_condition_with_timeout( - std::function fun, std::chrono::duration duration) + std::function fun, std::chrono::duration duration) { - // We need millisecond resolution for sleeping. - const std::chrono::milliseconds duration_ms(duration); - - unsigned iteration = 0; - while (!fun()) { - std::this_thread::sleep_for(duration_ms / 10); - if (iteration++ >= 10) { - return false; - } - } - return true; + // We need millisecond resolution for sleeping. + const std::chrono::milliseconds duration_ms(duration); + + unsigned iteration = 0; + + while (!fun()) { + std::this_thread::sleep_for(duration_ms / 10); + + if (iteration++ >= 10) { + return false; + } + } + + return true; } inline float sq(float x) { return x * x; }; diff --git a/test/mavsdk_tests/test_main.cpp b/test/mavsdk_tests/test_main.cpp index 65753cef115d..26c0f8ec5fc5 100644 --- a/test/mavsdk_tests/test_main.cpp +++ b/test/mavsdk_tests/test_main.cpp @@ -1,7 +1,35 @@ -// -// Multicopter mission test. -// -// Author: Julian Oes +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #define CATCH_CONFIG_RUNNER #include "catch2/catch.hpp" @@ -12,55 +40,59 @@ #include "autopilot_tester.h" -static void usage(const std::string& bin_name); -static void remove_argv(int& argc, char** argv, int pos); +static void usage(const std::string &bin_name); +static void remove_argv(int &argc, char **argv, int pos); -int main(int argc, char** argv) +int main(int argc, char **argv) { - for (int i = 0; i < argc; ++i) { - const std::string argv_string(argv[i]); + for (int i = 0; i < argc; ++i) { + const std::string argv_string(argv[i]); - if (argv_string == "-h") { - usage(argv[0]); - } + if (argv_string == "-h") { + usage(argv[0]); + } - if (argv_string == "--url") { - if (argc > i + 1) { - connection_url = argv[i+1]; - remove_argv(argc, argv, i); - remove_argv(argc, argv, i); - } else { - std::cerr << "No connection URL supplied" << std::endl; - usage(argv[0]); - return -1; - } - } - } + if (argv_string == "--url") { + if (argc > i + 1) { + connection_url = argv[i + 1]; + remove_argv(argc, argv, i); + remove_argv(argc, argv, i); - Catch::Session session; - const int catch_ret = session.applyCommandLine(argc, argv); - if (catch_ret != 0) { - return catch_ret; - } - return session.run(); + } else { + std::cerr << "No connection URL supplied" << std::endl; + usage(argv[0]); + return -1; + } + } + } + + Catch::Session session; + const int catch_ret = session.applyCommandLine(argc, argv); + + if (catch_ret != 0) { + return catch_ret; + } + + return session.run(); } -void usage(const std::string& bin_name) +void usage(const std::string &bin_name) { - std::cout << std::endl - << "Usage : " << bin_name << " [--url CONNECTION_URL] [catch2 arguments]" << std::endl - << "Connection URL format should be :" << std::endl - << " For TCP : tcp://[server_host][:server_port]" << std::endl - << " For UDP : udp://[bind_host][:bind_port]" << std::endl - << " For Serial : serial:///path/to/serial/dev[:baudrate]" << std::endl - << "For example, to connect to the simulator use URL: udp://:14540" << std::endl - << std::endl; + std::cout << std::endl + << "Usage : " << bin_name << " [--url CONNECTION_URL] [catch2 arguments]" << std::endl + << "Connection URL format should be :" << std::endl + << " For TCP : tcp://[server_host][:server_port]" << std::endl + << " For UDP : udp://[bind_host][:bind_port]" << std::endl + << " For Serial : serial:///path/to/serial/dev[:baudrate]" << std::endl + << "For example, to connect to the simulator use URL: udp://:14540" << std::endl + << std::endl; } -void remove_argv(int& argc, char** argv, int pos) +void remove_argv(int &argc, char **argv, int pos) { - for (int i = pos; i+1 < argc; ++i) { - argv[i] = argv[i+1]; - } - argv[--argc] = nullptr; + for (int i = pos; i + 1 < argc; ++i) { + argv[i] = argv[i + 1]; + } + + argv[--argc] = nullptr; } diff --git a/test/mavsdk_tests/test_mission_multicopter.cpp b/test/mavsdk_tests/test_mission_multicopter.cpp index 435d24fc10b2..c94a86bb1eca 100644 --- a/test/mavsdk_tests/test_mission_multicopter.cpp +++ b/test/mavsdk_tests/test_mission_multicopter.cpp @@ -1,7 +1,35 @@ -// -// Multicopter mission test. -// -// Author: Julian Oes +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #include #include @@ -13,39 +41,39 @@ TEST_CASE("Takeoff and Land", "[multicopter][vtol]") { - AutopilotTester tester; - tester.connect(connection_url); - tester.wait_until_ready(); - tester.arm(); - tester.takeoff(); - tester.wait_until_hovering(); - tester.land(); - tester.wait_until_disarmed(); + AutopilotTester tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.arm(); + tester.takeoff(); + tester.wait_until_hovering(); + tester.land(); + tester.wait_until_disarmed(); } TEST_CASE("Fly square Multicopter Missions", "[multicopter][vtol]") { - AutopilotTester tester; - tester.connect(connection_url); - tester.wait_until_ready(); + AutopilotTester tester; + tester.connect(connection_url); + tester.wait_until_ready(); - SECTION("Mission including RTL") { - AutopilotTester::MissionOptions mission_options; - mission_options.rtl_at_end = true; - tester.prepare_square_mission(mission_options); - tester.arm(); - tester.execute_mission(); - tester.wait_until_disarmed(); - } + SECTION("Mission including RTL") { + AutopilotTester::MissionOptions mission_options; + mission_options.rtl_at_end = true; + tester.prepare_square_mission(mission_options); + tester.arm(); + tester.execute_mission(); + tester.wait_until_disarmed(); + } - SECTION("Mission with manual RTL") { - AutopilotTester::MissionOptions mission_options; - mission_options.rtl_at_end = false; - tester.prepare_square_mission(mission_options); - tester.arm(); - tester.execute_mission(); - tester.wait_until_hovering(); - tester.execute_rtl(); - tester.wait_until_disarmed(); - } + SECTION("Mission with manual RTL") { + AutopilotTester::MissionOptions mission_options; + mission_options.rtl_at_end = false; + tester.prepare_square_mission(mission_options); + tester.arm(); + tester.execute_mission(); + tester.wait_until_hovering(); + tester.execute_rtl(); + tester.wait_until_disarmed(); + } } diff --git a/test/mavsdk_tests/test_mission_vtol.cpp b/test/mavsdk_tests/test_mission_vtol.cpp index 5d8ff737470d..91ee98f03410 100644 --- a/test/mavsdk_tests/test_mission_vtol.cpp +++ b/test/mavsdk_tests/test_mission_vtol.cpp @@ -1,7 +1,35 @@ -// -// VTOL mission test. -// -// Author: Lorenz Meier +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #include #include @@ -13,13 +41,13 @@ TEST_CASE("Takeoff and transition and RTL", "[vtol]") { - AutopilotTester tester; - tester.connect(connection_url); - tester.wait_until_ready(); - tester.arm(); - tester.takeoff(); - tester.wait_until_hovering(); - tester.transition_to_fixedwing(); - tester.execute_rtl(); - tester.wait_until_disarmed(); + AutopilotTester tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.arm(); + tester.takeoff(); + tester.wait_until_hovering(); + tester.transition_to_fixedwing(); + tester.execute_rtl(); + tester.wait_until_disarmed(); } diff --git a/test/mavsdk_tests/test_multicopter_offboard.cpp b/test/mavsdk_tests/test_multicopter_offboard.cpp index 678e9a61eb74..0732e8a9a85a 100644 --- a/test/mavsdk_tests/test_multicopter_offboard.cpp +++ b/test/mavsdk_tests/test_multicopter_offboard.cpp @@ -1,7 +1,35 @@ -// -// Multicopter mission test. -// -// Author: Julian Oes +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #include #include @@ -13,35 +41,35 @@ TEST_CASE("Offboard takeoff and land", "[multicopter][offboard][nogps]") { - AutopilotTester tester; - Offboard::PositionNEDYaw takeoff_position {0.0f, 0.0f, -2.0f, 0.0f}; - tester.connect(connection_url); - tester.wait_until_ready_local_position_only(); - tester.store_home(); - tester.arm(); - tester.offboard_goto(takeoff_position, 0.5f); - tester.offboard_land(); - tester.wait_until_disarmed(); - tester.check_home_within(0.5f); + AutopilotTester tester; + Offboard::PositionNEDYaw takeoff_position {0.0f, 0.0f, -2.0f, 0.0f}; + tester.connect(connection_url); + tester.wait_until_ready_local_position_only(); + tester.store_home(); + tester.arm(); + tester.offboard_goto(takeoff_position, 0.5f); + tester.offboard_land(); + tester.wait_until_disarmed(); + tester.check_home_within(0.5f); } TEST_CASE("Offboard position control", "[multicopter][offboard][nogps]") { - AutopilotTester tester; - Offboard::PositionNEDYaw takeoff_position {0.0f, 0.0f, -2.0f, 0.0f}; - Offboard::PositionNEDYaw setpoint_1 {0.0f, 5.0f, -2.0f, 180.0f}; - Offboard::PositionNEDYaw setpoint_2 {5.0f, 5.0f, -4.0f, 180.0f}; - Offboard::PositionNEDYaw setpoint_3 {5.0f, 0.0f, -4.0f, 90.0f}; - tester.connect(connection_url); - tester.wait_until_ready_local_position_only(); - tester.store_home(); - tester.arm(); - tester.offboard_goto(takeoff_position, 0.5f); - tester.offboard_goto(setpoint_1, 1.0f); - tester.offboard_goto(setpoint_2, 1.0f); - tester.offboard_goto(setpoint_3, 1.0f); - tester.offboard_goto(takeoff_position, 0.2f); - tester.offboard_land(); - tester.wait_until_disarmed(); - tester.check_home_within(1.0f); + AutopilotTester tester; + Offboard::PositionNEDYaw takeoff_position {0.0f, 0.0f, -2.0f, 0.0f}; + Offboard::PositionNEDYaw setpoint_1 {0.0f, 5.0f, -2.0f, 180.0f}; + Offboard::PositionNEDYaw setpoint_2 {5.0f, 5.0f, -4.0f, 180.0f}; + Offboard::PositionNEDYaw setpoint_3 {5.0f, 0.0f, -4.0f, 90.0f}; + tester.connect(connection_url); + tester.wait_until_ready_local_position_only(); + tester.store_home(); + tester.arm(); + tester.offboard_goto(takeoff_position, 0.5f); + tester.offboard_goto(setpoint_1, 1.0f); + tester.offboard_goto(setpoint_2, 1.0f); + tester.offboard_goto(setpoint_3, 1.0f); + tester.offboard_goto(takeoff_position, 0.2f); + tester.offboard_land(); + tester.wait_until_disarmed(); + tester.check_home_within(1.0f); } From 92374b422e5f0ae77da049143bf65379166c5f3b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 17 Mar 2020 16:20:06 +0100 Subject: [PATCH 41/51] mavsdk_tests: name files consistently --- test/mavsdk_tests/CMakeLists.txt | 4 ++-- ...t_mission_multicopter.cpp => test_multicopter_mission.cpp} | 0 .../{test_mission_vtol.cpp => test_vtol_mission.cpp} | 0 3 files changed, 2 insertions(+), 2 deletions(-) rename test/mavsdk_tests/{test_mission_multicopter.cpp => test_multicopter_mission.cpp} (100%) rename test/mavsdk_tests/{test_mission_vtol.cpp => test_vtol_mission.cpp} (100%) diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index 50ee31ce180f..fcdb3b1925f4 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/test/mavsdk_tests/CMakeLists.txt @@ -13,8 +13,8 @@ if(MAVSDK_FOUND) test_main.cpp autopilot_tester.cpp test_multicopter_offboard.cpp - test_mission_multicopter.cpp - test_mission_vtol.cpp + test_multicopter_mission.cpp + test_vtol_mission.cpp ) target_link_libraries(mavsdk_tests diff --git a/test/mavsdk_tests/test_mission_multicopter.cpp b/test/mavsdk_tests/test_multicopter_mission.cpp similarity index 100% rename from test/mavsdk_tests/test_mission_multicopter.cpp rename to test/mavsdk_tests/test_multicopter_mission.cpp diff --git a/test/mavsdk_tests/test_mission_vtol.cpp b/test/mavsdk_tests/test_vtol_mission.cpp similarity index 100% rename from test/mavsdk_tests/test_mission_vtol.cpp rename to test/mavsdk_tests/test_vtol_mission.cpp From 355f15a4f209195d434d692f3bc6646cd2aa13a8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 17 Mar 2020 16:44:45 +0100 Subject: [PATCH 42/51] mavsdk_tests: fix error for Python < 3.8 --- test/mavsdk_tests/logger_helper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/mavsdk_tests/logger_helper.py b/test/mavsdk_tests/logger_helper.py index b63a0df0c051..b462f30d2fdf 100644 --- a/test/mavsdk_tests/logger_helper.py +++ b/test/mavsdk_tests/logger_helper.py @@ -39,7 +39,7 @@ def maybe_strip_color(text: str) -> str: return text -@lru_cache +@lru_cache() def _supports_color() -> bool: """Returns True if the running system's terminal supports color. From 92beffa9872d524b86c5d51642d4165706d5fa6c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 17 Mar 2020 17:15:41 +0100 Subject: [PATCH 43/51] mavsdk_tests: remove unused import --- test/mavsdk_tests/process_helper.py | 1 - 1 file changed, 1 deletion(-) diff --git a/test/mavsdk_tests/process_helper.py b/test/mavsdk_tests/process_helper.py index a7dfa7054796..fad7458bb0ee 100644 --- a/test/mavsdk_tests/process_helper.py +++ b/test/mavsdk_tests/process_helper.py @@ -7,7 +7,6 @@ import subprocess import threading import errno -from logger_helper import color, maybe_strip_color, colorize from typing import Dict, List, TextIO, Optional From 3093a22479246277102ac28667812f3f38291da3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 17 Mar 2020 17:16:17 +0100 Subject: [PATCH 44/51] github: new check for MAVSDK tester Python scripts --- .github/workflows/python_checks.yml | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) create mode 100644 .github/workflows/python_checks.yml diff --git a/.github/workflows/python_checks.yml b/.github/workflows/python_checks.yml new file mode 100644 index 000000000000..94e7dee700cf --- /dev/null +++ b/.github/workflows/python_checks.yml @@ -0,0 +1,23 @@ +name: Python CI checks + +on: + push: + branches: + - 'master' + pull_request: + branches: + - '*' + +jobs: + build: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v1 + with: + token: ${{ secrets.ACCESS_TOKEN }} + - name: Install tools + run: sudo apt-get install python-setuptools flake8 mypy -y + - name: Check MAVSDK test scripts with mypy + run: mypy --strict test/mavsdk_tests/*.py + - name: Check MAVSDK test scripts with flake8 + run: flake8 test/mavsdk_tests/*.py From f8a7f84c547d9e153add02efe10aebfd4285d8df Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 17 Mar 2020 17:23:51 +0100 Subject: [PATCH 45/51] github: try to get latest tools using pip --- .github/workflows/python_checks.yml | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/.github/workflows/python_checks.yml b/.github/workflows/python_checks.yml index 94e7dee700cf..db58013b9993 100644 --- a/.github/workflows/python_checks.yml +++ b/.github/workflows/python_checks.yml @@ -15,9 +15,11 @@ jobs: - uses: actions/checkout@v1 with: token: ${{ secrets.ACCESS_TOKEN }} + - name: Install Python3 + run: sudo apt-get install python3 python3-setuptools python3-pip -y - name: Install tools - run: sudo apt-get install python-setuptools flake8 mypy -y + run: pip3 install --user mypy flake8 - name: Check MAVSDK test scripts with mypy - run: mypy --strict test/mavsdk_tests/*.py + run: $HOME/.local/bin/mypy --strict test/mavsdk_tests/*.py - name: Check MAVSDK test scripts with flake8 - run: flake8 test/mavsdk_tests/*.py + run: $HOME/.local/bin/flake8 test/mavsdk_tests/*.py From 44cc8f6ed3195a472fac874243cf53d1fa5714ce Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 18 Mar 2020 11:11:54 +0100 Subject: [PATCH 46/51] mavsdk_tests: a timeout of 5min should be enough --- test/mavsdk_tests/configs/sitl.json | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/test/mavsdk_tests/configs/sitl.json b/test/mavsdk_tests/configs/sitl.json index d9ee544dfdd9..665c9768db0c 100644 --- a/test/mavsdk_tests/configs/sitl.json +++ b/test/mavsdk_tests/configs/sitl.json @@ -7,23 +7,23 @@ { "model": "iris", "test_filter": "[multicopter],[offboard]", - "timeout_min": 20 + "timeout_min": 5 }, { "model": "iris_opt_flow_mockup", "test_filter": "[multicopter][offboard][nogps]", - "timeout_min": 20 + "timeout_min": 5 }, { "model": "iris_vision", "test_filter": "[multicopter][offboard][nogps]", - "timeout_min": 20, + "timeout_min": 5, "max_speed_factor": 1 }, { "model": "standard_vtol", "test_filter": "[multicopter][vtol]", - "timeout_min": 20 + "timeout_min": 5 } ] } From b1db077cef2a16ce6e662edaf0218d3253b67634 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 18 Mar 2020 11:40:21 +0100 Subject: [PATCH 47/51] mavsdk_tests: remove commented out code --- test/mavsdk_tests/process_helper.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/test/mavsdk_tests/process_helper.py b/test/mavsdk_tests/process_helper.py index fad7458bb0ee..14fe6ca9c1ac 100644 --- a/test/mavsdk_tests/process_helper.py +++ b/test/mavsdk_tests/process_helper.py @@ -202,8 +202,6 @@ def __init__(self, self.cwd = workspace_dir self.env = {"PATH": os.environ['PATH'], "HOME": os.environ['HOME'], - # "GAZEBO_PLUGIN_PATH": - # workspace_dir + "/build/px4_sitl_default/build_gazebo", "GAZEBO_MODEL_PATH": workspace_dir + "/Tools/sitl_gazebo/models", "DISPLAY": os.environ['DISPLAY']} From 4aaaa4764a0a390fef2c81a511c08d842a624e01 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 18 Mar 2020 11:48:00 +0100 Subject: [PATCH 48/51] mavsdk_tests: pass PX4_HOME_ env variables on This way tests can be run at different locations. --- test/mavsdk_tests/process_helper.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/test/mavsdk_tests/process_helper.py b/test/mavsdk_tests/process_helper.py index 14fe6ca9c1ac..a41a2dff2091 100644 --- a/test/mavsdk_tests/process_helper.py +++ b/test/mavsdk_tests/process_helper.py @@ -183,7 +183,10 @@ def __init__(self, "GAZEBO_MODEL_PATH": workspace_dir + "/Tools/sitl_gazebo/models", "PX4_SIM_SPEED_FACTOR": str(speed_factor), - "DISPLAY": os.environ['DISPLAY']} + "DISPLAY": os.environ['DISPLAY'], + "PX4_HOME_LAT": os.environ['PX4_HOME_LAT'], + "PX4_HOME_LON": os.environ['PX4_HOME_LON'], + "PX4_HOME_ALT": os.environ['PX4_HOME_ALT']} self.cmd = "gzserver" self.args = ["--verbose", workspace_dir + "/Tools/sitl_gazebo/worlds/" + From d123a4250ceb37c1525cb8d01bfc898f3331e9f9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 18 Mar 2020 11:50:45 +0100 Subject: [PATCH 49/51] workflows: add more tests outside of Europe --- .github/workflows/sitl_tests.yml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.github/workflows/sitl_tests.yml b/.github/workflows/sitl_tests.yml index 829cfd0c6788..59a836c9cf3a 100644 --- a/.github/workflows/sitl_tests.yml +++ b/.github/workflows/sitl_tests.yml @@ -35,6 +35,12 @@ jobs: run: ccache -s && ccache -z - name: Run SITL tests run: test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 20 --abort-early test/mavsdk_tests/configs/sitl.json + - name: Run SITL tests in Southern hemisphere + run: PX4_HOME_LAT=-37.8134048 PX4_HOME_LON=175.304109 PX4_HOME_ALT=32 | + test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 20 --abort-early test/mavsdk_tests/configs/sitl.json + - name: Run SITL tests far in the West + run: PX4_HOME_LAT=59.6176928 PX4_HOME_LON=-151.1453163 PX4_HOME_ALT=48 | + test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 20 --abort-early test/mavsdk_tests/configs/sitl.json # Report test coverage - name: disable the keychain credential helper From e9708b47a6fe0cb0caf7e5d1ba99c7ff59e46975 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 18 Mar 2020 13:31:18 +0100 Subject: [PATCH 50/51] mavsdk_tests: only env vars that are set Otherwise this raises a KeyError. --- test/mavsdk_tests/process_helper.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/test/mavsdk_tests/process_helper.py b/test/mavsdk_tests/process_helper.py index a41a2dff2091..ca065535f08c 100644 --- a/test/mavsdk_tests/process_helper.py +++ b/test/mavsdk_tests/process_helper.py @@ -183,15 +183,19 @@ def __init__(self, "GAZEBO_MODEL_PATH": workspace_dir + "/Tools/sitl_gazebo/models", "PX4_SIM_SPEED_FACTOR": str(speed_factor), - "DISPLAY": os.environ['DISPLAY'], - "PX4_HOME_LAT": os.environ['PX4_HOME_LAT'], - "PX4_HOME_LON": os.environ['PX4_HOME_LON'], - "PX4_HOME_ALT": os.environ['PX4_HOME_ALT']} + "DISPLAY": os.environ['DISPLAY']} + self.add_to_env_if_set("PX4_HOME_LAT") + self.add_to_env_if_set("PX4_HOME_LON") + self.add_to_env_if_set("PX4_HOME_ALT") self.cmd = "gzserver" self.args = ["--verbose", workspace_dir + "/Tools/sitl_gazebo/worlds/" + self.model + ".world"] + def add_to_env_if_set(self, var: str) -> None: + if var in os.environ: + self.env[var] = os.environ[var] + class GzclientRunner(Runner): def __init__(self, From 3aaa4326fc4e1dfbd2fcae32df375058142b3c35 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 18 Mar 2020 16:11:27 +0100 Subject: [PATCH 51/51] mavsdk_tests: c++ test_runner is now mavsdk_tests --- test/mavsdk_tests/mavsdk_test_runner.py | 4 ++-- test/mavsdk_tests/process_helper.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 03ad2e837a1c..c479e5e0c4a6 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -390,14 +390,14 @@ def start_runners(self, self.verbose) self.active_runners.append(gzclient_runner) - test_runner = ph.TestRunner( + mavsdk_tests_runner = ph.TestRunner( os.getcwd(), log_dir, test['model'], case, self.config['mavlink_connection'], self.verbose) - self.active_runners.append(test_runner) + self.active_runners.append(mavsdk_tests_runner) for runner in self.active_runners: runner.set_log_filename( diff --git a/test/mavsdk_tests/process_helper.py b/test/mavsdk_tests/process_helper.py index ca065535f08c..38e31e9a36a5 100644 --- a/test/mavsdk_tests/process_helper.py +++ b/test/mavsdk_tests/process_helper.py @@ -225,7 +225,7 @@ def __init__(self, mavlink_connection: str, verbose: bool): super().__init__(log_dir, model, case, verbose) - self.name = "test_runner" + self.name = "mavsdk_tests" self.cwd = workspace_dir self.env = {"PATH": os.environ['PATH']} self.cmd = workspace_dir + \