Test mission protocol with dropped messages

CMakeLists.txt

@@ -40,17 +40,18 @@ add_executable(mavlink_testing_suite
     src/tests/base.cpp
     src/tests/mission.cpp
     src/tests/param.cpp
+    src/tests/lossy_link.cpp
 )
 add_dependencies(mavlink_testing_suite
-    third_party_yaml
-    third_party_dronecode_sdk
+    ${dependencies}
     )
 
 target_link_libraries(mavlink_testing_suite
     dronecode_sdk
     dronecode_sdk_action
 	dronecode_sdk_mission
 	dronecode_sdk_params_raw
+	dronecode_sdk_mavlink_passthrough
 	${CMAKE_CURRENT_BINARY_DIR}/build_yaml/libyaml-cpp.a
 )
 

README.md

@@ -25,6 +25,19 @@ Before committing, make sure to run the code formatting and tidying using clang.
 (cd build && make format; make clang_tidy)
 ```
 
+## Use custom Dronecode SDK build
+
+For debugging purposes it can be handy to use a custom version of the Dronecode SDK:
+
+```
+cd <wherever>/DronecodeSDK
+make clean
+export DRONECODE_SDK_INSTALL_DIR=$(pwd)/install
+make ENABLE_MAVLINK_PASSTHROUGH=1 INSTALL_PREFIX=$DRONECODE_SDK_INSTALL_DIR default install
+cd <wherever>/mavlink-testing-suite
+(mkdir -p build && cd build && cmake -DDRONECODE_SDK_INSTALL_DIR=$DRONECODE_SDK_INSTALL_DIR .. && make)
+```
+
 ### Sample Output
 This is the output of a successful test run:
 ```

cmake/dependencies.cmake

@@ -1,17 +1,27 @@
 include(ExternalProject)
 
