mavlink command fixes

core/CMakeLists.txt

@@ -93,5 +93,6 @@ list(APPEND UNIT_TEST_SOURCES
     ${CMAKE_SOURCE_DIR}/core/curl_test.cpp
     ${CMAKE_SOURCE_DIR}/core/any_test.cpp
     ${CMAKE_SOURCE_DIR}/core/cli_arg_test.cpp
+    ${CMAKE_SOURCE_DIR}/core/locked_queue_test.cpp
 )
 set(UNIT_TEST_SOURCES ${UNIT_TEST_SOURCES} PARENT_SCOPE)

core/locked_queue.h

@@ -1,52 +1,67 @@
 #pragma once
 
-
 #include <queue>
 #include <mutex>
+#include <memory>
 
 namespace dronecore {
 
 template <class T>
 class LockedQueue
 {
 public:
-    LockedQueue() :
-        _queue(),
-        _mutex()
-    {};
-
+    LockedQueue() {};
+    ~LockedQueue() {};
 
     void push_back(T item)
     {
         std::lock_guard<std::mutex> lock(_mutex);
-
         _queue.push_back(item);
     }
 
-    T &front()
+    // This allows to get access to the front and keep others
+    // from using it. This blocks if the front is already borrowed.
+    std::shared_ptr<T> borrow_front()
     {
-        std::lock_guard<std::mutex> lock(_mutex);
+        _mutex.lock();
+        if (_queue.size() == 0) {
+            // We couldn't borrow anything, therefore don't keep the lock.
+            _mutex.unlock();
+            return nullptr;
+        }
+        return std::make_shared<T>(_queue.front());
+    }
 
-        return _queue.front();
+    // This allows to return a borrowed queue.
+    void return_front()
+    {
+        // We don't know if the mutex is locked and Windows doesn't let us
+        // unlock an unowned mutex.
+        _mutex.try_lock();
+        _mutex.unlock();
     }
 
     void pop_front()
     {
-        std::lock_guard<std::mutex> lock(_mutex);
+        // In case it's not returned, do that now.
+        return_front();
 
+        std::lock_guard<std::mutex> lock(_mutex);
+        if (_queue.size() == 0) {
+            return;
+        }
         _queue.pop_front();
     }
 
     size_t size()
     {
         std::lock_guard<std::mutex> lock(_mutex);
-
         return _queue.size();
     }
 
 private:
-    std::deque<T> _queue;
-    std::mutex _mutex;
+    std::deque<T> _queue {};
+    std::mutex _mutex {};
 };
 
 } // namespace dronecore

core/locked_queue_test.cpp

@@ -0,0 +1,101 @@
+
+#include "locked_queue.h"
+
+#include <string>
+#include <future>
+#include <gtest/gtest.h>
+
+using namespace dronecore;
+
+TEST(LockedQueue, FillAndEmpty)
+{
+    int one = 1;
+    int two = 2;
+    int three = 3;
+
+    LockedQueue<int> locked_queue {};
+
+    locked_queue.push_back(one);
+    EXPECT_EQ(locked_queue.size(), 1);
+    locked_queue.push_back(two);
+    locked_queue.push_back(three);
+    EXPECT_EQ(locked_queue.size(), 3);
+
+    locked_queue.pop_front();
+    EXPECT_EQ(locked_queue.size(), 2);
+    locked_queue.pop_front();
+    locked_queue.pop_front();
+    EXPECT_EQ(locked_queue.size(), 0);
+
+    // Popping an empty queue should just fail silently.
+    locked_queue.pop_front();
+    EXPECT_EQ(locked_queue.size(), 0);
+}
+
+TEST(LockedQueue, BorrowAndReturn)
+{
+    int one = 1;
+    int two = 2;
+    int three = 3;
+
+    LockedQueue<int> locked_queue {};
+
+    locked_queue.push_back(one);
+    locked_queue.push_back(two);
+    locked_queue.push_back(three);
+
+    auto borrowed_item = locked_queue.borrow_front();
+    EXPECT_EQ(*borrowed_item, 1);
+    locked_queue.return_front();
+    locked_queue.pop_front();
+
+    borrowed_item = locked_queue.borrow_front();
+    EXPECT_EQ(*borrowed_item, 2);
+    locked_queue.return_front();
+    // Double returning shouldn't matter.
+    locked_queue.return_front();
+    locked_queue.pop_front();
+
+    borrowed_item = locked_queue.borrow_front();
+    EXPECT_EQ(*borrowed_item, 3);
+    // Popping without returning should automatically return it.
+    locked_queue.pop_front();
+    EXPECT_EQ(locked_queue.size(), 0);
+
+    borrowed_item = locked_queue.borrow_front();
+    EXPECT_EQ(borrowed_item, nullptr);
+}
+
+TEST(LockedQueue, ConcurrantAccess)
+{
+    int one = 1;
+    int two = 2;
+
+    LockedQueue<int> locked_queue {};
+
+    locked_queue.push_back(one);
+    locked_queue.push_back(two);
+
+    auto borrowed_item = locked_queue.borrow_front();
+    EXPECT_EQ(*borrowed_item, 1);
+
+    auto prom = std::make_shared<std::promise<void>>();
+    auto fut = prom->get_future();
+
+    auto some_future = std::async(std::launch::async,
+    [&prom, &locked_queue]() {
+        // This will wait in the lock until the first item is returned.
+        auto second_borrowed_item = locked_queue.borrow_front();
+        locked_queue.return_front();
+        prom->set_value();
+    });
+
+    // The promise should not be fulfilled yet because we have not
+    // returned the borrowed item.
+    auto status = fut.wait_for(std::chrono::milliseconds(10));
+    EXPECT_EQ(status, std::future_status::timeout);
+
+    locked_queue.return_front();
+    status = fut.wait_for(std::chrono::milliseconds(10));
+    EXPECT_EQ(status, std::future_status::ready);
+}