Switch to std::priority_queue

Signed-off-by: Christophe Bedard <[email protected]>

rosbag2_transport/CMakeLists.txt

@@ -115,7 +115,6 @@ if(BUILD_TESTING)
   add_definitions(-D_SRC_RESOURCES_DIR_PATH="${CMAKE_CURRENT_SOURCE_DIR}/test/resources")
 
   find_package(ament_cmake_gmock REQUIRED)
-  find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_index_cpp REQUIRED)
   find_package(ament_lint_auto REQUIRED)
   find_package(composition_interfaces REQUIRED)
@@ -537,15 +536,6 @@ if(BUILD_TESTING)
     rcpputils::rcpputils
     rosbag2_test_common::rosbag2_test_common
   )
-
-  ament_add_gtest(test_meta_priority_queue
-    test/rosbag2_transport/test_meta_priority_queue.cpp)
-  target_include_directories(test_meta_priority_queue PRIVATE
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/rosbag2_transport>)
-  target_link_libraries(test_meta_priority_queue
-    rcpputils::rcpputils
-    shared_queues_vendor::singleproducerconsumer
-  )
 endif()
 
 ament_package()

rosbag2_transport/package.xml

@@ -27,7 +27,6 @@
   <depend>keyboard_handler</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
-  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_index_cpp</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

rosbag2_transport/src/rosbag2_transport/locked_priority_queue.hpp

@@ -0,0 +1,106 @@
+// Copyright 2024 Apex.AI, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROSBAG2_TRANSPORT__LOCKED_PRIORITY_QUEUE_HPP_
+#define ROSBAG2_TRANSPORT__LOCKED_PRIORITY_QUEUE_HPP_
+
+#include <mutex>
+#include <queue>
+
+#include "rcpputils/thread_safety_annotations.hpp"
+#include "rcpputils/unique_lock.hpp"
+
+/// \brief `std::priority_queue` wrapper with locks.
+/// \tparam T the element type
+/// \tparam Container the underlying container type
+/// \tparam Compare the comparator
+/// \see std::priority_queue
+template<typename T, typename Container, typename Compare>
+class LockedPriorityQueue
+{
+public:
+  /// \brief Constructor.
+  /// \param compare the comparator object
+  /// \param empty_element the element to return when trying to take with an empty queue
+  LockedPriorityQueue(const Compare & compare, const T & empty_element)
+  : queue_(compare),
+    empty_element_{empty_element}
+  {}
+
+  LockedPriorityQueue() = delete;
+  LockedPriorityQueue(const LockedPriorityQueue &) = delete;
+  LockedPriorityQueue & operator=(const LockedPriorityQueue &) = delete;
+  LockedPriorityQueue(const LockedPriorityQueue &&) = delete;
+  LockedPriorityQueue & operator=(const LockedPriorityQueue &&) = delete;
+
+  /// \brief Insert element and sort queue.
+  /// \param element the element
+  void push(const T & element)
+  {
+    rcpputils::unique_lock<std::mutex> lk(queue_mutex_);
+    queue_.push(element);
+  }
+
+  /// \brief Remove the top element.
+  void pop()
+  {
+    rcpputils::unique_lock<std::mutex> lk(queue_mutex_);
+    queue_.pop();
+  }
+
+  /// \brief Check if the queue is empty.
+  /// \return whether the queue is empty
+  bool empty() const
+  {
+    rcpputils::unique_lock<std::mutex> lk(queue_mutex_);
+    return queue_.empty();
+  }
+
+  /// \brief Get the number of elements.
+  /// \return the number of elements
+  std::size_t size() const
+  {
+    rcpputils::unique_lock<std::mutex> lk(queue_mutex_);
+    return queue_.size();
+  }
+
+  /// Remove all elements from the queue.
+  void purge()
+  {
+    rcpputils::unique_lock<std::mutex> lk(queue_mutex_);
+    while (!queue_.empty()) {
+      queue_.pop();
+    }
+  }
+
+  /// \brief Try to take the top element from the queue.
+  /// \return the top element, or `empty_element` if the queue is empty
+  T take()
+  {
+    rcpputils::unique_lock<std::mutex> lk(queue_mutex_);
+    if (queue_.empty()) {
+      return empty_element_;
+    }
+    const T e = queue_.top();
+    queue_.pop();
+    return e;
+  }
+
+private:
+  mutable std::mutex queue_mutex_;
+  std::priority_queue<T, Container, Compare> queue_ RCPPUTILS_TSA_GUARDED_BY(queue_mutex_);
+  const T empty_element_;
+};
+
+#endif  // ROSBAG2_TRANSPORT__LOCKED_PRIORITY_QUEUE_HPP_