feat(sensor_data): added support for double precision (f64) sensor measurements

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<gr::tag_t>& tags)
                     sensor_samples.emplace_back(last_sample);
                 }
         }
+    for (auto& sensor_data : f64_data_)
+        {
+            std::vector<SensorDataSample<double>>& sensor_samples = sensor_data.second;
+            if (not sensor_samples.empty())
+                {
+                    SensorDataSample<double> 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<float> SensorDataAggregator::get_average_f32(SensorIdentifier::
 }
 
 
+const std::vector<SensorDataSample<double>>& 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<double> 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<SensorDataSample<double>> samples = f64_data_.at(sensor_id);
+    if (samples.empty())
+        {
+            return {0, 0};
+        }
+    return samples.back();
+}
+
+SensorDataSample<double> 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<SensorDataSample<double>> 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

src/algorithms/libs/sensor_data/sensor_data_aggregator.h

@@ -55,12 +55,17 @@ public:
     SensorDataSample<float> get_last_f32(SensorIdentifier::value_type sensor_id) const;
     SensorDataSample<float> get_average_f32(SensorIdentifier::value_type sensor_id) const;
 
+    const std::vector<SensorDataSample<double>>& get_f64(SensorIdentifier::value_type sensor_id) const;
+    SensorDataSample<double> get_last_f64(SensorIdentifier::value_type sensor_id) const;
+    SensorDataSample<double> 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<SensorIdentifier::value_type, std::vector<SensorDataSample<float>>> f32_data_{};
+    std::unordered_map<SensorIdentifier::value_type, std::vector<SensorDataSample<double>>> f64_data_{};
     // More maps to be added in the future for different types
     // For now, all supported sensors are represented as f32
 };

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;
         }