diff --git a/src/algorithms/libs/sensor_data/sensor_data_aggregator.cc b/src/algorithms/libs/sensor_data/sensor_data_aggregator.cc index 69bfeed93..d4204c832 100644 --- a/src/algorithms/libs/sensor_data/sensor_data_aggregator.cc +++ b/src/algorithms/libs/sensor_data/sensor_data_aggregator.cc @@ -37,6 +37,9 @@ SensorDataAggregator::SensorDataAggregator(const SensorDataSourceConfiguration& case SensorDataType::F32: f32_data_[required_sensor] = {}; break; + case SensorDataType::F64: + f64_data_[required_sensor] = {}; + break; // More maps to be populated in the future for different types // For now, all supported sensors are represented as f32 @@ -76,6 +79,16 @@ void SensorDataAggregator::update(const std::vector& tags) sensor_samples.emplace_back(last_sample); } } + for (auto& sensor_data : f64_data_) + { + std::vector>& sensor_samples = sensor_data.second; + if (not sensor_samples.empty()) + { + SensorDataSample last_sample = sensor_samples.back(); + sensor_samples.clear(); + sensor_samples.emplace_back(last_sample); + } + } // More maps to be cleared in the future for different types // For now, all supported sensors are represented as f32 @@ -140,6 +153,56 @@ SensorDataSample SensorDataAggregator::get_average_f32(SensorIdentifier:: } +const std::vector>& SensorDataAggregator::get_f64(SensorIdentifier::value_type sensor_id) const +{ + // The map is populated on construction with empty vectors for each provided sensor. + // If a required sensor is not provided, the error is handled on construction. + return f64_data_.at(sensor_id); +} + + +SensorDataSample SensorDataAggregator::get_last_f64(SensorIdentifier::value_type sensor_id) const +{ + // The map is populated on construction with empty vectors for each provided sensor. + // If a required sensor is not provided, the error is handled on construction. + const std::vector> samples = f64_data_.at(sensor_id); + if (samples.empty()) + { + return {0, 0}; + } + return samples.back(); +} + +SensorDataSample SensorDataAggregator::get_average_f64(SensorIdentifier::value_type sensor_id) const +{ + // The map is populated on construction with empty vectors for each provided sensor. + // If a required sensor is not provided, the error is handled on construction. + const std::vector> samples = f64_data_.at(sensor_id); + if (samples.empty()) + { + return {0, 0}; + } + double acc = 0.0; + double count = 0; + bool first = true; + for (const auto& sample : samples) + { + if (first) + { + first = false; + continue; + } + acc += sample.value; + ++count; + } + if (count == 0) + { + return {samples.back().timestamp, 0}; + } + return {samples.back().timestamp, acc / count}; +} + + void SensorDataAggregator::append_data(const pmt::pmt_t& data_dict) { static pmt::pmt_t SAMPLE_STAMP_KEY = pmt::mp(SensorIdentifier::to_string(SensorIdentifier::SAMPLE_STAMP)); @@ -165,6 +228,12 @@ void SensorDataAggregator::append_data(const pmt::pmt_t& data_dict) f32_data_.at(sensor_id).emplace_back(sample_stamp, pmt::to_float(std::move(val))); } break; + case SensorDataType::F64: + if (f64_data_.find(sensor_id) != f64_data_.end()) + { + f64_data_.at(sensor_id).emplace_back(sample_stamp, pmt::to_double(std::move(val))); + } + break; // More types to be handled in the future for different types // For now, all supported sensors are represented as f32 diff --git a/src/algorithms/libs/sensor_data/sensor_data_aggregator.h b/src/algorithms/libs/sensor_data/sensor_data_aggregator.h index 3f1516c69..89ba123a7 100644 --- a/src/algorithms/libs/sensor_data/sensor_data_aggregator.h +++ b/src/algorithms/libs/sensor_data/sensor_data_aggregator.h @@ -55,12 +55,17 @@ public: SensorDataSample get_last_f32(SensorIdentifier::value_type sensor_id) const; SensorDataSample get_average_f32(SensorIdentifier::value_type sensor_id) const; + const std::vector>& get_f64(SensorIdentifier::value_type sensor_id) const; + SensorDataSample get_last_f64(SensorIdentifier::value_type sensor_id) const; + SensorDataSample get_average_f64(SensorIdentifier::value_type sensor_id) const; + // More getters to be added in the future for different types // For now, all supported sensors are represented as f32 private: void append_data(const pmt::pmt_t& data_dict); std::unordered_map>> f32_data_{}; + std::unordered_map>> f64_data_{}; // More maps to be added in the future for different types // For now, all supported sensors are represented as f32 }; diff --git a/src/algorithms/libs/sensor_data/sensor_identifier.cc b/src/algorithms/libs/sensor_data/sensor_identifier.cc index 293a26874..fede5c9f6 100644 --- a/src/algorithms/libs/sensor_data/sensor_identifier.cc +++ b/src/algorithms/libs/sensor_data/sensor_identifier.cc @@ -33,6 +33,10 @@ static const ConversionEntry conversion_table[] = { {SensorDataType::F64, SensorDataType::F32, [](const pmt::pmt_t& val) { return pmt::from_float(pmt::to_double(val)); }}, {SensorDataType::I32, SensorDataType::F32, [](const pmt::pmt_t& val) { return pmt::from_float(pmt::to_long(val)); }}, {SensorDataType::I64, SensorDataType::F32, [](const pmt::pmt_t& val) { return pmt::from_float(pmt::to_long(val)); }}, + {SensorDataType::F32, SensorDataType::F64, [](const pmt::pmt_t& val) { return pmt::from_double(pmt::to_float(val)); }}, + {SensorDataType::F64, SensorDataType::F64, [](const pmt::pmt_t& val) { return val; }}, + {SensorDataType::I32, SensorDataType::F64, [](const pmt::pmt_t& val) { return pmt::from_double(pmt::to_long(val)); }}, + {SensorDataType::I64, SensorDataType::F64, [](const pmt::pmt_t& val) { return pmt::from_double(pmt::to_long(val)); }}, }; @@ -174,7 +178,7 @@ SensorDataType::value_type SensorIdentifier::get_internal_type(value_type sensor case IMU_ANG_ACC_X: case IMU_ANG_ACC_Y: case IMU_ANG_ACC_Z: - return SensorDataType::F32; + return SensorDataType::F64; default: return SensorDataType::F32; }