[v2.12] mavsdk_server: fix autopilot discovery

src/mavsdk_server/src/connection_initiator.h

@@ -1,7 +1,9 @@
 #pragma once
 
+#include <chrono>
 #include <future>
 #include <mutex>
+#include <thread>
 #include <string>
 
 #include "connection_result.h"
@@ -15,29 +17,37 @@ template<typename Mavsdk> class ConnectionInitiator {
     ConnectionInitiator() {}
     ~ConnectionInitiator() {}
 
-    bool start(Mavsdk& mavsdk, const std::string& connection_url)
+    bool connect(Mavsdk& mavsdk, const std::string& connection_url)
     {
         LogInfo() << "Waiting to discover system on " << connection_url << "...";
-        _discovery_future = wrapped_subscribe_on_new_system(mavsdk);
 
         if (!add_any_connection(mavsdk, connection_url)) {
             return false;
         }
 
-        return true;
-    }
-
-    bool wait() { return _discovery_future.get(); }
+        // Instead of using the subscribe_on_new_system callback, we just use a
+        // while loop here. That's because the subscribe_on_new_system callback
+        // won't tell us if an autopilot is discovered when another component
+        // of the same system is discovered first, thus triggering the callback
+        // early when not autopilot is around.
+        // With a stupid old while loop we can just keep checking this until
+        // we have an autopilot and then exit.
+        while (!_should_exit) {
+            for (const auto& system : mavsdk.systems()) {
+                if (system->has_autopilot()) {
+                    LogInfo() << "System discovered";
+                    return true;
+                }
+            }
 
-    void cancel()
-    {
-        std::lock_guard<std::mutex> guard(_mutex);
-        if (!_is_discovery_finished) {
-            _is_discovery_finished = true;
-            _discovery_promise->set_value(false);
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
         }
+
+        return false;
     }
 
+    void cancel() { _should_exit = true; }
+
 private:
     bool add_any_connection(Mavsdk& mavsdk, const std::string& connection_url)
     {
@@ -51,30 +61,7 @@ template<typename Mavsdk> class ConnectionInitiator {
         return true;
     }
 
-    std::future<bool> wrapped_subscribe_on_new_system(Mavsdk& mavsdk)
-    {
-        auto future = _discovery_promise->get_future();
-
-        mavsdk.subscribe_on_new_system([this, &mavsdk]() {
-            std::lock_guard<std::mutex> guard(_mutex);
-            for (auto system : mavsdk.systems()) {
-                if (!_is_discovery_finished && system->has_autopilot() && system->is_connected()) {
-                    LogInfo() << "System discovered";
-
-                    _is_discovery_finished = true;
-                    _discovery_promise->set_value(true);
-                    break;
-                }
-            }
-        });
-
-        return future;
-    }
-
-    std::mutex _mutex;
-    std::atomic<bool> _is_discovery_finished = false;
-    std::shared_ptr<std::promise<bool>> _discovery_promise = std::make_shared<std::promise<bool>>();
-    std::future<bool> _discovery_future{};
+    std::atomic<bool> _should_exit{false};
 };
 
 } // namespace mavsdk_server

src/mavsdk_server/src/mavsdk_server.cpp

@@ -16,8 +16,7 @@ class MavsdkServer::Impl {
 
     bool connect(const std::string& connection_url)
     {
-        _connection_initiator.start(_mavsdk, connection_url);
-        return _connection_initiator.wait();
+        return _connection_initiator.connect(_mavsdk, connection_url);
     }
 
     int startGrpcServer(const int port)

src/mavsdk_server/test/CMakeLists.txt

@@ -4,7 +4,6 @@ add_executable(unit_tests_mavsdk_server
     action_service_impl_test.cpp
     mavsdk_server_main.cpp
     camera_service_impl_test.cpp
-    connection_initiator_test.cpp
     core_service_impl_test.cpp
     mission_service_impl_test.cpp
     offboard_service_impl_test.cpp