-# add SDK
-ExternalProject_Add(third_party_dronecode_sdk
-    SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/third_party/DronecodeSDK
-    CMAKE_ARGS
-        -DCMAKE_INSTALL_PREFIX=${CMAKE_CURRENT_BINARY_DIR}/build_dronecode_sdk/install
-    GIT_REPOSITORY https://github.com/Dronecode/DronecodeSDK.git
-    GIT_TAG v0.13.0
-    BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/build_dronecode_sdk
-    INSTALL_COMMAND ${CMAKE_COMMAND} --build . --target install
-    )
-include_directories(${CMAKE_CURRENT_BINARY_DIR}/build_dronecode_sdk/install/include)
-link_directories(${CMAKE_CURRENT_BINARY_DIR}/build_dronecode_sdk/install/lib)
+
+if(DRONECODE_SDK_INSTALL_DIR)
+    message(STATUS "Using DronecodeSDK from: ${DRONECODE_SDK_INSTALL_DIR}")
+    include_directories(${DRONECODE_SDK_INSTALL_DIR}/include)
+    link_directories(${DRONECODE_SDK_INSTALL_DIR}/lib)
+else()
+    # clone and build SDK via ExternalProject
+    ExternalProject_Add(third_party_dronecode_sdk
+        SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/third_party/DronecodeSDK
+        CMAKE_ARGS
+            -DCMAKE_INSTALL_PREFIX=${CMAKE_CURRENT_BINARY_DIR}/build_dronecode_sdk/install
+            -DENABLE_MAVLINK_PASSTHROUGH=1
+        GIT_REPOSITORY https://github.com/Dronecode/DronecodeSDK.git
+        GIT_TAG 1a62225df0ef4f0f1034fa6979b087e679afd58e
+        BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/build_dronecode_sdk
+        INSTALL_COMMAND ${CMAKE_COMMAND} --build . --target install
+        )
+    include_directories(${CMAKE_CURRENT_BINARY_DIR}/build_dronecode_sdk/install/include)
+    link_directories(${CMAKE_CURRENT_BINARY_DIR}/build_dronecode_sdk/install/lib)
+    list(APPEND dependencies third_party_dronecode_sdk)
+endif()
+
 
 # add YAML (build from source)
 ExternalProject_Add(third_party_yaml
@@ -24,3 +34,4 @@ ExternalProject_Add(third_party_yaml
     INSTALL_COMMAND ""
     )
 include_directories(${CMAKE_CURRENT_BINARY_DIR}/third_party/yaml-cpp/include)
+list(APPEND dependencies third_party_yaml)

config/autopilot.yaml

@@ -1,6 +1,11 @@
 tests:
     - name: Mission
       num_waypoints: 500
+      message_loss: 0.0
+
+    - name: Mission
+      num_waypoints: 500
+      message_loss: 0.25
 
     - name: ParamChange
       param_name: SYS_HITL

src/main.cpp

@@ -1,10 +1,12 @@
 
+#include <algorithm>
 #include <chrono>
 #include <cstdint>
 #include <functional>
 #include <future>
 #include <iostream>
 #include <thread>
+#include <vector>
 
 #include <dronecode_sdk/dronecode_sdk.h>
 
@@ -74,11 +76,14 @@ int main(int argc, char** argv)
     YAML::Node config = YAML::LoadFile("../config/autopilot.yaml");
     YAML::Node tests_node = config["tests"];
 
+    std::vector<bool> fails(tests_node.size(), false);
+
     // Run the test(s)
     tests::Context context{.system = system};
-    bool failed = false;
+    unsigned i = 0;
     for (auto test_node : tests_node) {
         std::string test_name = test_node["name"].as<std::string>();
+        fails[i] = false;
         std::unique_ptr<tests::TestBase> test =
             tests::TestFactory::instance().getTest(test_name, context);
 
@@ -88,21 +93,42 @@ int main(int argc, char** argv)
             test->run();
         } catch (tests::TestBase::TestAborted& e) {
             std::cout << test_name << " aborted early (" << e.what() << ")" << std::endl;
-            failed = true;
+            fails[i] = true;
         }
 
-        if (!failed) {
+        if (!fails[i]) {
             tests::TestBase::Result result = test->getResult();
             if (result != tests::TestBase::Result::Success) {
-                failed = true;
+                fails[i] = true;
             }
             std::cout << test_name << " test result: " << result << std::endl;
         }
 
         // store the actually used config (which includes the default values) back
         // in the YAML config node
         test->storeConfig(test_node);
+        std::cout << "----" << std::endl;
+        ++i;
+    }
+
+    std::cout << "Test summary:" << std::endl;
+
+    i = 0;
+    for (auto test_node : tests_node) {
+        std::cout << test_node["name"].as<std::string>() << ":" << std::endl;
+        for (auto config : test_node) {
+            std::cout << config.first.as<std::string>() << ": " << config.second.as<std::string>()
+                      << std::endl;
+        }
+
+        std::cout << "  => " << (fails[i] ? std::string("failed") : std::string("passed"))
+                  << std::endl;
+        std::cout << "----" << std::endl;
+        ++i;
     }
 
-    return failed ? -1 : 0;
+    return (std::any_of(fails.cbegin(), fails.cend(),
+                        [](std::vector<bool>::const_reference item) { return item; })
+                ? -1
+                : 0);
 }

src/tests/lossy_link.cpp

@@ -0,0 +1,11 @@
+#include "lossy_link.h"
+
+LossyLink::LossyLink(int seed_num) : _engine(seed_num) {}
+
+bool LossyLink::drop(float ratio)
+{
+    std::uniform_real_distribution<> distribution(0.0f, 1.0f);
+    const float random_value = distribution(_engine);
+    const bool should_drop = (random_value < ratio);
+    return should_drop;
+}

src/tests/lossy_link.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include <random>
+
+class LossyLink
+{
+public:
+    explicit LossyLink(int seed_num);
+    ~LossyLink() = default;
+
+    /** Checks if we should drop a message given a ratio of dropped messages.
+     *
+     * @param ratio Ratio of drops (0: no drops, 1: everything dropped)
+     * @return true if message should be dropped.
+     */
+    bool drop(float ratio);
+
+private:
+    std::default_random_engine _engine;
+};

src/tests/mission.cpp

