24 #ifndef SRC_ORIENTATION_ORIENTATIONHISTORY_H_
25 #define SRC_ORIENTATION_ORIENTATIONHISTORY_H_
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"
61 int quaternion_quantity_index;
62 int timestamp_quantity_index;
68 const int MAX_ORIENTATION_HISTORY_LENGTH_NUM_SAMPLES = 1000;
85 int history_length_num_samples) {
86 this->p_orientation_history = NULL;
87 this->p_quat_sensor = &p_sensor->getSensorDataSource();
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);
96 if (item->getName().compare(
"Quaternion") == 0) {
97 quaternion_quantity_index = index;
98 p_quat_data_source_info = item;
100 if (item->getName().compare(
"Timestamp") == 0) {
101 timestamp_quantity_index = index;
106 if (quaternion_quantity_index == -1) {
107 printf(
"The provided ISensorInfo (quat_sensor) object"
108 "must contain a SensorDataSourceInfo object named 'Quaternion'.");
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;
119 string data_source_name = p_quat_data_source_info->
getName();
122 history_length_num_samples,
123 p_sensor->getSensorTimestampInfo(),
130 delete p_orientation_history;
138 p_orientation_history->
reset();
161 return p_orientation_history->
get(requested_timestamp, out);
175 if (getQuaternionAtTime(requested_timestamp, match)) {
177 return temp_s.get() * Angle::Degrees::RADIANS_TO_DEGREES;
194 if (getQuaternionAtTime(requested_timestamp, match)) {
196 return temp_s.get() * Angle::Degrees::RADIANS_TO_DEGREES;
213 if (getQuaternionAtTime(requested_timestamp, match)) {
214 match.getValue().
getRoll(temp_s);
215 return temp_s.get() * Angle::Degrees::RADIANS_TO_DEGREES;
221 virtual void publish(vector<IQuantity*> curr_values,
223 Timestamp* p_sensor_timestamp = &sys_timestamp;
224 if ( timestamp_quantity_index != -1 ) {
225 p_sensor_timestamp = ((
Timestamp *)curr_values[timestamp_quantity_index]);
228 temp_tsq.
set(*p_q, p_sensor_timestamp->getMilliseconds());
229 p_orientation_history->
add(temp_tsq);
232 bool writeToDirectory(
string directory_path) {
233 return p_orientation_history->writeToDirectory(directory_path);
236 bool writeToFile(
string file_path) {
237 return p_orientation_history->writeToFile(file_path);
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...
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