Add flow tests to MAVSDK tests (#14039)

* workflows: remove unneeded pip dep and comments

* mavsdk_tests: simple test using flow/offboard

This adds a first proof of concept of a test using flow and offboard.

* Flow MAVSDK test: check against ground truth

* CleanUp

* workflows: use latest docker image with psutil

* mavsdk_tests: fix PEP8 issues

* Add VTOL CI tests back

* switch to non-rendering flow mockup

* workflows: install mavsdk from GitHub release .deb

* Add vision test and disable VTOL temporarlly

* Fist draft of combining test coverage and test in one workflow

* Add VTOL mavsdk tests back in CI

Co-authored-by: Julian Oes <julian@oes.ch>
Co-authored-by: kritz <kritz@ethz.ch>

.github/workflows/sitl_tests.yml

@@ -11,20 +11,42 @@ on:
 jobs:
   build:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-simulation-bionic:2020-01-13
+    container: px4io/px4-dev-simulation-bionic:2020-01-29
     steps:
     - uses: actions/checkout@v1
       with:
         token: ${{ secrets.ACCESS_TOKEN }}
     - name: Download MAVSDK
-      run: wget https://github.com/mavlink/MAVSDK/releases/download/v0.23.0/mavsdk_0.23.0_ubuntu18.04_amd64.deb
+      run: wget https://github.com/mavlink/MAVSDK/releases/download/v0.24.0/mavsdk_0.24.0_ubuntu18.04_amd64.deb
     - name: Install MAVSDK
-      run: dpkg -i mavsdk_0.23.0_ubuntu18.04_amd64.deb
-    - name: Install psutil
-      run: pip3 install psutil
+      run: dpkg -i mavsdk_0.24.0_ubuntu18.04_amd64.deb
     - name: Build PX4 in SITL integration test mode
-      # Build all targets
-      run: DONT_RUN=1 make px4_sitl gazebo mavsdk_tests
-    - name: Run simulation tests
-      # Grind configuration: Change iterations to a number between 20 and 100 (30 minutes to about 3 hours runtime)
-      run:  test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 20 --iterations 1 --fail-early
+      run: DONT_RUN=1 make px4_sitl PX4_CMAKE_BUILD_TYPE=Coverage gazebo 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
+
+    # Report test coverage
+    - name: disable the keychain credential helper
+      run: git config --global credential.helper ""
+    - name: enable the local store credential helper
+      run: git config --global --add credential.helper store
+    - name: add credential
+      run: echo "https://x-access-token:${{ secrets.ACCESS_TOKEN }}@github.com" >> ~/.git-credentials
+    - name: tell git to use https instead of ssh whenever it encounters it
+      run: 'git config --global url."https://github.com/".insteadof git@github.com:'
+    - name: Check code coverage
+      run: |
+        mkdir -p coverage
+        lcov --directory build/px4_sitl_default --base-directory build/px4_sitl_default --gcov-tool gcov --capture -o coverage/lcov.info
+    - name: Upload coverage information to Codecov
+      uses: codecov/codecov-action@v1
+      with:
+        token: ${{ secrets.CODECOV_TOKEN }}
+        flags: mavsdk
+        file: coverage/lcov.info

.github/workflows/sitl_tests_coverage.yml

@@ -1,52 +0,0 @@
-name: SITL Tests (Code Coverage)
-
-on:
-  push:
-    branches:
-    - 'master'
-  pull_request:
-    branches:
-    - '*'
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-simulation-bionic:2020-01-13
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
-    # see https://github.com/actions/checkout/issues/14
-    - name: disable the keychain credential helper
-      run: git config --global credential.helper ""
-    - name: enable the local store credential helper
-      run: git config --global --add credential.helper store
-    - name: add credential
-      run: echo "https://x-access-token:${{ secrets.ACCESS_TOKEN }}@github.com" >> ~/.git-credentials
-    - name: tell git to use https instead of ssh whenever it encounters it
-      run: 'git config --global url."https://github.com/".insteadof git@github.com:'
-    - name: get submodules
-      run: 'git submodule update --init --recursive'
-    - name: Set Python 3 as default
-      run: update-alternatives --install /usr/bin/python python /usr/bin/python3 10
-    - name: Install psutil
-      run: pip3 install psutil
-    # - name: Download newer mavsdk
-    #   run: git clone https://github.com/mavlink/MAVSDK.git && cd MAVSDK && git submodule init && git submodule update && mkdir -p build/default
-    # - name: Build newer mavsdk
-    #   run: mkdir -p MAVSDK/build/default && cd MAVSDK/build/default && cmake ../.. && make -j4 && make install
-    - name: Run Coverage Tests
-      run: make tests_integration_coverage
-    # We are not actively using coveralls, but we keep the config
-    # in case the service should be re-enabled later
-    # - name: Upload coverage information to Coveralls
-    #   uses: coverallsapp/github-action@master
-    #   with:
-    #     path-to-lcov: coverage/lcov.info
-    #     github-token: ${{ secrets.GITHUB_TOKEN }}
-    - name: Upload coverage information to Codecov
-      uses: codecov/codecov-action@v1
-      with:
-        token: ${{ secrets.CODECOV_TOKEN }}
-        flags: mavsdk
-        file: coverage/lcov.info

test/mavsdk_tests/CMakeLists.txt

@@ -12,6 +12,7 @@ if(MAVSDK_FOUND)
     add_executable(mavsdk_tests
         test_main.cpp
         autopilot_tester.cpp
+        test_multicopter_offboard.cpp
         test_mission_multicopter.cpp
         test_mission_plane.cpp
         test_mission_vtol.cpp
@@ -21,6 +22,7 @@ if(MAVSDK_FOUND)
         MAVSDK::mavsdk
         MAVSDK::mavsdk_action
         MAVSDK::mavsdk_mission
+        MAVSDK::mavsdk_offboard
         MAVSDK::mavsdk_telemetry
     )
 

test/mavsdk_tests/autopilot_tester.cpp

@@ -2,9 +2,6 @@
 #include <iostream>
 #include <future>
 
-using namespace mavsdk;
-using namespace mavsdk::geometry;
-
 std::string connection_url {"udp://"};
 
 void AutopilotTester::connect(const std::string uri)
@@ -21,6 +18,7 @@ void AutopilotTester::connect(const std::string uri)
     _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()
@@ -30,6 +28,20 @@ void AutopilotTester::wait_until_ready()
         [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::chrono::seconds(20)));
