From 71126ac267a10d1c5a7fcb7a3c963688b2b32277 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 Sep 2024 07:54:11 +1200 Subject: [PATCH] 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 | 59 ++++------ src/mavsdk_server/src/mavsdk_server.cpp | 3 +- src/mavsdk_server/test/CMakeLists.txt | 1 - .../test/connection_initiator_test.cpp | 111 ------------------ 4 files changed, 24 insertions(+), 150 deletions(-) delete mode 100644 src/mavsdk_server/test/connection_initiator_test.cpp diff --git a/src/mavsdk_server/src/connection_initiator.h b/src/mavsdk_server/src/connection_initiator.h index 95cfafc798..7e87188c90 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 9702fce49f..e11f97eb00 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 153f1bf491..eff0e065af 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 fcca8b3a42..0000000000 --- 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