libvisiontransfer  8.0.0
sensorringbuffer.h
1 /*******************************************************************************
2  * Copyright (c) 2019 Nerian Vision GmbH
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining a copy
5  * of this software and associated documentation files (the "Software"), to deal
6  * in the Software without restriction, including without limitation the rights
7  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8  * copies of the Software, and to permit persons to whom the Software is
9  * furnished to do so, subject to the following conditions:
10  *
11  * The above copyright notice and this permission notice shall be included in
12  * all copies or substantial portions of the Software.
13  *******************************************************************************/
14 
15 #ifndef VISIONTRANSFER_SENSORRINGBUFFER_H
16 #define VISIONTRANSFER_SENSORRINGBUFFER_H
17 
18 #include <array>
19 #include <vector>
20 #include <chrono>
21 #include <thread>
22 #include <mutex>
23 #include <tuple>
24 
25 #include <visiontransfer/sensordata.h>
26 
27 using namespace visiontransfer;
28 
29 namespace visiontransfer {
30 namespace internal {
31 
41 template<typename RecordType, int RINGBUFFER_SIZE>
43  private:
44  int read_horizon, write_position, read_next;
45  unsigned long lostSamples;
46  std::array<RecordType, RINGBUFFER_SIZE> buffer;
47  std::recursive_mutex mutex;
48  public:
49  constexpr unsigned int ringbufferSize() const { return RINGBUFFER_SIZE; }
50  SensorDataRingBuffer(): read_horizon(0), write_position(0), read_next(0), lostSamples(0) { }
51  constexpr int capacity() const { return ringbufferSize() - 1; }
52  int size() const { return (ringbufferSize() + (write_position - read_next)) % ringbufferSize(); }
53  int samplesLost() const { return lostSamples; }
54  bool isFull() const { return size()==capacity(); }
55  bool isEmpty() const { return write_position==read_next; }
56 
57  bool advanceWritePosition() {
58  write_position = (write_position + 1) % ringbufferSize();
59  if (write_position==read_next) {
60  // Ring buffer overrun: advance and increment lost samples count
61  read_next = (write_position + 1) % ringbufferSize();
62  lostSamples++;
63  }
64  return lostSamples==0;
65  }
66 
67  bool pushData(const std::vector<RecordType>& data) {
68  // A more efficient implementation could be substituted on demand
69  std::unique_lock<std::recursive_mutex> lock(mutex);
70  for (auto const& d: data) {
71  (void) pushData(d);
72  }
73  return lostSamples==0;
74  }
75 
76  bool pushData(const RecordType& data) {
77  std::unique_lock<std::recursive_mutex> lock(mutex);
78  buffer[write_position] = data;
79  return advanceWritePosition();
80  }
81 
82  bool pushData(RecordType&& data) {
83  std::unique_lock<std::recursive_mutex> lock(mutex);
84  buffer[write_position] = std::move(data);
85  return advanceWritePosition();
86  }
87 
88  // \brief Pop and return the whole ring buffer contents
89  std::vector<RecordType> popAllData() {
90  std::unique_lock<std::recursive_mutex> lock(mutex);
91  lostSamples = 0;
92  if (write_position < read_next) {
93  // wrapped
94  std::vector<RecordType> v(buffer.begin()+read_next, buffer.end());
95  v.reserve(v.size() + write_position);
96  std::copy(buffer.begin(), buffer.begin() + write_position, std::back_inserter(v));
97  read_next = (write_position) % ringbufferSize();
98  return v;
99  } else {
100  std::vector<RecordType> v(buffer.begin()+read_next, buffer.begin()+write_position);
101  read_next = (write_position) % ringbufferSize();
102  return v;
103  }
104  }
105 
107  std::vector<RecordType> popBetweenTimes(int fromSec = 0, int fromUSec = 0, int untilSec = 0x7fffFFFFl, int untilUSec = 0x7fffFFFFl) {
108  std::unique_lock<std::recursive_mutex> lock(mutex);
109  lostSamples = 0;
110  int tsSec, tsUSec;
111  if (write_position == read_next) return std::vector<RecordType>();
112  // Find first relevant sample (matching or exceeding the specified start time)
113  buffer[read_next].getTimestamp(tsSec, tsUSec);
114  while ((tsSec < fromSec) || ((tsSec == fromSec) && (tsUSec < fromUSec))) {
115  read_next = (read_next + 1) % ringbufferSize();
116  if (write_position == read_next) return std::vector<RecordType>();
117  }
118  // Find last relevant sample (not exceeding the specified end time)
119  int lastidx = read_next;
120  int li;
121  buffer[lastidx].getTimestamp(tsSec, tsUSec);
122  while ((tsSec < untilSec) || ((tsSec == untilSec) && (tsUSec <= untilUSec))) {
123  li = (lastidx + 1) % ringbufferSize();
124  lastidx = li;
125  if (li == write_position) break;
126  }
127  if (lastidx < read_next) {
128  // Wrapped
129  std::vector<RecordType> v(buffer.begin()+read_next, buffer.end());
130  v.reserve(v.size() + lastidx);
131  std::copy(buffer.begin(), buffer.begin() + lastidx, std::back_inserter(v));
132  read_next = lastidx;
133  return v;
134  } else {
135  std::vector<RecordType> v(buffer.begin()+read_next, buffer.begin()+lastidx);
136  read_next = (lastidx) % ringbufferSize();
137  return v;
138  }
139  }
140 };
141 
142 }} // namespace
143 
144 #endif
145 
146 
std::vector< RecordType > popBetweenTimes(int fromSec=0, int fromUSec=0, int untilSec=0x7fffFFFFl, int untilUSec=0x7fffFFFFl)
Pop and return the data between timestamps (or the whole ring buffer contents if not provided) ...
Nerian Vision Technologies