Fix stop in mavsdk_server

src/mavsdk_server/src/connection_initiator.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <future>
+#include <mutex>
 #include <string>
 
 #include "connection_result.h"
@@ -16,8 +17,6 @@ template<typename Mavsdk> class ConnectionInitiator {
 
     bool start(Mavsdk& mavsdk, const std::string& connection_url)
     {
-        init_mutex();
-
         LogInfo() << "Waiting to discover system on " << connection_url << "...";
         _discovery_future = wrapped_subscribe_on_new_system(mavsdk);
 
@@ -28,11 +27,18 @@ template<typename Mavsdk> class ConnectionInitiator {
         return true;
     }
 
-    void wait() { _discovery_future.wait(); }
+    bool wait() { return _discovery_future.get(); }
 
-private:
-    void init_mutex() { _discovery_promise = std::make_shared<std::promise<void>>(); }
+    void cancel()
+    {
+        std::lock_guard<std::mutex> guard(_mutex);
+        if (!_is_discovery_finished) {
+            _is_discovery_finished = true;
+            _discovery_promise->set_value(false);
+        }
+    }
 
+private:
     bool add_any_connection(Mavsdk& mavsdk, const std::string& connection_url)
     {
         mavsdk::ConnectionResult connection_result = mavsdk.add_any_connection(connection_url);
@@ -45,29 +51,29 @@ template<typename Mavsdk> class ConnectionInitiator {
         return true;
     }
 
-    std::future<void> wrapped_subscribe_on_new_system(Mavsdk& mavsdk)
+    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);
             const auto system = mavsdk.systems().at(0);
 
-            if (system->is_connected()) {
-                std::call_once(_discovery_flag, [this]() {
-                    LogInfo() << "System discovered";
-                    _discovery_promise->set_value();
-                });
-            } else {
-                LogInfo() << "System timed out";
+            if (!_is_discovery_finished && system->is_connected()) {
+                LogInfo() << "System discovered";
+
+                _is_discovery_finished = true;
+                _discovery_promise->set_value(true);
             }
         });
 
         return future;
     }
 
-    std::once_flag _discovery_flag{};
-    std::shared_ptr<std::promise<void>> _discovery_promise{};
-    std::future<void> _discovery_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{};
 };
 
 } // namespace mavsdk_server

src/mavsdk_server/src/mavsdk_server.cpp

@@ -13,10 +13,10 @@ class MavsdkServer::Impl {
     Impl() {}
     ~Impl() {}
 
-    void connect(const std::string& connection_url)
+    bool connect(const std::string& connection_url)
     {
         _connection_initiator.start(_mavsdk, connection_url);
-        _connection_initiator.wait();
+        return _connection_initiator.wait();
     }
 
     int startGrpcServer(const int port)
@@ -29,7 +29,14 @@ class MavsdkServer::Impl {
 
     void wait() { _server->wait(); }
 
-    void stop() { _server->stop(); }
+    void stop()
+    {
+        _connection_initiator.cancel();
+
+        if (_server != nullptr) {
+            _server->stop();
+        }
+    }
 
     int getPort() { return _grpc_port; }
 
@@ -47,14 +54,17 @@ int MavsdkServer::startGrpcServer(const int port)
 {
     return _impl->startGrpcServer(port);
 }
-void MavsdkServer::connect(const std::string& connection_url)
+
+bool MavsdkServer::connect(const std::string& connection_url)
 {
     return _impl->connect(connection_url);
 }
+
 void MavsdkServer::wait()
 {
     _impl->wait();
 }
+
 void MavsdkServer::stop()
 {
     _impl->stop();

src/mavsdk_server/src/mavsdk_server.h

@@ -12,7 +12,7 @@ struct MavsdkServer {
     MavsdkServer& operator=(MavsdkServer&&) = delete;
 
     int startGrpcServer(int port);
-    void connect(const std::string& connection_url = "udp://:14540");
+    bool connect(const std::string& connection_url = "udp://:14540");
     void wait();
     void stop();
     int getPort();

src/mavsdk_server/src/mavsdk_server_api.cpp

@@ -2,19 +2,26 @@
 #include "mavsdk_server.h"
 #include <string>
 
-MavsdkServer* mavsdk_server_run(const char* system_address, const int mavsdk_server_port)
+void mavsdk_server_init(MavsdkServer** mavsdk_server)
 {
-    auto mavsdk_server = new MavsdkServer();
+    *mavsdk_server = new MavsdkServer();
+}
 
-    mavsdk_server->connect(std::string(system_address));
+int mavsdk_server_run(
+    MavsdkServer* mavsdk_server, const char* system_address, const int mavsdk_server_port)
+{
+    if (!mavsdk_server->connect(std::string(system_address))) {
+        // Connection was cancelled
+        return false;
+    }
 
     auto grpc_port = mavsdk_server->startGrpcServer(mavsdk_server_port);
     if (grpc_port == 0) {
         // Server failed to start
-        return nullptr;
+        return false;
     }
 
-    return mavsdk_server;
+    return true;
 }
 
 int mavsdk_server_get_port(MavsdkServer* mavsdk_server)
@@ -30,5 +37,9 @@ void mavsdk_server_attach(MavsdkServer* mavsdk_server)
 void mavsdk_server_stop(MavsdkServer* mavsdk_server)
 {
     mavsdk_server->stop();
+}
+
+void mavsdk_server_destroy(MavsdkServer* mavsdk_server)
+{
     delete mavsdk_server;
 }

src/mavsdk_server/src/mavsdk_server_api.h

@@ -10,15 +10,19 @@ extern "C" {
 #define DLLExport __attribute__((visibility("default")))
 #endif
 
-DLLExport struct MavsdkServer*
-mavsdk_server_run(const char* system_address, const int mavsdk_server_port);
+DLLExport void mavsdk_server_init(struct MavsdkServer** mavsdk_server);
+
+DLLExport int mavsdk_server_run(
+    struct MavsdkServer* mavsdk_server, const char* system_address, const int mavsdk_server_port);
 
 DLLExport int mavsdk_server_get_port(struct MavsdkServer* mavsdk_server);
 
 DLLExport void mavsdk_server_attach(struct MavsdkServer* mavsdk_server);
 
 DLLExport void mavsdk_server_stop(struct MavsdkServer* mavsdk_server);
 
+DLLExport void mavsdk_server_destroy(struct MavsdkServer* mavsdk_server);
+
 #ifdef __cplusplus
 }
 #endif

src/mavsdk_server/src/mavsdk_server_bin.cpp

@@ -41,8 +41,20 @@ int main(int argc, char** argv)
         }
     }
 
-    auto mavsdk_server = mavsdk_server_run(connection_url.c_str(), mavsdk_server_port);
+    MavsdkServer* mavsdk_server;
+    mavsdk_server_init(&mavsdk_server);
+    auto is_started = mavsdk_server_run(mavsdk_server, connection_url.c_str(), mavsdk_server_port);
+
+    if (!is_started) {
+        std::cout << "Failed to start, exiting...\n";
+        mavsdk_server_destroy(mavsdk_server);
+        return 1;
+    }
+
     mavsdk_server_attach(mavsdk_server);
+
+    mavsdk_server_destroy(mavsdk_server);
+    return 0;
 }
 
 void usage(const char* bin_name)