diff --git a/src/mavsdk_server/src/connection_initiator.h b/src/mavsdk_server/src/connection_initiator.h index 95cfafc79..7e87188c9 100644 --- a/src/mavsdk_server/src/connection_initiator.h +++ b/src/mavsdk_server/src/connection_initiator.h @@ -1,7 +1,9 @@ #pragma once +#include #include #include +#include #include #include "connection_result.h" @@ -15,29 +17,37 @@ template 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 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 class ConnectionInitiator { return true; } - std::future wrapped_subscribe_on_new_system(Mavsdk& mavsdk) - { - auto future = _discovery_promise->get_future(); - - mavsdk.subscribe_on_new_system([this, &mavsdk]() { - std::lock_guard 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 _is_discovery_finished = false; - std::shared_ptr> _discovery_promise = std::make_shared>(); - std::future _discovery_future{}; + std::atomic _should_exit{false}; }; } // namespace mavsdk_server diff --git a/src/mavsdk_server/src/mavsdk_server.cpp b/src/mavsdk_server/src/mavsdk_server.cpp index 9702fce49..e11f97eb0 100644 --- a/src/mavsdk_server/src/mavsdk_server.cpp +++ b/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) diff --git a/src/mavsdk_server/test/CMakeLists.txt b/src/mavsdk_server/test/CMakeLists.txt index 153f1bf49..eff0e065a 100644 --- a/src/mavsdk_server/test/CMakeLists.txt +++ b/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 diff --git a/src/mavsdk_server/test/connection_initiator_test.cpp b/src/mavsdk_server/test/connection_initiator_test.cpp deleted file mode 100644 index fcca8b3a4..000000000 --- a/src/mavsdk_server/test/connection_initiator_test.cpp +++ /dev/null @@ -1,111 +0,0 @@ -#include -#include - -#include "connection_initiator.h" -#include "mocks/mavsdk_mock.h" -#include "mocks/system_mock.h" - -namespace { - -using testing::_; - -using NewSystemCallback = mavsdk::testing::NewSystemCallback; -using MockMavsdk = mavsdk::testing::MockMavsdk; -using MockSystem = mavsdk::testing::MockSystem; -using ConnectionInitiator = mavsdk::mavsdk_server::ConnectionInitiator; - -static constexpr auto ARBITRARY_CONNECTION_URL = "udp://1291"; - -ACTION_P(SaveCallback, change_callback) -{ - *change_callback = arg0; -} - -TEST(ConnectionInitiator, subscribeChangeIsCalledExactlyOnce) -{ - ConnectionInitiator initiator; - MockMavsdk mavsdk; - EXPECT_CALL(mavsdk, subscribe_on_new_system(_)).Times(1); - - initiator.start(mavsdk, ARBITRARY_CONNECTION_URL); -} - -TEST(ConnectionInitiator, startAddsUDPConnection) -{ - ConnectionInitiator initiator; - MockMavsdk mavsdk; - - EXPECT_CALL(mavsdk, add_any_connection(_)); - - initiator.start(mavsdk, ARBITRARY_CONNECTION_URL); -} - -TEST(ConnectionInitiator, startHangsUntilSystemDiscovered) -{ - ConnectionInitiator initiator; - MockMavsdk mavsdk; - NewSystemCallback change_callback; - - EXPECT_CALL(mavsdk, subscribe_on_new_system(_)).WillOnce(SaveCallback(&change_callback)); - - std::vector> systems; - auto system = std::make_shared(); - systems.push_back(system); - EXPECT_CALL(mavsdk, systems()).WillOnce(testing::Return(systems)); - - EXPECT_CALL(*system, is_connected()).WillOnce(testing::Return(true)); - EXPECT_CALL(*system, has_autopilot()).WillOnce(testing::Return(true)); - - auto async_future = std::async(std::launch::async, [&initiator, &mavsdk]() { - initiator.start(mavsdk, ARBITRARY_CONNECTION_URL); - initiator.wait(); - }); - - EXPECT_TRUE( - async_future.wait_for(std::chrono::milliseconds(100)) == std::future_status::timeout) - << "Call to 'start(...)' should hang until 'change_callback(...)' is actually called!"; - change_callback(); -} - -TEST(ConnectionInitiator, connectionDetectedIfDiscoverCallbackCalledBeforeWait) -{ - ConnectionInitiator initiator; - MockMavsdk mavsdk; - NewSystemCallback change_callback; - - EXPECT_CALL(mavsdk, subscribe_on_new_system(_)).WillOnce(SaveCallback(&change_callback)); - - std::vector> systems; - auto system = std::make_shared(); - systems.push_back(system); - EXPECT_CALL(mavsdk, systems()).WillOnce(testing::Return(systems)); - - EXPECT_CALL(*system, is_connected()).WillOnce(testing::Return(true)); - EXPECT_CALL(*system, has_autopilot()).WillOnce(testing::Return(true)); - - initiator.start(mavsdk, ARBITRARY_CONNECTION_URL); - change_callback(); - initiator.wait(); -} - -TEST(ConnectionInitiator, doesNotCrashIfDiscoverCallbackCalledMoreThanOnce) -{ - ConnectionInitiator initiator; - MockMavsdk mavsdk; - NewSystemCallback change_callback; - EXPECT_CALL(mavsdk, subscribe_on_new_system(_)).WillOnce(SaveCallback(&change_callback)); - - std::vector> systems; - auto system = std::make_shared(); - systems.push_back(system); - EXPECT_CALL(mavsdk, systems()).WillRepeatedly(testing::Return(systems)); - - EXPECT_CALL(*system, is_connected()).WillRepeatedly(testing::Return(true)); - EXPECT_CALL(*system, has_autopilot()).WillOnce(testing::Return(true)); - - initiator.start(mavsdk, ARBITRARY_CONNECTION_URL); - change_callback(); - change_callback(); -} - -} // namespace