@@ -11,7 +11,14 @@ namespace tests
 {
 REGISTER_TEST(Mission);
 
-Mission::Mission(const Context& context) : TestBase(context), _mission(context.system) {}
+Mission::Mission(const Context& context)
+    : TestBase(context),
+      _mission(context.system),
+      _mavlink_passthrough(context.system),
+      _lossy_link_incoming(0),
+      _lossy_link_outgoing(1)
+{
+}
 
 shared_ptr<dcsdk::MissionItem> Mission::makeMissionItem(double latitude_deg, double longitude_deg,
                                                         float relative_altitude_m)
@@ -23,6 +30,13 @@ shared_ptr<dcsdk::MissionItem> Mission::makeMissionItem(double latitude_deg, dou
 }
 
 void Mission::run()
+{
+    dropMessages(_config.message_loss);
+    uploadDownloadCompare();
+    eraseMission();
+}
+
+void Mission::uploadDownloadCompare()
 {
     std::vector<std::shared_ptr<dcsdk::MissionItem>> items = assembleMissionItems();
 
@@ -32,6 +46,16 @@ void Mission::run()
     compareMissions(items, downloaded_items);
 }
 
+void Mission::eraseMission()
+{
+    // TODO: presumably the Dronecode SDK should expose a function to do `MISSION_CLEAR_ALL`
+    //       instead of uploading an empty mission list.
+
+    // This doesn't seem to work for now.
+    // std::vector<std::shared_ptr<dcsdk::MissionItem>> no_items{};
+    // uploadMission(no_items);
+}
+
 std::vector<std::shared_ptr<dcsdk::MissionItem>> Mission::assembleMissionItems()
 {
     cout << "Number of waypoints: " << _config.num_waypoints << endl;
@@ -97,4 +121,21 @@ void Mission::compareMissions(const std::vector<std::shared_ptr<dcsdk::MissionIt
     }
 }
 
+void Mission::dropMessages(const float ratio)
+{
+    _mavlink_passthrough.intercept_incoming_messages_async(
+        [this, ratio](mavlink_message_t& message) {
+            UNUSED(message);
+            const bool should_keep = !_lossy_link_incoming.drop(ratio);
+            return should_keep;
+        });
+
+    _mavlink_passthrough.intercept_outgoing_messages_async(
+        [this, ratio](mavlink_message_t& message) {
+            UNUSED(message);
+            const bool should_keep = !_lossy_link_outgoing.drop(ratio);
+            return should_keep;
+        });
+}
+
 }  // namespace tests

src/tests/mission.h

@@ -1,8 +1,10 @@
 #pragma once
 
 #include "base.h"
+#include "lossy_link.h"
 
 #include <dronecode_sdk/dronecode_sdk.h>
+#include <dronecode_sdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
 #include <dronecode_sdk/plugins/mission/mission.h>
 
 namespace dcsdk = dronecode_sdk;
@@ -18,8 +20,13 @@ class Mission : public TestBase
 public:
     struct Config {
         int num_waypoints{10};
+        float message_loss{0.0f};
 
-        void serialize(ConfigProvider& c) { c("num_waypoints", num_waypoints); }
+        void serialize(ConfigProvider& c)
+        {
+            c("num_waypoints", num_waypoints);
+            c("message_loss", message_loss);
+        }
     };
 
     explicit Mission(const Context& context);
@@ -31,6 +38,8 @@ class Mission : public TestBase
     void serialize(ConfigProvider& c) override { _config.serialize(c); }
 
 private:
+    void uploadDownloadCompare();
+    void eraseMission();
     std::shared_ptr<dcsdk::MissionItem> makeMissionItem(double latitude_deg, double longitude_deg,
                                                         float relative_altitude_m);
 
@@ -40,8 +49,14 @@ class Mission : public TestBase
     void compareMissions(const std::vector<std::shared_ptr<dcsdk::MissionItem>>& items_a,
                          const std::vector<std::shared_ptr<dcsdk::MissionItem>>& items_b);
 
+    void dropMessages(float ratio);
+
     dcsdk::Mission _mission;
+    dcsdk::MavlinkPassthrough _mavlink_passthrough;
     Config _config;
+
+    LossyLink _lossy_link_incoming;
+    LossyLink _lossy_link_outgoing;
 };
 
 }  // namespace tests

src/tests/param.h

@@ -3,7 +3,7 @@
 #include "base.h"
 
 #include <dronecode_sdk/dronecode_sdk.h>
-#include <dronecode_sdk/plugins/param_raw/params_raw.h>
+#include <dronecode_sdk/plugins/params_raw/params_raw.h>
 
 namespace tests
 {