Queue library (#25)

* feat: separated queue into a shared library

* fix: compile structure done
feat: tests added

* fix: queue tests finalised

* fix: header-only library built

* fix: linter errors fixed

* fix: requested changes implemented
fix: added header-only include location correctly

packages/camera/CMakeLists.txt

@@ -15,24 +15,34 @@ find_package(yaml_cpp_vendor REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(Boost REQUIRED)
 find_package(camera_srvs REQUIRED)
+find_package(common REQUIRED)
 
 add_library(mvsdk SHARED IMPORTED)
 set_target_properties(mvsdk PROPERTIES IMPORTED_LOCATION "/lib/libMVSDK.so")
 
+add_library(queue_lib INTERFACE)
+
 add_executable(camera src/camera_main.cpp src/camera.cpp src/params.cpp
                       src/camera_status.cpp)
 add_executable(calibration src/calibration_main.cpp src/calibration.cpp
                            src/params.cpp)
 
 target_include_directories(
   camera PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-                "/usr/include")
+                "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>" "/usr/include")
+
 target_include_directories(
   calibration PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                      "/usr/include")
 
-ament_target_dependencies(camera rclcpp cv_bridge OpenCV sensor_msgs
-                          yaml_cpp_vendor)
+ament_target_dependencies(
+  camera
+  rclcpp
+  cv_bridge
+  OpenCV
+  sensor_msgs
+  yaml_cpp_vendor
+  common)
 
 ament_target_dependencies(
   calibration
@@ -49,7 +59,7 @@ ament_target_dependencies(
   Boost
   camera_srvs)
 
-target_link_libraries(camera mvsdk)
+target_link_libraries(camera mvsdk common::common)
 
 install(TARGETS camera calibration DESTINATION lib/${PROJECT_NAME})
 

packages/camera/include/camera.h

@@ -6,7 +6,7 @@
 #include <opencv2/core/core.hpp>
 
 #include "CameraApi.h"
-#include "lock_free_queue.h"
+#include "common/lock_free_queue.h"
 #include "params.h"
 
 #include <chrono>

packages/camera/package.xml

@@ -21,6 +21,7 @@
   <depend>yaml_cpp_vendor</depend>
   <depend>Boost</depend>
   <depend>camera_srvs</depend>
+  <depend>common</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

packages/common/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.8)
+project(common)
+
+find_package(ament_cmake REQUIRED)
+
+add_library(${PROJECT_NAME} INTERFACE)
+target_include_directories(
+  ${PROJECT_NAME}
+  INTERFACE "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+            "$<INSTALL_INTERFACE:include>")
+
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT "export_${PROJECT_NAME}"
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES
+  DESTINATION include)
+
+install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory("tests")
+endif()
+
+ament_export_targets("export_${PROJECT_NAME}")
+ament_package()

packages/camera/include/lock_free_queue.h → packages/common/include/lock_free_queue.h

@@ -34,7 +34,7 @@ class LockFreeQueue {
         // while could not proccess
         while (true) {
             int snap = tail_.load();
-            if (snap - head_.load() == data_.size()) {
+            if (snap - head_.load() == static_cast<int>(data_.size())) {
                 // buffer is full and can't be updated
                 // in fact, slot can be freed during verification, but we do not double-check
                 return false;

packages/common/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>common</name>
+  <version>0.0.0</version>
+  <description>common</description>
+  <license>MIT</license>
+  <maintainer email="[email protected]">Denis Bakin</maintainer>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <build_depend>builtin_interfaces</build_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

packages/common/tests/CMakeLists.txt

@@ -0,0 +1,4 @@
+ament_add_gtest(queue_test src/test.cpp)
+target_include_directories(
+  queue_test PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>")
+target_link_libraries(queue_test ${PROJECT_NAME})

packages/common/tests/src/test.cpp

@@ -0,0 +1,124 @@
+#include "lock_free_queue.h"
+
+#include <gtest/gtest.h>
+
+#include <thread>
+#include <vector>
+
+TEST(Correctness, push) {
+    handy::LockFreeQueue<int> queue(2);
+    ASSERT_TRUE(queue.push(2));
+    ASSERT_TRUE(queue.push(2));
+    ASSERT_FALSE(queue.push(2));
+    ASSERT_FALSE(queue.push(2));
+}
+
+TEST(Correctness, pop) {
+    int value;
+    handy::LockFreeQueue<int> queue(2);
+    ASSERT_FALSE(queue.pop(value));
+    ASSERT_FALSE(queue.pop(value));
+}
+
+TEST(Correctness, pushpop) {
+    int value = 0;
+    handy::LockFreeQueue<int> queue(2);
+    ASSERT_TRUE(queue.push(1));
+    ASSERT_TRUE(queue.pop(value));
+    ASSERT_EQ(value, 1);
+    ASSERT_FALSE(queue.pop(value));
+
+    ASSERT_TRUE(queue.push(2));
+    ASSERT_TRUE(queue.push(3));
+    ASSERT_FALSE(queue.push(4));
+
+    ASSERT_TRUE(queue.pop(value));
+    ASSERT_EQ(value, 2);
+    ASSERT_TRUE(queue.pop(value));
+    ASSERT_EQ(value, 3);
+
+    ASSERT_FALSE(queue.pop(value));
+}
+
+TEST(Performance, NoSpuriousFails) {
+    const int n = 1024 * 1024;
+    const int n_threads = 8;
+    handy::LockFreeQueue<int> queue(n * n_threads);
+
+    std::vector<std::thread> writers;
+    // check that all threads managed to push elements without fake fails
+    for (int i = 0; i < n_threads; i++) {
+        writers.emplace_back([&] {  // NOLINT
+            for (int j = 0; j < n; ++j) {
+                ASSERT_TRUE(queue.push(0));
+            }
+        });
+    }
+
+    for (auto& t : writers) {
+        t.join();
+    }
+
+    std::vector<std::thread> readers;
+    // check that all threads managed to pop elements without fake fails
+    // and no elements were lost
+    for (int i = 0; i < n_threads; i++) {
+        readers.emplace_back([&] {  // NOLINT
+            for (int j = 0; j < n; ++j) {
+                int k;
+                ASSERT_TRUE(queue.pop(k));
+            }
+        });
+    }
+
+    for (auto& t : readers) {
+        t.join();
+    }
+}
+
+TEST(Performance, NoQueueLock) {
+    const int n = 1024 * 1024;
+    const int n_threads = 8;
+    handy::LockFreeQueue<int> queue(64);
+
+    std::vector<std::thread> threads;
+    int id = 0;
+    for (int i = 0; i < n_threads; i++) {
+        const int current_id = id++;
+        if (current_id % 2 == 0) {
+            threads.emplace_back([&] {  // NOLINT
+                for (int cnt = 0; cnt < n; ++cnt) {
+                    queue.push(0);
+                }
+            });
+        } else {
+            threads.emplace_back([&] {  // NOLINT
+                for (int cnt = 0; cnt < n; ++cnt) {
+                    int _;
+                    queue.pop(_);
+                }
+            });
+        }
+    }
+
+    for (auto& t : threads) {
+        t.join();
+    }
+
+    int k;
+    // empty queue
+    while (queue.pop(k)) {
+        queue.pop(k);
+    }
+    // check that it still works
+    ASSERT_TRUE(queue.push(0));
+    ASSERT_TRUE(queue.pop(k));
+    ASSERT_EQ(k, 0);
+    // must be empty
+    ASSERT_FALSE(queue.pop(k));
+}
+
+int main(int argc, char** argv) {
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}