lock_free_pooled_queue.hpp

// Copyright 2025 PAL Robotics S.L.
//
// 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 REALTIME_TOOLS__LOCK_FREE_POOLED_QUEUE_HPP_
#define REALTIME_TOOLS__LOCK_FREE_POOLED_QUEUE_HPP_
 
#include <array>
#include <cstddef>
#include <type_traits>
#include <utility>
 
#include "realtime_tools/lock_free_queue.hpp"
 
namespace realtime_tools
{
template <typename T, std::size_t Capacity>
class LockFreePooledSPSCQueue
{
  static_assert(Capacity > 0, "Capacity must be greater than zero");
 
public:
  LockFreePooledSPSCQueue()
  {
    for (std::size_t i = 0; i < Capacity; ++i) {
      (void)free_.push(i);  // cannot fail, the free queue has room for every slot
    }
  }
 
  template <typename Fill>
  [[nodiscard]] bool produce(Fill && fill)
  {
    std::size_t index;
    if (!free_.pop(index)) {
      return false;
    }
    fill(slots_[index]);
    return filled_.push(index);  // cannot fail, at most Capacity indices exist
  }
 
  // SFINAE instead of a C++20 requires-clause so the header stays usable from C++17 code. U must be
  // T, otherwise an explicit push<U>() / pop<U>() would bypass the nothrow check on T.
  template <
    typename U = T,
    std::enable_if_t<std::is_same_v<U, T> && std::is_nothrow_move_assignable_v<U>, int> = 0>
  [[nodiscard]] bool push(T && value)
  {
    return produce([&value](T & slot) { slot = std::move(value); });
  }
 
  template <typename Use>
  [[nodiscard]] bool consume(Use && use)
  {
    std::size_t index;
    if (!filled_.pop(index)) {
      return false;
    }
    use(slots_[index]);
    return free_.push(index);  // cannot fail, at most Capacity indices exist
  }
 
  template <
    typename U = T,
    std::enable_if_t<std::is_same_v<U, T> && std::is_nothrow_move_assignable_v<U>, int> = 0>
  [[nodiscard]] bool pop(T & out)
  {
    return consume([&out](T & slot) { out = std::move(slot); });
  }
 
  bool empty() const { return filled_.empty(); }
 
private:
  std::array<T, Capacity> slots_;
  LockFreeSPSCQueue<std::size_t, Capacity> filled_;
  LockFreeSPSCQueue<std::size_t, Capacity> free_;
};
}  // namespace realtime_tools
#endif  // REALTIME_TOOLS__LOCK_FREE_POOLED_QUEUE_HPP_