+}
+
 void AutopilotTester::set_takeoff_altitude(const float altitude_m)
 {
     CHECK(Action::Result::SUCCESS == _action->set_takeoff_altitude(altitude_m));
@@ -125,7 +137,6 @@ CoordinateTransformation AutopilotTester::_get_coordinate_transformation()
     const auto home = _telemetry->home_position();
     REQUIRE(std::isfinite(home.latitude_deg));
     REQUIRE(std::isfinite(home.longitude_deg));
-    REQUIRE(std::isfinite(home.longitude_deg));
     return CoordinateTransformation({home.latitude_deg, home.longitude_deg});
 }
 
@@ -145,3 +156,64 @@ void AutopilotTester::execute_rtl()
 {
     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)
+{
+    _offboard->set_position_ned(target);
+    REQUIRE(_offboard->start() == Offboard::Result::SUCCESS);
+    REQUIRE(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);
+}
+
+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);
+}
+
+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);
+}
+
+void AutopilotTester::request_ground_truth()
+{
+    REQUIRE(_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();
+}
+
+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));
+    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));
+    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);
+}

test/mavsdk_tests/autopilot_tester.h

@@ -4,6 +4,7 @@
 #include <mavsdk/geometry.h>
 #include <mavsdk/plugins/action/action.h>
 #include <mavsdk/plugins/mission/mission.h>
+#include <mavsdk/plugins/offboard/offboard.h>
 #include <mavsdk/plugins/telemetry/telemetry.h>
 #include "catch2/catch.hpp"
 #include <chrono>
@@ -12,6 +13,9 @@
 
 extern std::string connection_url;
 
+using namespace mavsdk;
+using namespace mavsdk::geometry;
+
 class AutopilotTester {
 public:
     struct MissionOptions {
@@ -22,6 +26,7 @@ public:
 
     void connect(const std::string uri);
     void wait_until_ready();
+    void wait_until_ready_local_position_only();
     void set_takeoff_altitude(const float altitude_m);
     void arm();
     void takeoff();
@@ -33,6 +38,15 @@ public:
     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,
+                       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();
@@ -45,6 +59,7 @@ private:
     std::unique_ptr<mavsdk::Telemetry> _telemetry{};
     std::unique_ptr<mavsdk::Action> _action{};
     std::unique_ptr<mavsdk::Mission> _mission{};
+    std::unique_ptr<mavsdk::Offboard> _offboard{};
 };
 
 template<typename Rep, typename Period>
@@ -63,3 +78,5 @@ bool poll_condition_with_timeout(
     }
     return true;
 }
+
+inline float sq(float x) { return x * x; };

test/mavsdk_tests/mavsdk_test_runner.py

@@ -18,10 +18,25 @@ test_matrix = [
         "timeout_min": 20,
     },
     {
-        "model": "standard_vtol",
-        "test_filter": "[vtol]",
+        "model": "iris_opt_flow",
+        "test_filter": "[multicopter_offboard]",
         "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,
+    },
+    {
+       "model": "standard_vtol",
+       "test_filter": "[vtol]",
+       "timeout_min": 20,
+    },
     # {
     #     "model": "plane",
     #     "test_filter": "[plane]",
@@ -175,6 +190,8 @@ def main():
                         help="Abort on first unsuccessful test")
     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")
     args = parser.parse_args()
 
     if not is_everything_ready():
@@ -253,7 +270,20 @@ def run(args):
 
 def run_test_group(args):
     overall_success = True
-    for group in test_matrix:
+
+    if args.model == 'all':
+        models = test_matrix
+    else:
+        found = False
+        for elem in test_matrix:
+            if elem['model'] == args.model:
+                models = [elem]
+                found = True
+        if not found:
+            print("Specified model is not defined")
+            models = []
+
+    for group in models:
         print("Running test group for '{}' with filter '{}'"
               .format(group['model'], group['test_filter']))
 

test/mavsdk_tests/test_multicopter_offboard.cpp

@@ -0,0 +1,49 @@
+//
+// Multicopter mission test.
+//
+// Author: Julian Oes <julian@oes.ch>
+
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/telemetry/telemetry.h>
+#include <iostream>
+#include <string>
+#include "autopilot_tester.h"
+
+
+TEST_CASE("Takeoff and Land (Multicopter offboard)", "[multicopter_offboard]")
+{
+    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.request_ground_truth();
+    Telemetry::GroundTruth home = tester.get_ground_truth_position();
+    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));
+}
+
+TEST_CASE("Mission (Multicopter offboard )", "[multicopter_offboard]")
+{
+    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.request_ground_truth();
+    Telemetry::GroundTruth home = tester.get_ground_truth_position();
+    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();
+    REQUIRE(tester.ground_truth_horizontal_position_close_to(home, 1.0f));
+}