Added first two simple MAVSDK tests

mavsdk_tests/.gitignore

@@ -0,0 +1 @@
+build

mavsdk_tests/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.5.1)
+
+project(test_mission_multicopter)
+
+add_definitions("-std=c++17 -Wall -Wextra -Werror")
+
+find_package(MAVSDK REQUIRED)
+
+add_executable(test_mission_multicopter
+    autopilot_tester.cpp
+    test_mission_multicopter.cpp
+)
+
+target_link_libraries(test_mission_multicopter
+    MAVSDK::mavsdk
+    MAVSDK::mavsdk_action
+    MAVSDK::mavsdk_mission
+    MAVSDK::mavsdk_telemetry
+)

mavsdk_tests/autopilot_tester.cpp

@@ -0,0 +1,123 @@
+#include "autopilot_tester.h"
+#include <iostream>
+#include <future>
+
+using namespace mavsdk;
+using namespace mavsdk::geometry;
+
+void AutopilotTester::connect(const std::string uri)
+{
+    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(10)));
+
+    auto& system = _mavsdk.system();
+
+    _telemetry.reset(new Telemetry(system));
+    _action.reset(new Action(system));
+    _mission.reset(new Mission(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(10)));
+}
+
+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));
+}
+
+void AutopilotTester::arm()
+{
+    const auto result = _action->arm();
+    REQUIRE(result == Action::Result::SUCCESS);
+}
+
+void AutopilotTester::takeoff()
+{
+    const auto result = _action->takeoff();
+    REQUIRE(result == Action::Result::SUCCESS);
+}
+
+void AutopilotTester::land()
+{
+    const auto result = _action->land();
+    REQUIRE(result == Action::Result::SUCCESS);
+}
+
+void AutopilotTester::wait_until_disarmed()
+{
+    REQUIRE(poll_condition_with_timeout(
+        [this]() { return !_telemetry->armed(); }, std::chrono::seconds(10)));
+}
+
+void AutopilotTester::wait_until_hovering()
+{
+    REQUIRE(poll_condition_with_timeout(
+        [this]() { return _telemetry->landed_state() == Telemetry::LandedState::IN_AIR; }, std::chrono::seconds(10)));
+}
+
+void AutopilotTester::prepare_square_mission(MissionOptions mission_options)
+{
+    const auto ct = _get_coordinate_transformation();
+
+    std::vector<std::shared_ptr<MissionItem>> 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);
+
+    std::promise<void> prom;
+    auto fut = prom.get_future();
+
+    _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);
+}
+
+void AutopilotTester::execute_mission()
+{
+    std::promise<void> prom;
+    auto fut = prom.get_future();
+
+    _mission->start_mission_async([&prom](Mission::Result result) {
+        REQUIRE(Mission::Result::SUCCESS == result);
+        prom.set_value();
+    });
+
+    REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready);
+}
+
+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});
+}
+
+std::shared_ptr<MissionItem>  AutopilotTester::_create_mission_item(
+    const CoordinateTransformation::LocalCoordinate& local_coordinate,
+    const MissionOptions& mission_options,
+    const CoordinateTransformation& ct)
+{
+    auto mission_item = std::make_shared<MissionItem>();
+    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;
+}

mavsdk_tests/autopilot_tester.h

@@ -0,0 +1,60 @@
+#pragma once
+
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/geometry.h>
+#include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/mission/mission.h>
+#include <mavsdk/plugins/telemetry/telemetry.h>
+#include "catch2/catch.hpp"
+#include <chrono>
+#include <memory>
+#include <thread>
+
+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 set_takeoff_altitude(const float altitude_m);
+    void arm();
+    void takeoff();
+    void land();
+    void wait_until_disarmed();
+    void wait_until_hovering();
+    void prepare_square_mission(MissionOptions mission_options);
+    void execute_mission();
+
+private:
+    mavsdk::geometry::CoordinateTransformation _get_coordinate_transformation();
+    std::shared_ptr<mavsdk::MissionItem> _create_mission_item(
+        const mavsdk::geometry::CoordinateTransformation::LocalCoordinate& local_coordinate,
+        const MissionOptions& mission_options,
+        const mavsdk::geometry::CoordinateTransformation& ct);
+
+    mavsdk::Mavsdk _mavsdk{};
+    std::unique_ptr<mavsdk::Telemetry> _telemetry{};
+    std::unique_ptr<mavsdk::Action> _action{};
+    std::unique_ptr<mavsdk::Mission> _mission{};
+};
+
+template<typename Rep, typename Period>
+bool poll_condition_with_timeout(
+    std::function<bool()> fun, std::chrono::duration<Rep, Period> 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;
+}

mavsdk_tests/test_mission_multicopter.cpp

@@ -0,0 +1,99 @@
+//
+// Multicopter mission test.
+//
+// Author: Julian Oes <julian@oes.ch>
+
+#define CATCH_CONFIG_RUNNER
+#include "catch2/catch.hpp"
+
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/telemetry/telemetry.h>
+#include <iostream>
+#include <string>
+#include "autopilot_tester.h"
+
+using namespace mavsdk;
+
+static std::string connection_url{"udp://"};
+
+static void usage(const std::string& bin_name);
+static void remove_argv(int& argc, char** argv, int pos);
+
+int main(int argc, char** argv)
+{
+    for (int i = 0; i < argc; ++i) {
+        const std::string argv_string(argv[i]);
+
+        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;
+            }
+        }
+    }
+
+    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)
+{
+    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)
+{
+    for (int i = pos; i+1 < argc; ++i) {
+        argv[i] = argv[i+1];
+    }
+    argv[--argc] = nullptr;
+}
+
+TEST_CASE("We can takeoff and land", "[multicopter]")
+{
+    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("We can fly a square mission and do RTL", "[multicopter]")
+{
+    AutopilotTester tester;
+    tester.connect(connection_url);
+    tester.wait_until_ready();
+
+    AutopilotTester::MissionOptions mission_options;
+    mission_options.rtl_at_end = true;
+    tester.prepare_square_mission(mission_options);
+
+    tester.arm();
+    tester.execute_mission();
+    tester.wait_until_hovering();
+    tester.wait_until_disarmed();
+}