SF2 C++ FRC Class Library
Sensor Fusion Framework (SF2) for FRC
OrientationHistory.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 #ifndef SRC_ORIENTATION_ORIENTATIONHISTORY_H_
25 #define SRC_ORIENTATION_ORIENTATIONHISTORY_H_
26 
27 #include <string>
28 #include <cmath>
29 #include <vector>
30 using namespace std;
31 
32 #include "sensor/ISensorDataSource.h"
33 #include "sensor/SensorDataSourceInfo.h"
34 #include "sensor/ISensorInfo.h"
35 #include "time/ThreadsafeInterpolatingTimeHistory.h"
36 #include "orientation/Quaternion.h"
37 #include "time/TimestampedValue.h"
38 #include "quantity/Scalar.h"
39 
57  ISensorDataSource* p_quat_sensor;
58  SensorDataSourceInfo* p_quat_data_source_info;
60  Scalar temp_s;
61  int quaternion_quantity_index;
62  int timestamp_quantity_index;
64  Timestamp system_timestamp;
65 
66 public:
67 
68  const int MAX_ORIENTATION_HISTORY_LENGTH_NUM_SAMPLES = 1000;
69 
85  int history_length_num_samples) {
86  this->p_orientation_history = NULL;
87  this->p_quat_sensor = &p_sensor->getSensorDataSource();
88 
89  int index = 0;
90  quaternion_quantity_index = -1;
91  timestamp_quantity_index = -1;
92  vector<SensorDataSourceInfo *> sensor_data_source_infos;
93  p_quat_sensor->getSensorDataSourceInfos(
94  sensor_data_source_infos);
95  for (SensorDataSourceInfo* item : sensor_data_source_infos) {
96  if (item->getName().compare("Quaternion") == 0) {
97  quaternion_quantity_index = index;
98  p_quat_data_source_info = item;
99  }
100  if (item->getName().compare("Timestamp") == 0) {
101  timestamp_quantity_index = index;
102  }
103  index++;
104  }
105 
106  if (quaternion_quantity_index == -1) {
107  printf( "The provided ISensorInfo (quat_sensor) object"
108  "must contain a SensorDataSourceInfo object named 'Quaternion'.");
109  return;
110  }
111 
112  if (history_length_num_samples
113  > MAX_ORIENTATION_HISTORY_LENGTH_NUM_SAMPLES) {
114  history_length_num_samples =
115  MAX_ORIENTATION_HISTORY_LENGTH_NUM_SAMPLES;
116  }
117  Quaternion default_quat;
118  TimestampedValue<Quaternion> default_ts_quat(default_quat);
119  string data_source_name = p_quat_data_source_info->getName();
120  this->p_orientation_history = new ThreadsafeInterpolatingTimeHistory<
121  TimestampedValue<Quaternion>>(default_ts_quat,
122  history_length_num_samples,
123  p_sensor->getSensorTimestampInfo(),
124  data_source_name);
125 
126  p_quat_sensor->subscribe(this);
127  }
128 
129  virtual ~OrientationHistory() {
130  delete p_orientation_history;
131  }
132 
137  void reset() {
138  p_orientation_history->reset();
139  }
140 
146  return p_orientation_history->getMostRecent(out);
147  }
148 
159  bool getQuaternionAtTime(long requested_timestamp,
161  return p_orientation_history->get(requested_timestamp, out);
162  }
163 
173  float getYawDegreesAtTime(long requested_timestamp) {
175  if (getQuaternionAtTime(requested_timestamp, match)) {
176  match.getValue().getYawRadians(temp_s);
177  return temp_s.get() * Angle::Degrees::RADIANS_TO_DEGREES;
178  } else {
179  return NAN;
180  }
181  }
182 
192  float getPitchDegreesAtTime(long requested_timestamp) {
194  if (getQuaternionAtTime(requested_timestamp, match)) {
195  match.getValue().getPitch(temp_s);
196  return temp_s.get() * Angle::Degrees::RADIANS_TO_DEGREES;
197  } else {
198  return NAN;
199  }
200  }
201 
211  float getRollDegreesAtTime(long requested_timestamp) {
213  if (getQuaternionAtTime(requested_timestamp, match)) {
214  match.getValue().getRoll(temp_s);
215  return temp_s.get() * Angle::Degrees::RADIANS_TO_DEGREES;
216  } else {
217  return NAN;
218  }
219  }
220 
221  virtual void publish(vector<IQuantity*> curr_values,
222  Timestamp& sys_timestamp) {
223  Timestamp* p_sensor_timestamp = &sys_timestamp;
224  if ( timestamp_quantity_index != -1 ) {
225  p_sensor_timestamp = ((Timestamp *)curr_values[timestamp_quantity_index]);
226  }
227  Quaternion* p_q = ((Quaternion *) curr_values[quaternion_quantity_index]);
228  temp_tsq.set(*p_q, p_sensor_timestamp->getMilliseconds());
229  p_orientation_history->add(temp_tsq);
230  }
231 
232  bool writeToDirectory(string directory_path) {
233  return p_orientation_history->writeToDirectory(directory_path);
234  }
235 
236  bool writeToFile(string file_path) {
237  return p_orientation_history->writeToFile(file_path);
238  }
239 };
240 
241 #endif /* SRC_ORIENTATION_ORIENTATIONHISTORY_H_ */
void add(T &t)
Adds the provided object to the ThreadsafeInterpolatingTimeHistory.
Definition: ThreadsafeInterpolatingTimeHistory.h:144
Definition: SensorDataSourceInfo.h:37
bool get(long requested_timestamp, T &out)
Retrieves the object in the ThreadsafeInterpolatingTimeHistory which matches the provided timestamp...
Definition: ThreadsafeInterpolatingTimeHistory.h:166
OrientationHistory(ISensorInfo *p_sensor, int history_length_num_samples)
Constructs an OrientationHistory object with a specified size.
Definition: OrientationHistory.h:84
float getPitchDegreesAtTime(long requested_timestamp)
Retrieves the pitch angle in degrees at the specified sensor timestamp.
Definition: OrientationHistory.h:192
The Quaternion class provides methods to operate on a quaternion.
Definition: Quaternion.h:76
void reset()
Clears all contents of the ThreadsafeInterpolatingTimeHistory by marking all contained objects as inv...
Definition: ThreadsafeInterpolatingTimeHistory.h:122
Definition: ThreadsafeInterpolatingTimeHistory.h:75
void set(TimestampedValue< T > &src)
Initalizes this TimestampedValue to be equal to the source TimestampedValue.
Definition: TimestampedValue.h:98
Interface to be implemented by any subscriber of Sensor Data of type T.
Definition: ISensorDataSubscriber.h:40
void reset()
Reset the OrientationHistory, clearing all existing entries.
Definition: OrientationHistory.h:137
virtual bool subscribe(ISensorDataSubscriber *subscriber)=0
Subscribes the provided subscriber object for callbacks whenever new TimestampedQuaternion data is re...
Definition: Scalar.h:35
float getRollDegreesAtTime(long requested_timestamp)
Retrieves the roll angle in degrees at the specified sensor timestamp.
Definition: OrientationHistory.h:211
float getYawDegreesAtTime(long requested_timestamp)
Retrieves the yaw angle in degrees at the specified sensor timestamp.
Definition: OrientationHistory.h:173
bool getCurrentQuaternion(TimestampedValue< Quaternion > &out)
Retrieves the most recently added Quaternion.
Definition: OrientationHistory.h:145
void getRoll(Scalar &roll)
Extracts the roll angle value from the Quaternion.
Definition: Quaternion.h:218
string getName()
Returns the name of this sensor data.
Definition: SensorDataSourceInfo.h:61
The ITimestampedQuaternionSensor interface should be implemented by any sensor which generates timest...
Definition: ISensorDataSource.h:42
Definition: Timestamp.h:30
The OrientationHistory class implements a timestamped history of orientation data (e...
Definition: OrientationHistory.h:56
void getYawRadians(Scalar &yaw)
Extracts the yaw angle value from the Quaternion.
Definition: Quaternion.h:198
Definition: ISensorInfo.h:34
bool getMostRecent(T &out)
Retrieves the most recently-added object in the ThreadsafeInterpolatingTimeHistory.
Definition: ThreadsafeInterpolatingTimeHistory.h:248
bool getQuaternionAtTime(long requested_timestamp, TimestampedValue< Quaternion > &out)
Retrieves the TimestampedQuaterion at the specified sensor timestamp.
Definition: OrientationHistory.h:159
void getPitch(Scalar &pitch)
Extracts the pitch angle value from the Quaternion.
Definition: Quaternion.h:208