franky 1.1.4
A High-Level Motion API for Franka
Loading...
Searching...
No Matches
wait_free_triple_buffer.hpp
Go to the documentation of this file.
1#pragma once
2
3#include <array>
4#include <atomic>
5#include <cstdint>
6
7namespace franky {
8
16template <typename T>
18 public:
20
22
26 void set(const T &value) {
27 // 1. Write the payload into our privately owned write buffer
28 buffers_[active_write_index_] = value;
29 last_written_index_ = active_write_index_;
30
31 // 2. Pack the write index (bits 0-1) and set the "new data" flag (bit 2)
32 const uint8_t new_state = active_write_index_ | 0x04;
33
34 // 3. Atomically publish our buffer to the shared pool and take the old buffer.
35 // Memory order acq_rel ensures the payload write finishes before the index swap.
36 const uint8_t old_state = shared_state_.exchange(new_state, std::memory_order_acq_rel);
37
38 // 4. Our next write will go to the buffer that the reader just gave up (or never used).
39 active_write_index_ = old_state & 0x03;
40 }
41
49 [[nodiscard]] T getLastWritten() const { return buffers_[last_written_index_]; }
50
56 [[nodiscard]] T get() {
57 const uint8_t current_state = shared_state_.load(std::memory_order_acquire);
58
59 // If the writer has published a new buffer since we last checked...
60 if ((current_state & 0x04) != 0) {
61 // Atomically hand our current read buffer back to the shared pool
62 // (with the "new" flag cleared) and take ownership of the newest buffer.
63 const uint8_t old_state = shared_state_.exchange(active_read_index_, std::memory_order_acq_rel);
64 active_read_index_ = old_state & 0x03;
65 }
66
67 return buffers_[active_read_index_];
68 }
69
70 private:
71 std::array<T, 3> buffers_{};
72
73 // Start indices offset from one another so they never point to the same buffer initially.
74 uint8_t active_write_index_{1};
75 uint8_t active_read_index_{2};
76
77 // Owned by the writer thread: index of the buffer holding the last written value.
78 // A published buffer is never written again until the writer reclaims it in a
79 // later set(), so the writer may safely read it back without synchronization.
80 uint8_t last_written_index_{0};
81
82 // Packs both the shared clean index (bits 0-1) and the "new data" flag (bit 2).
83 // Initial state: index 0, new data flag = 0.
84 std::atomic<uint8_t> shared_state_{0};
85};
86
87} // namespace franky
Wait-free, Single-Producer Single-Consumer (SPSC) triple buffer.
Definition wait_free_triple_buffer.hpp:17
void set(const T &value)
Publish new data.
Definition wait_free_triple_buffer.hpp:26
T get()
Get the most recently published data.
Definition wait_free_triple_buffer.hpp:56
WaitFreeTripleBuffer(const T &initial_value)
Definition wait_free_triple_buffer.hpp:21
T getLastWritten() const
Get the most recently written value from the writer's side.
Definition wait_free_triple_buffer.hpp:49
Definition dynamics_limit.cpp:8
std::array< double, dims > toStdD(const Eigen::Matrix< double, dims, 1 > &vector)
Definition util.hpp:18