From 5934879ae77e5bd39c0f1911645f537e9958a5f4 Mon Sep 17 00:00:00 2001 From: Victor Castillo Date: Mon, 9 Jun 2025 04:28:28 +0200 Subject: [PATCH] feat: added more data types --- .../sensor_data/sensor_data_aggregator.cc | 14 +++++- .../libs/sensor_data/sensor_data_aggregator.h | 5 ++- .../libs/sensor_data/sensor_data_file.cc | 9 +++- .../libs/sensor_data/sensor_data_source.cc | 4 +- .../sensor_data_source_configuration.cc | 2 +- .../libs/sensor_data/sensor_data_type.cc | 44 ++++++++++++++++--- .../libs/sensor_data/sensor_data_type.h | 8 +++- .../libs/sensor_data/sensor_identifier.cc | 11 +++-- .../libs/sensor_data/sensor_identifier.h | 4 +- 9 files changed, 81 insertions(+), 20 deletions(-) diff --git a/src/algorithms/libs/sensor_data/sensor_data_aggregator.cc b/src/algorithms/libs/sensor_data/sensor_data_aggregator.cc index e9d69e153..49454128b 100644 --- a/src/algorithms/libs/sensor_data/sensor_data_aggregator.cc +++ b/src/algorithms/libs/sensor_data/sensor_data_aggregator.cc @@ -32,9 +32,13 @@ SensorDataAggregator::SensorDataAggregator(const SensorDataSourceConfiguration& // Populate sensor sample maps switch (SensorIdentifier::get_internal_type(required_sensor)) { - case SensorDataType::FLOAT: + case SensorDataType::F32: f32_data_[required_sensor] = {}; break; + + // More maps to be populated in the future for different types + // For now, all supported sensors are represented as f32 + default: break; } @@ -69,6 +73,8 @@ void SensorDataAggregator::update(const std::vector& tags) 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 // Append new data for (const auto& sensor_tag : tags) @@ -119,7 +125,7 @@ void SensorDataAggregator::append_data(const pmt::pmt_t& data_dict) { switch (SensorIdentifier::get_internal_type(sensor_id)) { - case SensorDataType::FLOAT: + case SensorDataType::F32: if (f32_data_.contains(sensor_id)) { f32_data_.at(sensor_id).emplace_back( @@ -127,6 +133,10 @@ void SensorDataAggregator::append_data(const pmt::pmt_t& data_dict) pmt::to_float(val)); } break; + + // More types to be handled in the future for different types + // For now, all supported sensors are represented as f32 + default: break; } diff --git a/src/algorithms/libs/sensor_data/sensor_data_aggregator.h b/src/algorithms/libs/sensor_data/sensor_data_aggregator.h index edd577fea..9c5033a02 100644 --- a/src/algorithms/libs/sensor_data/sensor_data_aggregator.h +++ b/src/algorithms/libs/sensor_data/sensor_data_aggregator.h @@ -46,13 +46,16 @@ public: void update(const std::vector& tags); const std::vector>& get_f32(SensorIdentifier::value_type sensor_id) const; - SensorDataSample get_last_f32(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_{}; + // 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_data_file.cc b/src/algorithms/libs/sensor_data/sensor_data_file.cc index 694986a09..6e1ca21ee 100644 --- a/src/algorithms/libs/sensor_data/sensor_data_file.cc +++ b/src/algorithms/libs/sensor_data/sensor_data_file.cc @@ -25,7 +25,7 @@ SensorDataFile::SensorDataFile( const std::size_t& item_size, const bool& repeat) : path_(path), - file_(path_), + file_(path_, std::ios::binary), sample_delay_(sample_delay), sample_period_(sample_period), offset_in_file_(offset_in_file), @@ -44,6 +44,7 @@ SensorDataFile::SensorDataFile( void SensorDataFile::reset() { + file_.clear(); file_.seekg(offset_in_file_, std::ios_base::beg); offset_in_io_buffer_ = io_buffer_size_; done_ = false; @@ -97,6 +98,12 @@ void SensorDataFile::read_into_io_buffer() { reset(); file_.read(reinterpret_cast(&io_buffer_[bytes_read]), io_buffer_size_ - bytes_read); + const std::size_t new_bytes_read = file_.gcount(); + if (new_bytes_read < io_buffer_size_ - bytes_read) + { + // Buffer is too big for this file + io_buffer_size_ = bytes_read + new_bytes_read; + } } else { diff --git a/src/algorithms/libs/sensor_data/sensor_data_source.cc b/src/algorithms/libs/sensor_data/sensor_data_source.cc index 87bb1f072..e7462040c 100644 --- a/src/algorithms/libs/sensor_data/sensor_data_source.cc +++ b/src/algorithms/libs/sensor_data/sensor_data_source.cc @@ -110,7 +110,9 @@ int SensorDataSource::work(int noutput_items, data_tag = pmt::dict_add(data_tag, CHUNK_COUNT_KEY, pmt::from_long(data_file->get_chunks_read())); for (const auto& sensor : sensor_config_map_.at(file_id)) { - data_tag = pmt::dict_add(data_tag, sensor.tag_key, SensorDataType::make_value(sensor.type, &chunk[sensor.offset])); + pmt::pmt_t raw_value = SensorDataType::make_value(sensor.type, &chunk[sensor.offset]); + pmt::pmt_t value = SensorIdentifier::convert_to_internal_type(sensor.identifier, sensor.type, raw_value); + data_tag = pmt::dict_add(data_tag, sensor.tag_key, value); } add_item_tag(0, sample_stamp, TAG_KEY, data_tag); } diff --git a/src/algorithms/libs/sensor_data/sensor_data_source_configuration.cc b/src/algorithms/libs/sensor_data/sensor_data_source_configuration.cc index b806e35af..4ae8cd922 100644 --- a/src/algorithms/libs/sensor_data/sensor_data_source_configuration.cc +++ b/src/algorithms/libs/sensor_data/sensor_data_source_configuration.cc @@ -127,7 +127,7 @@ void SensorDataSourceConfiguration::configure_sensors(const ConfigurationInterfa } // Configure sensor data type, default to same data type as previous sensor - SensorDataType::value_type data_type = SensorDataType::FLOAT; + SensorDataType::value_type data_type = SensorDataType::F32; if (id > 0 and not configuration->is_present(role + ".type")) { data_type = sensors_[id - 1].type; diff --git a/src/algorithms/libs/sensor_data/sensor_data_type.cc b/src/algorithms/libs/sensor_data/sensor_data_type.cc index 5d94b2e4c..61060a7ac 100644 --- a/src/algorithms/libs/sensor_data/sensor_data_type.cc +++ b/src/algorithms/libs/sensor_data/sensor_data_type.cc @@ -32,9 +32,21 @@ SensorDataType::value_type SensorDataType::from_string(const std::string& s) { return SensorDataType::UINT64; } - else if (str == "FLOAT") + else if (str == "F32") { - return SensorDataType::FLOAT; + return SensorDataType::F32; + } + else if (str == "F64") + { + return SensorDataType::F64; + } + else if (str == "I32") + { + return SensorDataType::I32; + } + else if (str == "I64") + { + return SensorDataType::I64; } throw std::runtime_error{"Unknown sensor data type: " + s}; } @@ -45,10 +57,16 @@ std::string SensorDataType::to_string(const SensorDataType::value_type& v) { case SensorDataType::UINT64: return "UINT64"; - case SensorDataType::FLOAT: - return "FLOAT"; + case SensorDataType::F32: + return "F32"; + case SensorDataType::F64: + return "F64"; + case SensorDataType::I32: + return "I32"; + case SensorDataType::I64: + return "I64"; default: - return "UNKNOWN SENSOR"; + return "UNKNOWN TYPE"; } } @@ -58,8 +76,14 @@ uint64_t SensorDataType::get_size(const SensorDataType::value_type& v) { case SensorDataType::UINT64: return sizeof(uint64_t); - case SensorDataType::FLOAT: + case SensorDataType::F32: return sizeof(float); + case SensorDataType::F64: + return sizeof(double); + case SensorDataType::I32: + return sizeof(int); + case SensorDataType::I64: + return sizeof(long); default: return 0UL; } @@ -71,8 +95,14 @@ pmt::pmt_t SensorDataType::make_value(const SensorDataType::value_type& v, void* { case SensorDataType::UINT64: return pmt::from_uint64(*static_cast(value)); - case SensorDataType::FLOAT: + case SensorDataType::F32: return pmt::from_float(*static_cast(value)); + case SensorDataType::F64: + return pmt::from_double(*static_cast(value)); + case SensorDataType::I32: + return pmt::from_long(long{*static_cast(value)}); + case SensorDataType::I64: + return pmt::from_long(*static_cast(value)); default: throw std::runtime_error{"Unknown sensor data type: " + to_string(v)}; } diff --git a/src/algorithms/libs/sensor_data/sensor_data_type.h b/src/algorithms/libs/sensor_data/sensor_data_type.h index 645cc7901..77ca25ac3 100644 --- a/src/algorithms/libs/sensor_data/sensor_data_type.h +++ b/src/algorithms/libs/sensor_data/sensor_data_type.h @@ -32,8 +32,12 @@ struct SensorDataType SensorDataType() = delete; enum value_type { - UINT64, - FLOAT + UINT64, // Used internally for SAMPLE_STAMP and CHUNK_COUNT + F32, + F64, + I32, + I64, + // More types can be added here, don't forget to update the static functions }; static value_type from_string(const std::string& s); diff --git a/src/algorithms/libs/sensor_data/sensor_identifier.cc b/src/algorithms/libs/sensor_data/sensor_identifier.cc index aea7906a1..607d5dc11 100644 --- a/src/algorithms/libs/sensor_data/sensor_identifier.cc +++ b/src/algorithms/libs/sensor_data/sensor_identifier.cc @@ -16,6 +16,7 @@ #include "sensor_identifier.h" #include "sensor_data_type.h" +#include #include #include @@ -28,7 +29,10 @@ struct ConversionEntry }; static const ConversionEntry conversion_table[] = { - {SensorDataType::FLOAT, SensorDataType::FLOAT, [](const pmt::pmt_t& val) { return val; }}, + {SensorDataType::F32, SensorDataType::F32, [](const pmt::pmt_t& val) { return val; }}, + {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)); }}, }; const ConversionEntry* lookup_conversion(SensorDataType::value_type from_type, SensorDataType::value_type to_type) @@ -168,9 +172,9 @@ 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::FLOAT; + return SensorDataType::F32; default: - return SensorDataType::FLOAT; + return SensorDataType::F32; } } @@ -180,6 +184,7 @@ pmt::pmt_t SensorIdentifier::convert_to_internal_type(value_type sensor_id, Sens const ConversionEntry* conversion = lookup_conversion(original_type, get_internal_type(sensor_id)); if (conversion == nullptr) { + throw std::runtime_error("Could not convert sensor value to internal type"); } return conversion->conversion_fun(value); diff --git a/src/algorithms/libs/sensor_data/sensor_identifier.h b/src/algorithms/libs/sensor_data/sensor_identifier.h index e6776b3b9..206df7b83 100644 --- a/src/algorithms/libs/sensor_data/sensor_identifier.h +++ b/src/algorithms/libs/sensor_data/sensor_identifier.h @@ -31,8 +31,8 @@ struct SensorIdentifier SensorIdentifier() = delete; enum value_type : unsigned short { - SAMPLE_STAMP = 0, - CHUNK_COUNT, + SAMPLE_STAMP = 0, // Used internally + CHUNK_COUNT, // Used internally IMU_VEL_X, IMU_VEL_Y, IMU_VEL_Z,