mavsdk_server: fix autopilot discovery (#2388)

Instead of using the subscribe_on_new_systems 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 no autopilot is around.

With a stupid old while loop we can just keep checking this until we
have an autopilot and then exit.

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