SF2 C++ FRC Class Library
Sensor Fusion Framework (SF2) for FRC
ThreadsafeInterpolatingTimeHistory.h
1 /* ============================================
2  SF2 source code is placed under the MIT license
3  Copyright (c) 2017 Kauai Labs
4 
5  Permission is hereby granted, free of charge, to any person obtaining a copy
6  of this software and associated documentation files (the "Software"), to deal
7  in the Software without restriction, including without limitation the rights
8  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9  copies of the Software, and to permit persons to whom the Software is
10  furnished to do so, subject to the following conditions:
11 
12  The above copyright notice and this permission notice shall be included in
13  all copies or substantial portions of the Software.
14 
15  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
21  THE SOFTWARE.
22  ===============================================
23  */
24 
25 #ifndef SRC_TIME_THREADSAFEINTERPOLATINGTIMEHISTORY_H_
26 #define SRC_TIME_THREADSAFEINTERPOLATINGTIMEHISTORY_H_
27 
46 #include <forward_list>
47 #include <vector>
48 #include <string>
49 #include <climits>
50 #include <regex>
51 #include <mutex>
52 #include "../unit/Unit.h"
53 #include "../platform/File.h"
54 #include "TimestampInfo.h"
55 using namespace std;
56 
58  string& prefix;
59  string& suffix;
60 public:
61  CustomFilenameFilter(string& prefix, string& suffix) :
62  prefix(prefix),
63  suffix(suffix){
64  }
65  bool accept(File& dir, const string& name) {
66  bool starts_with = (name.find(prefix) == 0);
67  bool ends_with = (name.find(suffix)
68  == name.size() - suffix.size() - 1);
69  return starts_with && ends_with;
70  }
72 };
73 
74 template<typename T>
76  TimestampInfo ts_info;
77  mutex list_mutex;
78  vector<T *> history;
79  int history_size;
80  int curr_index;
81  int num_valid_samples;
82  T default_obj;
83  string value_name;
84 
85 public:
93  ThreadsafeInterpolatingTimeHistory(T& default_obj, int num_samples,
94  TimestampInfo& ts_info, string& name) :
95  ts_info(ts_info),
96  list_mutex() {
97  history_size = num_samples;
98 
99  for ( int i = 0; i < num_samples; i++) {
100  T *p_t = new T();
101  history.push_back(p_t);
102  }
103 
104  this->default_obj = default_obj;
105  curr_index = 0;
106  num_valid_samples = 0;
107  this->value_name = name;
108  }
109 
111  {
112  for ( size_t i = 0; i < history.size(); i++) {
113  delete history[i];
114  history[i] = NULL;
115  }
116  }
117 
122  void reset() {
123  std::unique_lock<mutex> sync(list_mutex);
124  for (int i = 0; i < history_size; i++) {
125  T* p_t = history[i];
126  p_t->setValid(false);
127  }
128  curr_index = 0;
129  num_valid_samples = 0;
130  }
131 
137  return num_valid_samples;
138  }
139 
144  void add(T& t) {
145  std::unique_lock<mutex> sync(list_mutex);
146  T* p_existing = history[curr_index];
147  p_existing->copy(t);
148  curr_index++;
149  if (curr_index >= history_size) {
150  curr_index = 0;
151  }
152  if (num_valid_samples < history_size) {
153  num_valid_samples++;
154  }
155  }
156 
166  bool get(long requested_timestamp, T& out) {
167  bool success = false;
168  T* p_match = NULL;
169  long nearest_preceding_timestamp = LONG_MIN;
170  long nearest_preceding_timestamp_delta = LONG_MIN;
171  T *p_nearest_preceding_obj = NULL;
172  long nearest_following_timestamp_delta = LONG_MAX;
173  T *p_nearest_following_obj = NULL;
174  bool copy_object = true;
175  {
176  std::unique_lock<mutex> sync(list_mutex);
177 
178  int entry_index = curr_index;
179  for (int i = 0; i < num_valid_samples; i++) {
180  T *p_obj = history[entry_index];
181  long entry_timestamp = p_obj->getTimestamp();
182  long delta = entry_timestamp - requested_timestamp;
183  if (delta < 0) {
184  if (delta > nearest_preceding_timestamp_delta) {
185  nearest_preceding_timestamp_delta = delta;
186  nearest_preceding_timestamp = entry_timestamp;
187  p_nearest_preceding_obj = p_obj;
188  /* To optimize, break out once both nearest preceding
189  * and following entries are found. This optimization
190  * relies on entries being in descending timestamp
191  * order, beginning with the current entry.
192  */
193  if (p_nearest_following_obj != NULL)
194  break;
195  }
196  } else if (delta > 0) {
197  if (delta < nearest_following_timestamp_delta) {
198  nearest_following_timestamp_delta = delta;
199  p_nearest_following_obj = p_obj;
200  }
201  } else { /* entry_timestamp == requested_timestamp */
202  p_match = p_obj;
203  break;
204  }
205  entry_index--;
206  if (entry_index < 0) {
207  entry_index = history_size - 1;
208  }
209  }
210 
211  /* If a match was not found, and the requested timestamp falls
212  * within two entries in the history, interpolate an intermediate
213  * value.
214  */
215  if ((p_match == NULL) && (p_nearest_preceding_obj != NULL)
216  && (p_nearest_following_obj != NULL)) {
217  double timestamp_delta = nearest_following_timestamp_delta
218  - nearest_preceding_timestamp_delta;
219  double requested_timestamp_offset = requested_timestamp
220  - nearest_preceding_timestamp;
221  double requested_timestamp_ratio = requested_timestamp_offset
222  / timestamp_delta;
223 
224  p_nearest_preceding_obj->interpolate(*p_nearest_following_obj,
225  requested_timestamp_ratio, out);
226  out.setInterpolated(true);
227  copy_object = false;
228  success = true;
229  }
230 
231  if ((p_match != NULL) && copy_object) {
232  /* Make a copy of the object, so that caller does not directly reference
233  * an object within the volatile (threadsafe) history.
234  */
235  out.copy(*p_match);
236  out.setInterpolated(false);
237  success = true;
238  }
239  }
240 
241  return success;
242  }
243 
248  bool getMostRecent(T& out) {
249  T* p_most_recent_t = NULL;
250  {
251  std::unique_lock<mutex> sync(list_mutex);
252 
253  if (num_valid_samples > 0) {
254  int curr_idx = this->curr_index;
255  curr_idx--;
256  if (curr_idx < 0) {
257  curr_idx = (history_size - 1);
258  }
259  p_most_recent_t = history[curr_idx];
260  if (!p_most_recent_t->getValid()) {
261  p_most_recent_t = NULL;
262  }
263  }
264  }
265  if (p_most_recent_t != NULL) {
266  /* Make a copy of the object, so that caller does not directly
267  * reference an object within the volatile (threadsafe) history. */
268  out.copy(*p_most_recent_t);
269  return true;
270  } else {
271  return false;
272  }
273  }
274 
275  bool writeToDirectory(string& directory) {
276  File dir(directory);
277  if (!dir.isDirectory() || !dir.canWrite()) {
278  printf("Directory parameter '%s' must be a writable directory.", directory.c_str());
279  return false;
280  }
281 
282  if ((directory[(directory.length() - 1)] != '/')
283  && (directory[(directory.length() - 1)] != '\\')) {
284  directory += File::separatorChar;
285  }
286 
287  string filename_prefix = value_name + "History";
288  string filename_suffix = "csv";
289 
290  File f(directory);
291  forward_list<File *> matching_files;
292  CustomFilenameFilter filter(filename_prefix, filename_suffix);
293  f.listFiles(matching_files, filter);
294 
295  int next_available_index = -1;
296 
297  for (File *p_matching_file : matching_files) {
298  string file_name = p_matching_file->getName();
299  regex expression("[.][^.]+$");
300  string replacement("");
301  string file_name_prefix = regex_replace(file_name, expression,
302  replacement);
303  string file_counter = file_name_prefix.substr(
304  filename_prefix.length());
305  int counter = atoi(file_counter.c_str());
306  if (counter > next_available_index) {
307  next_available_index = counter;
308  }
309  delete p_matching_file;
310  }
311 
312  next_available_index++;
313 
314  string new_filename = filename_prefix + std::to_string(next_available_index);
315  return writeToFile(directory + new_filename + "." + filename_suffix);
316  }
317 
318  bool writeToFile(const string& file_path) {
319  PrintWriter out(file_path);
320  bool success = writeToDiskInternal(out);
321  out.close();
322  return success;
323  }
324 
325  bool writeToDiskInternal(PrintWriter& out) {
326  bool success = true;
327  // Write header
328  int oldest_index;
329  int num_to_write;
330  if (num_valid_samples > 0) {
331  if (num_valid_samples == history_size) {
332  oldest_index = curr_index + 1;
333  if (oldest_index >= history_size) {
334  oldest_index = 0;
335  }
336  num_to_write = num_valid_samples;
337  } else { /* List is not completely filled */
338  oldest_index = 0;
339  num_to_write = curr_index + 1;
340  }
341  T *p_first_entry = history[oldest_index];
342  vector<string> quantity_names;
343  IQuantity& quantity = p_first_entry->getQuantity();
344  bool is_quantity_container = quantity.getContainedQuantityNames(
345  quantity_names);
346  /* Write Header */
347  string header = "Timestamp";
348  if (is_quantity_container) {
349  for (string& quantity_name : quantity_names) {
350  header += "," + value_name + "." + quantity_name;
351  }
352  } else {
353  header += "," + value_name;
354  }
355  out.println(header);
356 
357  for (int i = 0; i < num_to_write; i++) {
358  vector<string> value_string;
359  T* p_entry_to_write = history[oldest_index++];
360  quantity = p_entry_to_write->getQuantity();
361  value_string.push_back(std::to_string(p_entry_to_write->getTimestamp()));
362  value_string.push_back(",");
363  if (is_quantity_container) {
364  vector<IQuantity *> contained_quantities;
365  quantity.getContainedQuantities(contained_quantities);
366  int index = 0;
367  for (IQuantity* contained_quantity : contained_quantities) {
368  vector<string> printable_string;
369  if (index++ != 0) {
370  value_string.push_back(",");
371  }
372  contained_quantity->getPrintableString(
373  printable_string);
374  for (auto string_part : printable_string) {
375  value_string.push_back(string_part);
376  }
377  }
378  } else {
379  quantity.getPrintableString(value_string);
380  }
381  string output_string;
382  for (auto string_part : value_string) {
383  output_string += string_part;
384  }
385  out.println(output_string);
386  if (oldest_index >= history_size) {
387  oldest_index = 0;
388  }
389  if (oldest_index == curr_index) {
390  break;
391  }
392  }
393  }
394  return success;
395  }
396 };
397 
398 #endif /* SRC_TIME_THREADSAFEINTERPOLATINGTIMEHISTORY_H_ */
void add(T &t)
Adds the provided object to the ThreadsafeInterpolatingTimeHistory.
Definition: ThreadsafeInterpolatingTimeHistory.h:144
ThreadsafeInterpolatingTimeHistory(T &default_obj, int num_samples, TimestampInfo &ts_info, string &name)
Constructs a ThreadsafeInterpolatingTimeHihstory to hold up to a specified number of objects of the s...
Definition: ThreadsafeInterpolatingTimeHistory.h:93
Definition: File.h:49
Definition: TimestampInfo.h:28
int getValidSampleCount()
Returns the current count of valid objects in this ThreadsafeInterpolatingTimeHistory.
Definition: ThreadsafeInterpolatingTimeHistory.h:136
void reset()
Clears all contents of the ThreadsafeInterpolatingTimeHistory by marking all contained objects as inv...
Definition: ThreadsafeInterpolatingTimeHistory.h:122
Definition: ThreadsafeInterpolatingTimeHistory.h:75
Definition: File.h:42
Definition: ThreadsafeInterpolatingTimeHistory.h:57
Definition: IQuantity.h:33
bool getMostRecent(T &out)
Retrieves the most recently-added object in the ThreadsafeInterpolatingTimeHistory.
Definition: ThreadsafeInterpolatingTimeHistory.h:248
Definition: File.h:114