diff --git a/cartographer/common/lockless_queue.h b/cartographer/common/lockless_queue.h new file mode 100644 index 0000000..4c5250e --- /dev/null +++ b/cartographer/common/lockless_queue.h @@ -0,0 +1,174 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * 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 CARTOGRAPHER_COMMON_LOCKLESS_QUEUE_H_ +#define CARTOGRAPHER_COMMON_LOCKLESS_QUEUE_H_ + +#include +#include + +#include "cartographer/common/make_unique.h" +#include "glog/logging.h" + +namespace cartographer { +namespace common { + +// Lock-less queue which is thread safe for concurrent data producers and a +// single data consumer. +// +// This lockless queue implementation is adapted from +// https://github.com/resonance-audio/resonance-audio/blob/master/resonance_audio/utils/lockless_task_queue.h +template +class LocklessQueue { + public: + LocklessQueue() { + free_list_head_ = nullptr; + incoming_data_list_head_ = nullptr; + data_list_head_ = nullptr; + data_list_tail_ = nullptr; + } + + ~LocklessQueue() { + FreeNodes(free_list_head_.exchange(nullptr)); + FreeNodes(incoming_data_list_head_.exchange(nullptr)); + FreeNodes(data_list_head_); + } + + // Pushes the data item into the queue. + void Push(std::unique_ptr t) { + Node* const free_node = PopNodeFromFreeList(); + CHECK(free_node); + free_node->data = std::move(t); + PushNodeToList(&incoming_data_list_head_, free_node); + } + + // Pops the oldest data item from the queue. If the queue is empty returns a + // nullptr. + std::unique_ptr Pop() { + SwapLists(); + if (data_list_head_ != nullptr) { + Node* node = data_list_head_; + data_list_head_ = data_list_head_->next; + std::unique_ptr data = std::move(node->data); + PushNodeToList(&free_list_head_, node); + return std::move(data); + } + return nullptr; + } + + private: + // Node to model a single-linked list. + struct Node { + Node() = default; + + // Dummy copy constructor to enable vector::resize allocation. + Node(const Node& node) : next() {} + + // User data. + std::unique_ptr data; + + // Pointer to next node. + std::atomic next; + }; + + // Deallocates all nodes of the list starting with 'head'. + void FreeNodes(Node* node) { + while (node != nullptr) { + Node* next_node_ptr = node->next; + delete node; + node = next_node_ptr; + } + } + + // Pushes a node to the front of a list. + void PushNodeToList(std::atomic* list_head, Node* node) { + DCHECK(list_head); + DCHECK(node); + Node* list_head_ptr; + do { + list_head_ptr = list_head->load(); + node->next = list_head_ptr; + } while (!std::atomic_compare_exchange_strong_explicit( + list_head, &list_head_ptr, node, std::memory_order_release, + std::memory_order_relaxed)); + } + + // Pops a node from the front of the free node list. If the list is empty + // constructs a new node instance. + Node* PopNodeFromFreeList() { + Node* list_head_ptr; + Node* list_head_next_ptr; + do { + list_head_ptr = free_list_head_.load(); + if (list_head_ptr == nullptr) { + return new Node; + } + list_head_next_ptr = list_head_ptr->next.load(); + } while (!std::atomic_compare_exchange_strong_explicit( + &free_list_head_, &list_head_ptr, list_head_next_ptr, + std::memory_order_relaxed, std::memory_order_relaxed)); + return list_head_ptr; + } + + // Swaps the incoming data list for an empty list and appends all items + // to 'data_list_tail_'. + void SwapLists() { + Node* node_itr = incoming_data_list_head_.exchange(nullptr); + if (node_itr == nullptr) { + // There is no data on the incoming list. + return; + } + // The first node of the incoming data list will become the tail of the + // data list. + Node* const data_list_tail = node_itr; + + // Reverses the list order. After this operation 'prev_node_itr' points to + // head of the new data list items. + Node* prev_node_itr = nullptr; + while (node_itr != nullptr) { + Node* const next_node_ptr = node_itr->next; + node_itr->next = prev_node_itr; + prev_node_itr = node_itr; + node_itr = next_node_ptr; + } + + // If the previous data list was empty, replace head rather than appending + // to the list. + if (data_list_tail_ == nullptr) { + data_list_head_ = prev_node_itr; + } else { + data_list_tail_->next = prev_node_itr; + } + data_list_tail_ = data_list_tail; + } + + // Pointer to head node of free list. + std::atomic free_list_head_; + + // Pointer to head node of incoming data list, which is in FILO order. + std::atomic incoming_data_list_head_; + + // Pointer to head node of data list. + Node* data_list_head_; + + // Pointer to tail node of data list. + Node* data_list_tail_; +}; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_LOCKLESS_QUEUE_H_ diff --git a/cartographer/common/lockless_queue_test.cc b/cartographer/common/lockless_queue_test.cc new file mode 100644 index 0000000..5c9b565 --- /dev/null +++ b/cartographer/common/lockless_queue_test.cc @@ -0,0 +1,25 @@ +#include "cartographer/common/lockless_queue.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace common { +namespace { + +TEST(LocklessQueueTest, PushAndPop) { + LocklessQueue queue; + queue.Push(common::make_unique(1)); + queue.Push(common::make_unique(2)); + EXPECT_EQ(*queue.Pop(), 1); + queue.Push(common::make_unique(3)); + queue.Push(common::make_unique(4)); + EXPECT_EQ(*queue.Pop(), 2); + queue.Push(common::make_unique(5)); + EXPECT_EQ(*queue.Pop(), 3); + EXPECT_EQ(*queue.Pop(), 4); + EXPECT_EQ(*queue.Pop(), 5); + EXPECT_EQ(queue.Pop(), nullptr); +} + +} // namespace +} // namespace common +} // namespace cartographer \ No newline at end of file