diff --git a/rosbag2_transport/src/rosbag2_transport/locked_priority_queue.hpp b/rosbag2_transport/src/rosbag2_transport/locked_priority_queue.hpp index 5bebb14f8..b3809ee4a 100644 --- a/rosbag2_transport/src/rosbag2_transport/locked_priority_queue.hpp +++ b/rosbag2_transport/src/rosbag2_transport/locked_priority_queue.hpp @@ -16,6 +16,7 @@ #define ROSBAG2_TRANSPORT__LOCKED_PRIORITY_QUEUE_HPP_ #include +#include #include #include "rcpputils/thread_safety_annotations.hpp" @@ -32,10 +33,8 @@ 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(const Compare & compare) + : queue_(compare) {} LockedPriorityQueue() = delete; @@ -85,12 +84,12 @@ class LockedPriorityQueue } /// \brief Try to take the top element from the queue. - /// \return the top element, or `empty_element` if the queue is empty - T take() + /// \return the top element, or `std::nullopt` if the queue is empty + std::optional take() { rcpputils::unique_lock lk(queue_mutex_); if (queue_.empty()) { - return empty_element_; + return std::nullopt; } const T e = queue_.top(); queue_.pop(); @@ -100,7 +99,6 @@ class LockedPriorityQueue private: mutable std::mutex queue_mutex_; std::priority_queue queue_ RCPPUTILS_TSA_GUARDED_BY(queue_mutex_); - const T empty_element_; }; #endif // ROSBAG2_TRANSPORT__LOCKED_PRIORITY_QUEUE_HPP_ diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 5b874897a..17dde0168 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -388,8 +388,7 @@ PlayerImpl::PlayerImpl( : input_bags_(std::move(input_bags)), owner_(owner), play_options_(play_options), - // Return empty shared_ptr when taking from an empty queue - message_queue_(BagMessageRecvTimestampComparator{}, nullptr), + message_queue_(BagMessageRecvTimestampComparator{}), keyboard_handler_(std::move(keyboard_handler)), player_service_client_manager_(std::make_shared()) { @@ -698,7 +697,7 @@ rosbag2_storage::SerializedBagMessageSharedPtr PlayerImpl::take_next_message_fro std::this_thread::sleep_for(std::chrono::microseconds(100)); } // Note: this only returns nullptr if no more messages - return message_queue_.take(); + return message_queue_.take().value_or(nullptr); } bool PlayerImpl::play_next()