core: potentially fix timeout handler

I have seen curious cases where timeouts just didn't seem to trigger as
the they should have. My guess was that the void * pointer to the
location of the shared_ptr in the map was just a bad idea in the first
place. So I changed it to just a uint64_t handle which is easier to
debug and should always be unique. I also changed from a std::map to a
std::vector, however, the jury is out on what is actually faster for our
usage.

src/mavsdk/core/timeout_handler.cpp

@@ -6,21 +6,19 @@ TimeoutHandler::TimeoutHandler(Time& time) : _time(time) {}
 
 void TimeoutHandler::add(std::function<void()> callback, double duration_s, void** cookie)
 {
-    auto new_timeout = std::make_shared<Timeout>();
-    new_timeout->callback = callback;
-    new_timeout->time = _time.steady_time_in_future(duration_s);
-    new_timeout->duration_s = duration_s;
-
-    void* new_cookie = static_cast<void*>(new_timeout.get());
-
-    {
-        std::lock_guard<std::mutex> lock(_timeouts_mutex);
-        _timeouts.insert(std::pair<void*, std::shared_ptr<Timeout>>(new_cookie, new_timeout));
-    }
+    std::lock_guard<std::mutex> lock(_timeouts_mutex);
+    auto new_timeout = Timeout{};
+    new_timeout.callback = callback;
+    new_timeout.time = _time.steady_time_in_future(duration_s);
+    new_timeout.duration_s = duration_s;
+    new_timeout.cookie = _next_cookie++;
+    _timeouts.push_back(new_timeout);
 
     if (cookie != nullptr) {
-        *cookie = new_cookie;
+        // TODO: change this API in the future
+        *cookie = reinterpret_cast<void*>(new_timeout.cookie);
     }
+    _iterator_invalidated = true;
 }
 
 void TimeoutHandler::refresh(const void* cookie)
@@ -31,10 +29,12 @@ void TimeoutHandler::refresh(const void* cookie)
 
     std::lock_guard<std::mutex> lock(_timeouts_mutex);
 
-    auto it = _timeouts.find(const_cast<void*>(cookie));
+    auto it = std::find_if(_timeouts.begin(), _timeouts.end(), [&](const Timeout& timeout) {
+        return timeout.cookie == reinterpret_cast<uint64_t>(cookie);
+    });
     if (it != _timeouts.end()) {
-        auto future_time = _time.steady_time_in_future(it->second->duration_s);
-        it->second->time = future_time;
+        auto future_time = _time.steady_time_in_future(it->duration_s);
+        it->time = future_time;
     }
 }
 
@@ -46,46 +46,47 @@ void TimeoutHandler::remove(const void* cookie)
 
     std::lock_guard<std::mutex> lock(_timeouts_mutex);
 
-    auto it = _timeouts.find(const_cast<void*>(cookie));
+    auto it = std::find_if(_timeouts.begin(), _timeouts.end(), [&](auto& timeout) {
+        return timeout.cookie == reinterpret_cast<uint64_t>(cookie);
+    });
+
     if (it != _timeouts.end()) {
-        _timeouts.erase(const_cast<void*>(cookie));
+        _timeouts.erase(it);
         _iterator_invalidated = true;
     }
 }
 
 void TimeoutHandler::run_once()
 {
-    _timeouts_mutex.lock();
+    std::unique_lock<std::mutex> lock(_timeouts_mutex);
 
     auto now = _time.steady_time();
 
     for (auto it = _timeouts.begin(); it != _timeouts.end(); /* no ++it */) {
         // If time is passed, call timeout callback.
-        if (it->second->time < now) {
-            if (it->second->callback) {
-                // Get a copy for the callback because we will remove it.
-                std::function<void()> callback = it->second->callback;
+        if (it->time < now) {
+            // Get a copy for the callback because we will remove it.
+            auto callback = it->callback;
 
-                // Self-destruct before calling to avoid locking issues.
-                _timeouts.erase(it++);
+            // Self-destruct before calling to avoid locking issues.
+            it = _timeouts.erase(it);
 
-                // Unlock while we callback because it might in turn want to add timeouts.
-                _timeouts_mutex.unlock();
+            if (callback) {
+                // Unlock while we call back because it might in turn want to add timeouts.
+                lock.unlock();
                 callback();
-                _timeouts_mutex.lock();
-            }
+                lock.lock();
 
+                // We start over if anyone has messed with this while we called the callback.
+                if (_iterator_invalidated) {
+                    _iterator_invalidated = false;
+                    it = _timeouts.begin();
+                }
+            }
         } else {
             ++it;
         }
-
-        // We start over if anyone has messed with this while we called the callback.
-        if (_iterator_invalidated) {
-            _iterator_invalidated = false;
-            it = _timeouts.begin();
-        }
     }
-    _timeouts_mutex.unlock();
 }
 
 } // namespace mavsdk

src/mavsdk/core/timeout_handler.h

@@ -1,10 +1,12 @@
 #pragma once
 
+#include "mavsdk_time.h"
+
+#include <cstdint>
 #include <mutex>
 #include <memory>
 #include <functional>
-#include <unordered_map>
-#include "mavsdk_time.h"
+#include <vector>
 
 namespace mavsdk {
 
@@ -30,13 +32,16 @@ class TimeoutHandler {
         std::function<void()> callback{};
         SteadyTimePoint time{};
         double duration_s{0.0};
+        uint64_t cookie{0};
     };
 
-    std::unordered_map<void*, std::shared_ptr<Timeout>> _timeouts{};
+    std::vector<Timeout> _timeouts{};
     std::mutex _timeouts_mutex{};
     bool _iterator_invalidated{false};
 
     Time& _time;
+
+    uint64_t _next_cookie{1};
 };
 
 } // namespace mavsdk