From b86f7b17930d0af734ef06485fc01ea965446a09 Mon Sep 17 00:00:00 2001 From: Christian Brommer Date: Thu, 11 Apr 2024 10:32:36 +0200 Subject: [PATCH 1/5] fix: set default buffer size in constructor, preventing double print of buffer size --- source/mars/include/mars/buffer.h | 2 +- source/mars/include/mars/core_logic.h | 4 ++-- source/mars/source/buffer.cpp | 6 ++++++ 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/source/mars/include/mars/buffer.h b/source/mars/include/mars/buffer.h index c0fa3a6..77c5f3f 100644 --- a/source/mars/include/mars/buffer.h +++ b/source/mars/include/mars/buffer.h @@ -38,7 +38,7 @@ class Buffer /// \brief Buffer default constructor /// max buffer size is set to 400 by default /// - Buffer() = default; + Buffer(); /// /// \brief Buffer constructor diff --git a/source/mars/include/mars/core_logic.h b/source/mars/include/mars/core_logic.h index bd29887..5ee5b6d 100644 --- a/source/mars/include/mars/core_logic.h +++ b/source/mars/include/mars/core_logic.h @@ -30,8 +30,8 @@ class CoreLogic EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::shared_ptr core_states_{ nullptr }; /// Holds a pointer to the core_states - Buffer buffer_{ 300 }; /// Main buffer of the filter - Buffer buffer_prior_core_init_{ 100 }; /// Buffer that holds measurements prior initialization + Buffer buffer_; /// Main buffer of the filter + Buffer buffer_prior_core_init_; /// Buffer that holds measurements prior initialization SensorManager sensor_manager_; bool core_is_initialized_{ false }; /// core_is_initialized_ = true if the core state was initialized, false /// otherwise diff --git a/source/mars/source/buffer.cpp b/source/mars/source/buffer.cpp index eaf9a12..d72e984 100644 --- a/source/mars/source/buffer.cpp +++ b/source/mars/source/buffer.cpp @@ -15,6 +15,12 @@ namespace mars { +Buffer::Buffer() +{ + this->set_max_buffer_size(300); + std::cout << "Created: Buffer (Size=" << max_buffer_size_ << " (DEFAULT))" << std::endl; +} + Buffer::Buffer(const int& size) { this->set_max_buffer_size(size); From 87b4590d8d0e91c04a927186f92496604bd6868a Mon Sep 17 00:00:00 2001 From: Christian Brommer Date: Wed, 29 May 2024 10:51:09 +0200 Subject: [PATCH 2/5] feat: revised buffer implementation --- source/mars/include/mars/buffer.h | 27 +- source/mars/include/mars/core_logic.h | 13 +- .../include/mars/data_utils/read_baro_data.h | 4 +- .../mars/include/mars/data_utils/read_csv.h | 27 +- .../include/mars/data_utils/read_gps_data.h | 4 +- .../mars/data_utils/read_gps_w_vel_data.h | 4 +- .../include/mars/data_utils/read_imu_data.h | 4 +- .../include/mars/data_utils/read_mag_data.h | 4 +- .../include/mars/data_utils/read_pose_data.h | 4 +- .../mars/data_utils/read_position_data.h | 4 +- .../include/mars/data_utils/read_sim_data.h | 7 +- .../mars/data_utils/read_velocity_data.h | 4 +- .../mars/data_utils/read_vision_data.h | 4 +- .../include/mars/sensors/gps/gps_utils.cpp | 2 +- .../mars/sensors/pressure/pressure_utils.cpp | 2 +- .../mars/sensors/update_sensor_abs_class.h | 2 +- .../mars/type_definitions/buffer_data_type.h | 66 +- .../mars/type_definitions/buffer_entry_type.h | 47 +- source/mars/source/buffer.cpp | 202 +-- source/mars/source/buffer_entry_type.cpp | 80 +- source/mars/source/core_logic.cpp | 337 +++-- .../mars_e2e_imu_pose_update.cpp | 17 +- .../tests/mars-e2e-test/mars_e2e_imu_prop.cpp | 8 +- .../mars_e2e_imu_prop_empty_updates.cpp | 13 +- source/tests/mars-test/mars_buffer.cpp | 1296 ++++++++++------- source/tests/mars-test/mars_buffer_type.cpp | 40 +- source/tests/mars-test/mars_core_logic.cpp | 21 +- source/tests/mars-test/mars_gps_utils.cpp | 16 +- 28 files changed, 1278 insertions(+), 981 deletions(-) diff --git a/source/mars/include/mars/buffer.h b/source/mars/include/mars/buffer.h index 77c5f3f..9c9c1cd 100644 --- a/source/mars/include/mars/buffer.h +++ b/source/mars/include/mars/buffer.h @@ -211,10 +211,12 @@ class Buffer /// /// \brief Deletes all states after, and including the given index + /// This also deletes all auto generated states + /// /// \param idx Start index after which all states are deleted /// \return true if function was performed correct, false otherwise /// - bool DeleteStatesStartingAtIdx(const int& idx); + bool ClearStatesStartingAtIdx(const int& idx); /// /// \brief Checks if all buffer entrys are correctly sorted by time @@ -238,18 +240,6 @@ class Buffer /// bool InsertDataAtIndex(const BufferEntryType& new_entry, const int& index); - /// - /// \brief InsertIntermediateData Insert data during the intermediate propagation step - /// - /// This function is intended for inserting a auto generated measurement and state before a current measurement entry. - /// This is required for intermediate propagation information. - /// - /// \param measurement - /// \param state - /// \return - /// - bool InsertIntermediateData(const BufferEntryType& measurement, const BufferEntryType& state); - /// /// \brief get_latest_interm_entrie Get last state pair of imu prop and sensor update /// @@ -261,14 +251,15 @@ class Buffer /// /// \return True if successfull, false of no pair was found /// - bool get_intermediate_entry_pair(const std::shared_ptr& sensor_handle, BufferEntryType* imu_state, BufferEntryType* sensor_state) const; + bool get_intermediate_entry_pair(const std::shared_ptr& sensor_handle, BufferEntryType* imu_state, + BufferEntryType* sensor_state) const; /// - /// \brief CheckForLastHandle Checks if the given sensor handle only exists once in the buffer - /// \param sensor_handle - /// \return true if current sensor handle is the last in the buffer, false otherwise + /// \brief CheckForLastSensorHandleWithState Checks if the given sensor handle only exists once in the buffer and if + /// it has a state \param sensor_handle \return true if current sensor handle is the last in the buffer, false + /// otherwise /// - bool CheckForLastSensorHandlePair(const std::shared_ptr& sensor_handle) const; + bool CheckForLastSensorHandleWithState(const std::shared_ptr& sensor_handle) const; /// /// \brief RemoveOverflowEntrys Removes the oldest entries if max buffer size is reached diff --git a/source/mars/include/mars/core_logic.h b/source/mars/include/mars/core_logic.h index 5ee5b6d..076c52a 100644 --- a/source/mars/include/mars/core_logic.h +++ b/source/mars/include/mars/core_logic.h @@ -30,8 +30,8 @@ class CoreLogic EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::shared_ptr core_states_{ nullptr }; /// Holds a pointer to the core_states - Buffer buffer_; /// Main buffer of the filter - Buffer buffer_prior_core_init_; /// Buffer that holds measurements prior initialization + Buffer buffer_; /// Main buffer of the filter + Buffer buffer_prior_core_init_; /// Buffer that holds measurements prior initialization SensorManager sensor_manager_; bool core_is_initialized_{ false }; /// core_is_initialized_ = true if the core state was initialized, false /// otherwise @@ -72,8 +72,7 @@ class CoreLogic /// /// \brief PerformSensorUpdate Returns new state with corrected state and updated covariance /// - bool PerformSensorUpdate(BufferEntryType* state_buffer_entry_return, std::shared_ptr sensor, - const Time& timestamp, std::shared_ptr data); + bool PerformSensorUpdate(std::shared_ptr sensor, const Time& timestamp, BufferEntryType* sensor_data); /// /// \brief PerformCoreStatePropagation Propagates the core state and returns the new state entry /// @@ -84,9 +83,9 @@ class CoreLogic /// The core_state propagation function needs to be able to /// handle the data structure of the propagation sensor. /// - BufferEntryType PerformCoreStatePropagation(std::shared_ptr sensor, const Time& timestamp, - const std::shared_ptr& data_measurement, - const std::shared_ptr& prior_state_entry); + void PerformCoreStatePropagation(std::shared_ptr sensor, const Time& timestamp, + const BufferEntryType& prior_state_entry, BufferEntryType* sensor_entry); + /// /// \brief ReworkBufferStartingAtIndex Reprocesses the buffer after an out of order update, /// starting at given 'idx' diff --git a/source/mars/include/mars/data_utils/read_baro_data.h b/source/mars/include/mars/data_utils/read_baro_data.h index ecf0dfc..a24d2f8 100644 --- a/source/mars/include/mars/data_utils/read_baro_data.h +++ b/source/mars/include/mars/data_utils/read_baro_data.h @@ -45,9 +45,9 @@ class ReadBarometerData double pressure(csv_data["p"][k]); BufferDataType data; - data.set_sensor_data(std::make_shared(pressure, temperature)); + data.set_measurement(std::make_shared(pressure, temperature)); - BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement); + BufferEntryType current_entry(time, data, sensor); data_out->at(k) = current_entry; } } diff --git a/source/mars/include/mars/data_utils/read_csv.h b/source/mars/include/mars/data_utils/read_csv.h index 36c8a5c..6b1e9f6 100644 --- a/source/mars/include/mars/data_utils/read_csv.h +++ b/source/mars/include/mars/data_utils/read_csv.h @@ -30,8 +30,9 @@ class ReadCsv { char delim{ ',' }; HeaderMapType header_map; + public: - ReadCsv(CsvDataType* csv_data, const std::string& file_path, char delim_=',') : delim(delim_) + ReadCsv(CsvDataType* csv_data, const std::string& file_path, char delim_ = ',') : delim(delim_) { if (!mars::filesystem::IsFile(file_path)) { @@ -62,18 +63,18 @@ class ReadCsv } CsvDataType csv_data_int; - for(auto it=header_map.begin(); it != header_map.end(); it++) { - csv_data_int[it->second].resize(rows-1, 0.0); + for (auto it = header_map.begin(); it != header_map.end(); it++) + { + csv_data_int[it->second].resize(rows - 1, 0.0); } // Read columns associated to header tokens std::string line; int line_counter = 0; - int parsed_row_counter = first_value_row; // header already parsed. + int parsed_row_counter = first_value_row; // header already parsed. while (std::getline(file_, line)) { - std::stringstream row_stream(line); std::string token; int column_counter = 0; @@ -81,23 +82,26 @@ class ReadCsv double item; while (std::getline(row_stream, token, delim)) { - if(column_counter >= (int)header_map.size()) { + if (column_counter >= (int)header_map.size()) + { std::cout << "ReadCsv(): Warning: too many entries in row!" << std::endl; - ++column_counter; // to indicate a corrupted row + ++column_counter; // to indicate a corrupted row break; } std::istringstream is(token); is >> item; - csv_data_int[header_map[column_counter]][line_counter]=(item); + csv_data_int[header_map[column_counter]][line_counter] = (item); ++column_counter; } // check if row was corrupted, if so, overwrite current line with the next one - if(column_counter != (int)header_map.size()) { + if (column_counter != (int)header_map.size()) + { std::cout << "ReadCsv(): Warning: corrupted row=" << parsed_row_counter << " will be skipped!" << std::endl; } - else { + else + { line_counter++; } @@ -106,7 +110,8 @@ class ReadCsv } // shrink to the actual size - for(auto it=header_map.begin(); it != header_map.end(); it++) { + for (auto it = header_map.begin(); it != header_map.end(); it++) + { csv_data_int[it->second].resize(line_counter); } diff --git a/source/mars/include/mars/data_utils/read_gps_data.h b/source/mars/include/mars/data_utils/read_gps_data.h index 0d05570..49b3c4f 100644 --- a/source/mars/include/mars/data_utils/read_gps_data.h +++ b/source/mars/include/mars/data_utils/read_gps_data.h @@ -42,10 +42,10 @@ class ReadGpsData Time time = csv_data["t"][k] + time_offset; BufferDataType data; - data.set_sensor_data( + data.set_measurement( std::make_shared(csv_data["lat"][k], csv_data["long"][k], csv_data["alt"][k])); - BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement); + BufferEntryType current_entry(time, data, sensor); data_out->at(k) = current_entry; } } diff --git a/source/mars/include/mars/data_utils/read_gps_w_vel_data.h b/source/mars/include/mars/data_utils/read_gps_w_vel_data.h index e420f95..8f4adbc 100644 --- a/source/mars/include/mars/data_utils/read_gps_w_vel_data.h +++ b/source/mars/include/mars/data_utils/read_gps_w_vel_data.h @@ -42,11 +42,11 @@ class ReadGpsWithVelData Time time = csv_data["t"][k] + time_offset; BufferDataType data; - data.set_sensor_data(std::make_shared(csv_data["lat"][k], csv_data["long"][k], + data.set_measurement(std::make_shared(csv_data["lat"][k], csv_data["long"][k], csv_data["alt"][k], csv_data["v_x"][k], csv_data["v_y"][k], csv_data["v_z"][k])); - BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement); + BufferEntryType current_entry(time, data, sensor); data_out->at(k) = current_entry; } } diff --git a/source/mars/include/mars/data_utils/read_imu_data.h b/source/mars/include/mars/data_utils/read_imu_data.h index b5210e5..ee173ab 100644 --- a/source/mars/include/mars/data_utils/read_imu_data.h +++ b/source/mars/include/mars/data_utils/read_imu_data.h @@ -45,9 +45,9 @@ class ReadImuData Eigen::Vector3d angular_velocity(csv_data["w_x"][k], csv_data["w_y"][k], csv_data["w_z"][k]); BufferDataType data; - data.set_sensor_data(std::make_shared(linear_acceleration, angular_velocity)); + data.set_measurement(std::make_shared(linear_acceleration, angular_velocity)); - BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement); + BufferEntryType current_entry(time, data, sensor); data_out->at(k) = current_entry; } } diff --git a/source/mars/include/mars/data_utils/read_mag_data.h b/source/mars/include/mars/data_utils/read_mag_data.h index 8fee6a7..f32ee20 100644 --- a/source/mars/include/mars/data_utils/read_mag_data.h +++ b/source/mars/include/mars/data_utils/read_mag_data.h @@ -45,9 +45,9 @@ class ReadMagData Eigen::Vector3d mag_vec(csv_data["cart_x"][k], csv_data["cart_y"][k], csv_data["cart_z"][k]); BufferDataType data; - data.set_sensor_data(std::make_shared(mag_vec)); + data.set_measurement(std::make_shared(mag_vec)); - BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement); + BufferEntryType current_entry(time, data, sensor); data_out->at(k) = current_entry; } } diff --git a/source/mars/include/mars/data_utils/read_pose_data.h b/source/mars/include/mars/data_utils/read_pose_data.h index 14bee4a..6dfa078 100644 --- a/source/mars/include/mars/data_utils/read_pose_data.h +++ b/source/mars/include/mars/data_utils/read_pose_data.h @@ -47,9 +47,9 @@ class ReadPoseData orientation = Utils::NormalizeQuaternion(orientation, "pose csv reader"); BufferDataType data; - data.set_sensor_data(std::make_shared(position, orientation)); + data.set_measurement(std::make_shared(position, orientation)); - BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement); + BufferEntryType current_entry(time, data, sensor); data_out->at(k) = current_entry; } } diff --git a/source/mars/include/mars/data_utils/read_position_data.h b/source/mars/include/mars/data_utils/read_position_data.h index bb4afe5..6544bc4 100644 --- a/source/mars/include/mars/data_utils/read_position_data.h +++ b/source/mars/include/mars/data_utils/read_position_data.h @@ -43,9 +43,9 @@ class ReadPositionData Eigen::Vector3d position(csv_data["p_x"][k], csv_data["p_y"][k], csv_data["p_z"][k]); BufferDataType data; - data.set_sensor_data(std::make_shared(position)); + data.set_measurement(std::make_shared(position)); - BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement); + BufferEntryType current_entry(time, data, sensor); data_out->at(k) = current_entry; } } diff --git a/source/mars/include/mars/data_utils/read_sim_data.h b/source/mars/include/mars/data_utils/read_sim_data.h index 75be927..acd2737 100644 --- a/source/mars/include/mars/data_utils/read_sim_data.h +++ b/source/mars/include/mars/data_utils/read_sim_data.h @@ -65,10 +65,11 @@ class ReadSimData core_ground_truth.b_a_ = bAcc; BufferDataType data; - data.set_core_data(std::make_shared(core_ground_truth)); - data.set_sensor_data(std::make_shared(a_imu, w_imu)); + // TODO + // data.set_core_state(std::make_shared(core_ground_truth)); + data.set_measurement(std::make_shared(a_imu, w_imu)); - BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement); + BufferEntryType current_entry(time, data, sensor); data_out->at(k) = current_entry; } } diff --git a/source/mars/include/mars/data_utils/read_velocity_data.h b/source/mars/include/mars/data_utils/read_velocity_data.h index 97c218e..d1597c8 100644 --- a/source/mars/include/mars/data_utils/read_velocity_data.h +++ b/source/mars/include/mars/data_utils/read_velocity_data.h @@ -43,9 +43,9 @@ class ReadVelocityData Eigen::Vector3d velocity(csv_data["v_x"][k], csv_data["v_y"][k], csv_data["v_z"][k]); BufferDataType data; - data.set_sensor_data(std::make_shared(velocity)); + data.set_measurement(std::make_shared(velocity)); - BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement); + BufferEntryType current_entry(time, data, sensor); data_out->at(k) = current_entry; } } diff --git a/source/mars/include/mars/data_utils/read_vision_data.h b/source/mars/include/mars/data_utils/read_vision_data.h index f408146..509cba3 100644 --- a/source/mars/include/mars/data_utils/read_vision_data.h +++ b/source/mars/include/mars/data_utils/read_vision_data.h @@ -46,9 +46,9 @@ class ReadVisionData orientation = Utils::NormalizeQuaternion(orientation, "vision csv reader"); BufferDataType data; - data.set_sensor_data(std::make_shared(position, orientation)); + data.set_measurement(std::make_shared(position, orientation)); - BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement); + BufferEntryType current_entry(time, data, sensor); data_out->at(k) = current_entry; } } diff --git a/source/mars/include/mars/sensors/gps/gps_utils.cpp b/source/mars/include/mars/sensors/gps/gps_utils.cpp index a3393e7..28a33c7 100644 --- a/source/mars/include/mars/sensors/gps/gps_utils.cpp +++ b/source/mars/include/mars/sensors/gps/gps_utils.cpp @@ -69,7 +69,7 @@ mars::GpsCoordinates mars::GPSInit::get_gps_mean(const std::shared_ptrtimestamp_).get_seconds() <= init_duration_) { - const GpsCoordinates meas = (*static_cast((*it)->data_.sensor_.get())).coordinates_; + const GpsCoordinates meas = (*static_cast((*it)->data_.measurement_.get())).coordinates_; avg_ref += meas; cnt_meas++; diff --git a/source/mars/include/mars/sensors/pressure/pressure_utils.cpp b/source/mars/include/mars/sensors/pressure/pressure_utils.cpp index 69ee31a..bcafa71 100644 --- a/source/mars/include/mars/sensors/pressure/pressure_utils.cpp +++ b/source/mars/include/mars/sensors/pressure/pressure_utils.cpp @@ -60,7 +60,7 @@ mars::Pressure mars::PressureInit::get_press_mean(const std::shared_ptrtimestamp_).get_seconds() <= init_duration_) { - const PressureMeasurementType meas = *static_cast((*it)->data_.sensor_.get()); + const PressureMeasurementType meas = *static_cast((*it)->data_.sensor_state_.get()); avg_pressure += meas.pressure_; cnt_meas++; diff --git a/source/mars/include/mars/sensors/update_sensor_abs_class.h b/source/mars/include/mars/sensors/update_sensor_abs_class.h index 215bef4..e8c25fc 100644 --- a/source/mars/include/mars/sensors/update_sensor_abs_class.h +++ b/source/mars/include/mars/sensors/update_sensor_abs_class.h @@ -41,6 +41,6 @@ class UpdateSensorAbsClass : public SensorAbsClass std::shared_ptr core_states_; }; -}; // namespace mars +} // namespace mars #endif // UPDATE_SENSOR_ABS_CLASS_H diff --git a/source/mars/include/mars/type_definitions/buffer_data_type.h b/source/mars/include/mars/type_definitions/buffer_data_type.h index a1b7c86..89abfd2 100644 --- a/source/mars/include/mars/type_definitions/buffer_data_type.h +++ b/source/mars/include/mars/type_definitions/buffer_data_type.h @@ -24,6 +24,9 @@ namespace mars /// A high-level class such as the PoseSensorClass needs to keep track of the type such that it can cast the data before /// it is used. /// +/// An entry always contains a measurement, if has_state is true, then the entry also has a core state. +/// Not all entrys have sensor states. +/// /// \attention Shared pointer should be passed by value to ensure the correct reference count. Further, using the move /// operator saves two internal atomic copys of the shared pointer and is more efficient. /// @@ -32,9 +35,6 @@ namespace mars class BufferDataType { public: - std::shared_ptr core_{ nullptr }; ///< Core data - std::shared_ptr sensor_{ nullptr }; ///< Sensor data - BufferDataType() = default; /// @@ -44,19 +44,69 @@ class BufferDataType /// /// \note Return/pass smart pointers by value /// - BufferDataType(std::shared_ptr core, std::shared_ptr sensor) : core_(move(core)), sensor_(move(sensor)) + BufferDataType(std::shared_ptr meas) { + set_measurement(meas); } - void set_core_data(std::shared_ptr core) + BufferDataType(std::shared_ptr core, std::shared_ptr sensor) { - core_ = std::move(core); + set_states(core, sensor); } - void set_sensor_data(std::shared_ptr sensor) + inline void set_core_state(std::shared_ptr core) { - sensor_ = std::move(sensor); + core_state_ = core; + has_core_state_ = true; } + + inline void set_sensor_state(std::shared_ptr sensor) + { + sensor_state_ = sensor; + has_sensor_state_ = true; + } + + inline void set_states(std::shared_ptr core, std::shared_ptr sensor) + { + set_core_state(core); + set_sensor_state(sensor); + } + + inline void set_measurement(std::shared_ptr meas) + { + measurement_ = meas; + } + + void ClearStates(void) + { + core_state_ = nullptr; + sensor_state_ = nullptr; + has_core_state_ = false; + has_sensor_state_ = false; + } + + inline bool HasCoreStates(void) const + { + return has_core_state_; + } + + inline bool HasSensorStates(void) const + { + return has_sensor_state_; + } + + inline bool HasStates(void) const + { + return HasCoreStates() || HasSensorStates(); + } + + std::shared_ptr core_state_{ nullptr }; ///< Core state data + std::shared_ptr sensor_state_{ nullptr }; ///< Sensor state data + std::shared_ptr measurement_{ nullptr }; ///< Sensor measurement + +private: + bool has_core_state_ = { false }; + bool has_sensor_state_ = { false }; }; } // namespace mars #endif // BUFFERDATATYPE_H diff --git a/source/mars/include/mars/type_definitions/buffer_entry_type.h b/source/mars/include/mars/type_definitions/buffer_entry_type.h index 4b64e88..0f8e1ec 100644 --- a/source/mars/include/mars/type_definitions/buffer_entry_type.h +++ b/source/mars/include/mars/type_definitions/buffer_entry_type.h @@ -25,14 +25,11 @@ namespace BufferMetadataTypes /// enum BufferMetadataType { + none, invalid, - core_state, - core_state_auto, ///< auto generated core_state e.g. introduced by interpolation - sensor_state, - init_state, - measurement, ///< regular measurement - measurement_ooo, ///< out of order measurement - measurement_auto ///< auto generated measurement e.g. introduced by interpolation + init, + out_of_order, + auto_add, ///< auto generated measurement e.g. introduced by interpolation }; } // namespace BufferMetadataTypes @@ -45,26 +42,26 @@ class BufferEntryType public: Time timestamp_{ 0.0 }; BufferDataType data_{}; - std::shared_ptr sensor_{ nullptr }; + std::shared_ptr sensor_handle_{ nullptr }; int metadata_{ BufferMetadataType::invalid }; BufferEntryType() = default; BufferEntryType(const Time& timestamp, BufferDataType data, std::shared_ptr sensor, - const int& metadata); + const int& metadata = BufferMetadataType::none); bool operator<(const BufferEntryType& rhs) const; bool operator<=(const BufferEntryType& rhs) const; bool operator>(const BufferEntryType& rhs) const; bool operator>=(const BufferEntryType& rhs) const; - friend std::ostream& operator<<(std::ostream& out, const BufferEntryType& entry); /// - /// \brief IsState - /// \return True if the metadata can be found in the metadata_state_filter_. False otherwise. + /// \brief get_metadata_label decodes the 'BufferMetadataType' enum to strings + /// \param label enum integer of 'BufferMetadataType' + /// \return String description /// - bool IsState() const; + static std::string get_metadata_label(int label); /// /// \brief IsMeasurement @@ -72,15 +69,35 @@ class BufferEntryType /// bool IsMeasurement() const; + /// + /// \brief IsValid Check if the entry is valid for usage + /// + /// \return true valid entry + /// \return false invalid e.g. a measurement that was an outlier and no state was calculated + /// + bool IsValid() const; + /// /// \brief IsAutoGenerated /// \return True if the entry is an auto generated entry /// bool IsAutoGenerated() const; + /// + /// \brief HasState checks if the BufferDataType has a (core) state + /// + /// \return true if the BufferDataType contains a core state + /// \return false otherwise + /// + bool HasStates() const; + + /// + /// \brief ClearStates resets states of the buffer entry + /// + void ClearStates(); + private: - std::set metadata_state_filter_; - std::set metadata_measurement_filter_; + std::set metadata_valid_filter_; std::set metadata_auto_filter_; }; } // namespace mars diff --git a/source/mars/source/buffer.cpp b/source/mars/source/buffer.cpp index d72e984..f129f6d 100644 --- a/source/mars/source/buffer.cpp +++ b/source/mars/source/buffer.cpp @@ -59,12 +59,16 @@ int Buffer::get_length() const void Buffer::PrintBufferEntries() const { - std::cout << "Idx \t Sensor Name \t Timestamp \t Metadata " << std::endl; + std::cout << "Idx" + << "\tSensor Name" + << "\tTimestamp" + << "\t\tMetadata" + << "\tStates" << std::endl; // iterate forwards for (size_t k = 0; k < data_.size(); ++k) { - std::cout << k << "\t " << data_[k] << std::endl; + std::cout << k << '\t' << data_[k] << std::endl; } } @@ -89,7 +93,7 @@ bool Buffer::get_latest_state(BufferEntryType* entry) const // iterate backwards for (auto k = data_.rbegin(); k != data_.rend(); ++k) { - if (k->IsState()) + if (k->HasStates()) { *entry = *k; return true; @@ -109,7 +113,7 @@ bool Buffer::get_oldest_state(BufferEntryType* entry) const // iterate forwards for (const auto& k : data_) { - if (k.IsState()) + if (k.HasStates()) { *entry = k; return true; @@ -129,7 +133,7 @@ bool Buffer::get_oldest_core_state(BufferEntryType* entry) const // iterate forwards (oldest to newest) for (const auto& k : data_) { - if (k.metadata_ == mars::BufferMetadataType::core_state) + if (k.HasStates() && k.IsValid()) { *entry = k; return true; @@ -149,9 +153,9 @@ bool Buffer::get_latest_init_state(BufferEntryType* entry) const // iterate backwards (newest to oldest) for (auto k = data_.rbegin(); k != data_.rend(); ++k) { - if (k->IsState()) + if (k->HasStates()) { - if (k->metadata_ == BufferMetadataType::init_state) + if (k->metadata_ == BufferMetadataType::init) { *entry = *k; return true; @@ -181,9 +185,9 @@ bool Buffer::get_latest_sensor_handle_state(const std::shared_ptr= 0; --k) { - if (data_[k].IsState()) + if (data_[k].HasStates()) { - if (data_[k].sensor_.get() == sensor_handle.get()) + if (data_[k].sensor_handle_.get() == sensor_handle.get()) { *entry = data_[k]; *index = k; @@ -207,9 +211,9 @@ bool Buffer::get_oldest_sensor_handle_state(const std::shared_ptrIsMeasurement()) + if (k->sensor_handle_.get() == sensor_handle.get()) { - if (k->sensor_.get() == sensor_handle.get()) - { - *entry = *k; - return true; - } + *entry = *k; + return true; } } @@ -258,12 +260,9 @@ bool Buffer::get_sensor_handle_measurements(const std::shared_ptrpush_back(&k); - } + entries->push_back(&k); } } @@ -293,7 +292,7 @@ bool Buffer::get_closest_state(const Time& timestamp, BufferEntryType* entry, in // iterate backwards / start with latest entry for (int k = data_.size() - 1; k >= 0; --k) { - if (data_[k].IsState()) + if (data_[k].HasStates()) { found_state = true; @@ -352,11 +351,17 @@ bool Buffer::RemoveSensorFromBuffer(const std::shared_ptr& senso return false; } - for (int k = 0; k < this->get_length(); k++) + for (auto it = data_.begin(); it != data_.end();) { - if (data_[k].sensor_ == sensor_handle) + if (it->sensor_handle_ == sensor_handle) { - *data_.erase(data_.begin() + k); + // Erase returns the next iterator + it = data_.erase(it); + } + else + { + // Only increment if we didn't delete + it++; } } @@ -367,17 +372,10 @@ int Buffer::AddEntrySorted(const BufferEntryType& new_entry) { int index = InsertDataAtTimestamp(new_entry); - if (this->get_length() > max_buffer_size_) + int del_idx = RemoveOverflowEntrys(); + if (del_idx > index) { - int del_idx = RemoveOverflowEntrys(); - if (del_idx < 0) - { - index = -1; - } - else - { - index -= del_idx < index ? 2 : 0; - } + index = -1; } return index; @@ -389,22 +387,30 @@ int Buffer::FindClosestTimestamp(const Time& /*timestamp*/) const return false; } -bool Buffer::DeleteStatesStartingAtIdx(const int& idx) +bool Buffer::ClearStatesStartingAtIdx(const int& idx) { if (this->IsEmpty()) { return false; } - // start deleting from the back to keep the running index valid if (idx < this->get_length()) { - for (int k = get_length() - 1; k >= idx; --k) + for (auto it = data_.begin() + idx; it != data_.end();) { - if (data_[k].IsState()) + if (it->IsAutoGenerated()) { - *data_.erase(data_.begin() + k); + // erase returns the next iterator + it = data_.erase(it); + continue; } + else if (it->HasStates()) + { + it->ClearStates(); + } + + // Only increment if we didn't delete or if the entry was only a measurement + it++; } return true; } @@ -452,8 +458,9 @@ int Buffer::InsertDataAtTimestamp(const BufferEntryType& new_entry) if (current_time_distance.get_seconds() >= 0) { - data_.insert(data_.begin() + k + 1, new_entry); - return k + 1; // return entry index + int insert_idx = k + 1; + data_.insert(data_.begin() + insert_idx, new_entry); + return insert_idx; // return entry index } } @@ -475,37 +482,6 @@ bool Buffer::InsertDataAtIndex(const BufferEntryType& new_entry, const int& inde return true; } -bool Buffer::InsertIntermediateData(const BufferEntryType& measurement, const BufferEntryType& state) -{ - // Ensure the given data has the right meta data - if (!(measurement.IsMeasurement() && state.IsState())) - { - return false; - } - - // Check of the latest entry is a state or measurement - if (data_.back().IsState()) - { - return false; - } - else if (data_.back().IsMeasurement()) - { - // insert set of data one element before latest - // change entry meta data to auto generated - BufferEntryType meas_auto(measurement); - meas_auto.metadata_ = BufferMetadataType::measurement_auto; - - BufferEntryType state_auto(state); - state_auto.metadata_ = BufferMetadataType::core_state_auto; - - int last_idx = get_length(); - InsertDataAtIndex(meas_auto, last_idx - 1); - InsertDataAtIndex(state_auto, last_idx); - } - - return true; -} - bool Buffer::get_intermediate_entry_pair(const std::shared_ptr& sensor_handle, BufferEntryType* imu_state, BufferEntryType* sensor_state) const { @@ -525,7 +501,7 @@ bool Buffer::get_intermediate_entry_pair(const std::shared_ptr& size_t interm_core_idx = size_t(found_sensor_state_idx - entry_offset); // Ensure that the entrie is a state entry, as expected, and has the same timestamp as the sensor state - if (data_[interm_core_idx].IsState() && data_[interm_core_idx].timestamp_ == found_sensor_state.timestamp_) + if (data_[interm_core_idx].HasStates() && data_[interm_core_idx].timestamp_ == found_sensor_state.timestamp_) { *sensor_state = found_sensor_state; *imu_state = data_[interm_core_idx]; @@ -537,82 +513,58 @@ bool Buffer::get_intermediate_entry_pair(const std::shared_ptr& int Buffer::RemoveOverflowEntrys() { - // Only delete if buffer would overflow - if (this->get_length() < this->max_buffer_size_) + // Only delete if buffer did overflow + if (this->get_length() <= this->max_buffer_size_) { return -1; } - int delete_idx = 0; // 0 is the oldest index + // Starting with the oldest at zero + + auto it = data_.begin(); - for (int k = 0; k < this->get_length() - 1; k++) + for (; it != data_.end(); it++) { - if (data_[delete_idx].IsMeasurement()) + if (CheckForLastSensorHandleWithState(it->sensor_handle_)) { - if (CheckForLastSensorHandlePair(data_[delete_idx].sensor_)) - { - // Is last SensorHandle, dont delete - delete_idx++; - } - else - { - bool same_timestamp = data_[delete_idx].timestamp_ == data_[delete_idx + 1].timestamp_; - bool same_handle = data_[delete_idx].sensor_ == data_[delete_idx + 1].sensor_; - - if (same_timestamp && same_handle) - { - *data_.erase(data_.begin() + delete_idx + 1); - *data_.erase(data_.begin() + delete_idx); - } - else - { - // Error - } - return delete_idx; - } + continue; } else { - delete_idx++; + it = data_.erase(it); + break; } } - return -1; -} // namespace mars + // return deleted index + return int(distance(data_.begin(), it)); +} -bool Buffer::CheckForLastSensorHandlePair(const std::shared_ptr& sensor_handle) const +bool Buffer::CheckForLastSensorHandleWithState(const std::shared_ptr& sensor_handle) const { - int num_found_ms_pair = 0; + int num_found_instances = 0; - for (auto current_it = data_.begin(); current_it != data_.end() - 1; ++current_it) + for (auto current_it = data_.begin(); current_it != data_.end(); ++current_it) { - auto next_it = current_it + 1; - - if (current_it->sensor_ == sensor_handle && current_it->IsMeasurement() && next_it->IsState()) + if ((current_it->sensor_handle_ == sensor_handle) && current_it->HasStates()) { - // Additional check - if (next_it->sensor_ != sensor_handle) + num_found_instances++; + + if (num_found_instances > 1) { - // Error + return false; } - - num_found_ms_pair++; - } - - if (num_found_ms_pair > 2) - { - return false; } } - if (num_found_ms_pair == 0) + if (num_found_instances == 1) { - // Cover the case that no entrie existed - return false; + return true; } else { - return true; + // Cover the case that no entrie existed + return false; } } } // namespace mars diff --git a/source/mars/source/buffer_entry_type.cpp b/source/mars/source/buffer_entry_type.cpp index eed7aea..fab8a85 100644 --- a/source/mars/source/buffer_entry_type.cpp +++ b/source/mars/source/buffer_entry_type.cpp @@ -17,15 +17,31 @@ namespace mars { BufferEntryType::BufferEntryType(const Time& timestamp, BufferDataType data, std::shared_ptr sensor, const int& metadata) - : timestamp_(timestamp), data_(std::move(data)), sensor_(move(sensor)), metadata_(metadata) + : timestamp_(timestamp), data_(std::move(data)), sensor_handle_(move(sensor)), metadata_(metadata) { - metadata_state_filter_ = std::set({ BufferMetadataType::core_state, BufferMetadataType::core_state_auto, - BufferMetadataType::sensor_state, BufferMetadataType::init_state }); + metadata_valid_filter_ = + std::set({ BufferMetadataType::none, BufferMetadataType::out_of_order, BufferMetadataType::auto_add }); - metadata_measurement_filter_ = std::set( - { BufferMetadataType::measurement, BufferMetadataType::measurement_ooo, BufferMetadataType::measurement_auto }); + metadata_auto_filter_ = std::set({ BufferMetadataType::auto_add }); +} - metadata_auto_filter_ = std::set({ BufferMetadataType::measurement_auto, BufferMetadataType::core_state_auto }); +std::string BufferEntryType::get_metadata_label(int label) +{ + switch (label) + { + case BufferMetadataType::none: + return "None"; + case BufferMetadataType::invalid: + return "Invalid"; + case BufferMetadataType::init: + return "Init"; + case BufferMetadataType::out_of_order: + return "Out of order"; + case BufferMetadataType::auto_add: + return "Auto add"; + default: + return "Unknown meta type"; + } } bool BufferEntryType::operator<(const BufferEntryType& rhs) const @@ -50,25 +66,61 @@ bool BufferEntryType::operator>=(const BufferEntryType& rhs) const std::ostream& operator<<(std::ostream& out, const BufferEntryType& entry) { - out << entry.sensor_->name_ << '\t'; + out << entry.sensor_handle_->name_ << "\t\t"; out << entry.timestamp_ << '\t'; - out << entry.metadata_ << '\t'; + out << BufferEntryType::get_metadata_label(entry.metadata_) << '\t'; + + out << "Core"; + if (entry.HasStates()) + { + out << "[x]"; + } + else + { + out << "[ ]"; + } + + out << " Sensor "; + if (entry.HasStates()) + { + out << "[x]"; + } + else + { + out << "[ ]"; + } + + out << " Measurement "; + if (entry.data_.measurement_ != nullptr) + { + out << "[x]"; + } + else + { + out << "[ ]"; + } return out; } -bool BufferEntryType::IsState() const +bool BufferEntryType::HasStates() const { - return (metadata_state_filter_.find(metadata_) != metadata_state_filter_.end()); + return data_.HasStates(); } -bool BufferEntryType::IsAutoGenerated() const +void BufferEntryType::ClearStates() { - return (metadata_auto_filter_.find(metadata_) != metadata_auto_filter_.end()); + data_.ClearStates(); } -bool BufferEntryType::IsMeasurement() const +bool BufferEntryType::IsValid() const { - return (metadata_measurement_filter_.find(metadata_) != metadata_measurement_filter_.end()); + return (metadata_valid_filter_.find(metadata_) != metadata_auto_filter_.end()); } + +bool BufferEntryType::IsAutoGenerated() const +{ + return (metadata_auto_filter_.find(metadata_) != metadata_auto_filter_.end()); +} + } // namespace mars diff --git a/source/mars/source/core_logic.cpp b/source/mars/source/core_logic.cpp index ba121a4..9c8099a 100644 --- a/source/mars/source/core_logic.cpp +++ b/source/mars/source/core_logic.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include namespace mars @@ -23,46 +24,44 @@ CoreLogic::CoreLogic(std::shared_ptr core_states) : core_states_(move int CoreLogic::Initialize(const Eigen::Vector3d& p_wi_init, const Eigen::Quaterniond& q_wi_init) { + // Warning if the prior buffer is empty and the filter should be initialized. if (buffer_prior_core_init_.IsEmpty()) { std::cout << "CoreLogic: " - << "No measurements to initialize the filter" << std::endl; + << "Warning: No measurements to initialize the filter" << std::endl; return false; } // Get Propagation Sensor Data from Prior Buffer + // NOTE: Additional modification of the past IMU data such as prefiltering can be done here. BufferEntryType latest_prop_sensor_prior_buffer_entry; - buffer_prior_core_init_.get_latest_sensor_handle_measurement(core_states_->propagation_sensor_, &latest_prop_sensor_prior_buffer_entry); - // NOTE: Additional modification of the past IMU data such as - // prefiltering can be done here. - buffer_.AddEntrySorted(latest_prop_sensor_prior_buffer_entry); - // Get Propagation Sensor Data from Main Buffer - BufferEntryType latest_prop_sensor_buffer_entry; - buffer_.get_latest_sensor_handle_measurement(core_states_->propagation_sensor_, &latest_prop_sensor_buffer_entry); + // Prepare the first main buffer entry by coping the pre buffer entry, clearing possible states and setting the meta + BufferEntryType init_main_buffer_entry(latest_prop_sensor_prior_buffer_entry); + init_main_buffer_entry.ClearStates(); + init_main_buffer_entry.metadata_ = BufferMetadataType::init; // Initialize the state with the latest IMU data, as well as previously set position and orientation (assuming zero // velocity at the moment) + + // Get IMU measurement IMUMeasurementType imu_measurement = - *static_cast(latest_prop_sensor_buffer_entry.data_.sensor_.get()); + *static_cast(init_main_buffer_entry.data_.measurement_.get()); + // Set core-states CoreType initial_core_state; - initial_core_state.state_ = core_states_->InitializeState( imu_measurement.angular_velocity_, imu_measurement.linear_acceleration_, p_wi_init, Eigen::Vector3d::Zero(), q_wi_init, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); initial_core_state.cov_ = core_states_->InitializeCovariance(); - BufferDataType init_core_state_data; - init_core_state_data.set_core_data(std::make_shared(initial_core_state)); + // Generate data element for initial state entry and add it to the existing entry + init_main_buffer_entry.data_.set_core_state(std::make_shared(initial_core_state)); - BufferEntryType new_core_state_entry(latest_prop_sensor_buffer_entry.timestamp_, init_core_state_data, - latest_prop_sensor_buffer_entry.sensor_, mars::BufferMetadataType::init_state); - - buffer_.AddEntrySorted(new_core_state_entry); + buffer_.AddEntrySorted(init_main_buffer_entry); core_is_initialized_ = true; std::cout << "Info: Filter was initialized" << std::endl; @@ -85,10 +84,10 @@ CoreStateMatrix CoreLogic::GenerateStateTransitionBlock(const int& first_transit BufferEntryType entry; buffer_.get_entry_at_idx(k, &entry); - if (entry.IsState() && entry.metadata_ != BufferMetadataType::sensor_state && - entry.metadata_ != BufferMetadataType::init_state) + if (entry.HasStates() && (entry.sensor_handle_ == core_states_->propagation_sensor_) && + (entry.metadata_ != BufferMetadataType::init)) { - CoreStateMatrix entry_st = static_cast(entry.data_.core_.get())->state_transition_; + CoreStateMatrix entry_st = static_cast(entry.data_.core_state_.get())->state_transition_; state_transition = entry_st * state_transition; } } @@ -124,8 +123,8 @@ Eigen::MatrixXd CoreLogic::PropagateSensorCrossCov(const Eigen::MatrixXd& sensor return propagated_cov; } -bool CoreLogic::PerformSensorUpdate(BufferEntryType* state_buffer_entry_return, std::shared_ptr sensor, - const Time& timestamp, std::shared_ptr sensor_data) +bool CoreLogic::PerformSensorUpdate(std::shared_ptr sensor, const Time& timestamp, + BufferEntryType* sensor_data) { if (verbose_) { @@ -144,11 +143,14 @@ bool CoreLogic::PerformSensorUpdate(BufferEntryType* state_buffer_entry_return, return false; } - CoreType latest_core_data = *static_cast(closest_state_entry.data_.core_.get()); + CoreType latest_core_data = *static_cast(closest_state_entry.data_.core_state_.get()); BufferDataType init_data = - sensor->Initialize(timestamp, sensor_data->sensor_, std::make_shared(latest_core_data)); - *state_buffer_entry_return = BufferEntryType(timestamp, init_data, sensor, BufferMetadataType::init_state); + sensor->Initialize(timestamp, sensor_data->data_.measurement_, std::make_shared(latest_core_data)); + + // TODO the init function should directly receive the state entry and return it + sensor_data->data_.set_states(init_data.core_state_, init_data.sensor_state_); + sensor_data->metadata_ = BufferMetadataType::init; return true; } @@ -173,28 +175,34 @@ bool CoreLogic::PerformSensorUpdate(BufferEntryType* state_buffer_entry_return, mars::BufferEntryType latest_state_buffer_entry; buffer_.get_latest_state(&latest_state_buffer_entry); - CoreType core_prev = *static_cast(latest_state_buffer_entry.data_.core_.get()); + // Get latest IMU measurement + CoreType core_prev = *static_cast(latest_state_buffer_entry.data_.core_state_.get()); IMUMeasurementType imu_meas_curr(core_prev.state_.a_m_, core_prev.state_.w_m_); BufferDataType interm_prop; - interm_prop.set_sensor_data(std::make_shared(imu_meas_curr)); + interm_prop.set_measurement(std::make_shared(imu_meas_curr)); + + // Copy IMU measurement for zero order hold interpolation + mars::BufferEntryType interm_buffer_entry(timestamp, interm_prop, core_states_->propagation_sensor_, + mars::BufferMetadataType::auto_add); - mars::BufferEntryType new_core_state_entry; - new_core_state_entry = PerformCoreStatePropagation(core_states_->propagation_sensor_, timestamp, - std::make_shared(interm_prop), - std::make_shared(latest_state_buffer_entry)); + PerformCoreStatePropagation(core_states_->propagation_sensor_, timestamp, latest_state_buffer_entry, + &interm_buffer_entry); // Extract prior information from buffer entries - CoreType prior_core_data = *static_cast(new_core_state_entry.data_.core_.get()); + CoreType prior_core_data = *static_cast(interm_buffer_entry.data_.core_state_.get()); Utils::CheckCov(prior_core_data.cov_, "CoreLogic: Core cov prior"); - Eigen::MatrixXd prior_sensor_covariance = sensor->get_covariance(prior_sensor_state_entry.data_.sensor_); + Eigen::MatrixXd prior_sensor_covariance = sensor->get_covariance(prior_sensor_state_entry.data_.sensor_state_); CoreStateMatrix state_transition; if (add_interm_buffer_entries_) { // Intermediate IMU Measurement and propergated state - mars::BufferEntryType interm_imu_measurement_entry(timestamp, interm_prop, core_states_->propagation_sensor_, - mars::BufferMetadataType::measurement); - buffer_.InsertIntermediateData(interm_imu_measurement_entry, new_core_state_entry); + buffer_.AddEntrySorted(interm_buffer_entry); + + if (verbose_) + { + std::cout << "[CoreLogic]: Intermediate propergated state was written to buffer." << std::endl; + } // Generate state transition block between prior_sensor_idx and prior_core_idx BufferEntryType tmp_entry; @@ -219,62 +227,52 @@ bool CoreLogic::PerformSensorUpdate(BufferEntryType* state_buffer_entry_return, // Perform the sensor update BufferDataType corrected_state_data; bool successful_update; - successful_update = sensor->CalcUpdate(timestamp, sensor_data->sensor_, prior_core_data.state_, - prior_sensor_state_entry.data_.sensor_, corrected_cov, &corrected_state_data); + successful_update = + sensor->CalcUpdate(timestamp, sensor_data->data_.measurement_, prior_core_data.state_, + prior_sensor_state_entry.data_.sensor_state_, corrected_cov, &corrected_state_data); - if (verbose_) - { - std::cout << "[CoreLogic]: Perform Sensor Update - DONE" << std::endl; - } + // TODO: This should also happen inside the update class or a preset object should be given that already has the + // measurement if (successful_update) { - // Generate buffer entry and return the corrected states - *state_buffer_entry_return = - BufferEntryType(timestamp, corrected_state_data, sensor, BufferMetadataType::sensor_state); + sensor_data->data_.set_states(corrected_state_data.core_state_, corrected_state_data.sensor_state_); + + PoseMeasurementType* meas = static_cast(sensor_data->data_.measurement_.get()); + mars::PoseSensorData* prior_sensor_data = + static_cast(sensor_data->data_.sensor_state_.get()); if (verbose_) { - std::cout << "[CoreLogic]: Perform Sensor Update - DONE" << std::endl; + std::cout << "[CoreLogic]: Perform Sensor Update (Successfull) - DONE" << std::endl; } return true; } else { - // Store the propagated core state as an auto generated state "zero order hold" in case the original update was - // rejected. The sensor handle will be that of the propagtion sensor - - BufferEntryType generated_prior_data(new_core_state_entry); - generated_prior_data.sensor_ = core_states_->propagation_sensor_; - generated_prior_data.metadata_ = BufferMetadataType::core_state_auto; - - *state_buffer_entry_return = generated_prior_data; - if (verbose_) { - std::cout << "[CoreLogic]: Measurement was rejected, propagated core state was written instead" << std::endl; - std::cout << "[CoreLogic]: Perform Sensor Update - DONE" << std::endl; + std::cout << "[CoreLogic]: Perform Sensor Update (Rejected) - DONE" << std::endl; } - return true; + return false; } } -BufferEntryType CoreLogic::PerformCoreStatePropagation(std::shared_ptr sensor, const Time& timestamp, - const std::shared_ptr& data_measurement, - const std::shared_ptr& prior_state_entry) +void CoreLogic::PerformCoreStatePropagation(std::shared_ptr sensor, const Time& timestamp, + const BufferEntryType& prior_state_entry, BufferEntryType* sensor_entry) { if (verbose_) { std::cout << "[CoreLogic]: Perform Core State Propagation" << std::endl; } - CoreType prior_core_data = *static_cast(prior_state_entry->data_.core_.get()); - IMUMeasurementType meas_system_input = *static_cast(data_measurement->sensor_.get()); + CoreType prior_core_data = *static_cast(prior_state_entry.data_.core_state_.get()); + IMUMeasurementType meas_system_input = *static_cast(sensor_entry->data_.measurement_.get()); const Time current_time = timestamp; - const Time previous_time = prior_state_entry->timestamp_; + const Time previous_time = prior_state_entry.timestamp_; const Time dt = (current_time - previous_time).abs(); if (dt == 0 && verbose_) @@ -287,94 +285,89 @@ BufferEntryType CoreLogic::PerformCoreStatePropagation(std::shared_ptrPropagateState(prior_core_data.state_, meas_system_input, dt.get_seconds()); - BufferDataType buffer_data; - buffer_data.set_core_data(std::make_shared(propagated_core_state)); - - BufferEntryType new_core_state_entry(current_time, buffer_data, sensor, mars::BufferMetadataType::core_state); + sensor_entry->data_.set_core_state(std::make_shared(propagated_core_state)); if (verbose_) { std::cout << "[CoreLogic]: Perform Core State Propagation - DONE" << std::endl; } - - return new_core_state_entry; } void CoreLogic::ReworkBufferStartingAtIndex(const int& index) { - if (verbose_) - { - std::cout << "[CoreLogic]: Rework Buffer Starting At Index " << index << std::endl; - } - - assert(index >= 0); - - buffer_.DeleteStatesStartingAtIdx(index); - - // The buffer grows by one index for each additional state, - // index_offset corrects this - int index_offset = 0; - - // The buffer size can change during the reiteration. The reiteration needs to be done with the initial size - // (current_buffer_length). - const int current_buffer_length = buffer_.get_length(); - for (int k = index; k < current_buffer_length; k++) - { - // All states after the "out of order" index have been deleted - // that's why getting the latest state returns the latest valid - // state before the out of order measurement at this point - - // get next measurement - int measurement_entry_idx = k + index_offset; - int state_insertion_idx = measurement_entry_idx + 1; - - BufferEntryType next_measurement_buffer_entry; - buffer_.get_entry_at_idx(measurement_entry_idx, &next_measurement_buffer_entry); - - std::shared_ptr sensor_handle = next_measurement_buffer_entry.sensor_; - Time timestamp = next_measurement_buffer_entry.timestamp_; - BufferDataType buffer_data = next_measurement_buffer_entry.data_; - - // Propagate State with System Input from Propagation Sensor - if (sensor_handle == core_states_->propagation_sensor_) - { - // Since the measurement was not out of order, get latest state is valid - mars::BufferEntryType latest_state_buffer_entry; - buffer_.get_latest_state(&latest_state_buffer_entry); - - // Zero-order hold for IMU measurement. Copy imu measurement from latest state to current sensor state - mars::BufferEntryType latest_prop_meas_entry; - buffer_.get_latest_sensor_handle_state(core_states_->propagation_sensor_, &latest_prop_meas_entry); - - latest_state_buffer_entry.data_.sensor_ = latest_prop_meas_entry.data_.sensor_; - - mars::BufferEntryType new_core_state_entry; - new_core_state_entry = - PerformCoreStatePropagation(sensor_handle, timestamp, std::make_shared(buffer_data), - std::make_shared(latest_state_buffer_entry)); - - buffer_.InsertDataAtIndex(new_core_state_entry, state_insertion_idx); - } - else - { - // Process non-propagation sensor information - mars::BufferEntryType new_state_buffer_entry; - PerformSensorUpdate(&new_state_buffer_entry, sensor_handle, timestamp, - std::make_shared(buffer_data)); - - buffer_.InsertDataAtIndex(new_state_buffer_entry, state_insertion_idx); - } - - index_offset = index_offset + 1; - } - - // delete last buffer entry if the max. buffer size is reached - buffer_.RemoveOverflowEntrys(); - - if (verbose_) - { - std::cout << "[CoreLogic]: Rework Buffer Starting At Index - DONE" << std::endl; - } + // if (verbose_) + // { + // std::cout << "[CoreLogic]: Rework Buffer Starting At Index " << index << std::endl; + // } + + // assert(index >= 0); + + // buffer_.ClearStatesStartingAtIdx(index); + + // // The buffer grows by one index for each additional state, + // // index_offset corrects this + // int index_offset = 0; + + // // The buffer size can change during the reiteration. The reiteration needs to be done with the initial size + // // (current_buffer_length). + // const int current_buffer_length = buffer_.get_length(); + // for (int k = index; k < current_buffer_length; k++) + // { + // // All states after the "out of order" index have been deleted + // // that's why getting the latest state returns the latest valid + // // state before the out of order measurement at this point + + // // get next measurement + // int measurement_entry_idx = k + index_offset; + // int state_insertion_idx = measurement_entry_idx + 1; + + // BufferEntryType next_measurement_buffer_entry; + // buffer_.get_entry_at_idx(measurement_entry_idx, &next_measurement_buffer_entry); + + // std::shared_ptr sensor_handle = next_measurement_buffer_entry.sensor_handle_; + // Time timestamp = next_measurement_buffer_entry.timestamp_; + // BufferDataType buffer_data = next_measurement_buffer_entry.data_; + + // // Propagate State with System Input from Propagation Sensor + // if (sensor_handle == core_states_->propagation_sensor_) + // { + // // Since the measurement was not out of order, get latest state is valid + // mars::BufferEntryType latest_state_buffer_entry; + // buffer_.get_latest_state(&latest_state_buffer_entry); + + // // Zero-order hold for IMU measurement. Copy imu measurement from latest state to current sensor state + // mars::BufferEntryType latest_prop_meas_entry; + // buffer_.get_latest_sensor_handle_state(core_states_->propagation_sensor_, &latest_prop_meas_entry); + + // latest_state_buffer_entry.data_.sensor_state_ = latest_prop_meas_entry.data_.sensor_state_; + + // mars::BufferEntryType new_core_state_entry; + // new_core_state_entry = + // PerformCoreStatePropagation(sensor_handle, timestamp, std::make_shared(buffer_data), + // std::make_shared(latest_state_buffer_entry)); + + // buffer_.InsertDataAtIndex(new_core_state_entry, state_insertion_idx); + // } + // else + // { + // // non-propagation sensor information + // mars::BufferEntryType new_state_buffer_entry; + // PerformSensorUpdate(&new_state_buffer_entry, sensor_handle, timestamp, + // std::make_shared(buffer_data)); + + // buffer_.InsertDataAtIndex(new_state_buffer_entry, state_insertion_idx); + // } + + // index_offset = index_offset + 1; + // } + + // // delete last buffer entry if the max. buffer size is reached + // buffer_.RemoveOverflowEntrys(); + + // if (verbose_) + // { + // std::cout << "[CoreLogic]: Rework Buffer Starting At Index - DONE" << std::endl; + // } } bool CoreLogic::ProcessMeasurement(std::shared_ptr sensor, const Time& timestamp, @@ -397,7 +390,7 @@ bool CoreLogic::ProcessMeasurement(std::shared_ptr sensor, const } // Generate buffer entry element for the measurement - mars::BufferEntryType new_measurement_buffer_entry(timestamp, data, sensor, mars::BufferMetadataType::measurement); + mars::BufferEntryType new_sensor_entry(timestamp, data, sensor); // Store measurements prior to the core initialization in the Prior-Buffer if (!this->core_is_initialized_) @@ -408,7 +401,7 @@ bool CoreLogic::ProcessMeasurement(std::shared_ptr sensor, const core_init_warn_once_ = true; } - buffer_prior_core_init_.AddEntrySorted(new_measurement_buffer_entry); + buffer_prior_core_init_.AddEntrySorted(new_sensor_entry); return false; } @@ -419,7 +412,30 @@ bool CoreLogic::ProcessMeasurement(std::shared_ptr sensor, const if (timestamp >= latest_buffer_entry.timestamp_) { // Measurement is newer than the latest entry - proceed normally - buffer_.AddEntrySorted(new_measurement_buffer_entry); + // Main part to process the measurement + if (sensor == core_states_->propagation_sensor_) + { + // Propagate State with System Input from Propagation Sensor + + // Since the measurement was not out of order, get latest state is valid + mars::BufferEntryType latest_state_buffer_entry; + buffer_.get_latest_state(&latest_state_buffer_entry); + + PerformCoreStatePropagation(sensor, timestamp, latest_state_buffer_entry, &new_sensor_entry); + + buffer_.AddEntrySorted(new_sensor_entry); + } + else + { + if (!PerformSensorUpdate(sensor, timestamp, &new_sensor_entry)) + { + return false; + } + + buffer_.AddEntrySorted(new_sensor_entry); + } + + return true; } else { @@ -495,7 +511,7 @@ bool CoreLogic::ProcessMeasurement(std::shared_ptr sensor, const // Store Measurement as out of order mars::BufferEntryType new_ooo_measurement_buffer_entry(timestamp, data, sensor, - mars::BufferMetadataType::measurement_ooo); + mars::BufferMetadataType::out_of_order); int out_of_order_buffer_idx = buffer_.AddEntrySorted(new_ooo_measurement_buffer_entry); @@ -509,34 +525,5 @@ bool CoreLogic::ProcessMeasurement(std::shared_ptr sensor, const return true; } - - // Main part to process the measurement - if (sensor == core_states_->propagation_sensor_) - { - // Propagate State with System Input from Propagation Sensor - - // Since the measurement was not out of order, get latest state is valid - mars::BufferEntryType latest_state_buffer_entry; - buffer_.get_latest_state(&latest_state_buffer_entry); - - mars::BufferEntryType new_core_state_entry; - new_core_state_entry = PerformCoreStatePropagation(sensor, timestamp, std::make_shared(data), - std::make_shared(latest_state_buffer_entry)); - - buffer_.AddEntrySorted(new_core_state_entry); - } - else - { - // Process non-propagation sensor information - mars::BufferEntryType new_state_buffer_entry; - if (!PerformSensorUpdate(&new_state_buffer_entry, sensor, timestamp, std::make_shared(data))) - { - return false; - } - - buffer_.AddEntrySorted(new_state_buffer_entry); - } - - return true; } } // namespace mars diff --git a/source/tests/mars-e2e-test/mars_e2e_imu_pose_update.cpp b/source/tests/mars-e2e-test/mars_e2e_imu_pose_update.cpp index 30997af..1c92225 100644 --- a/source/tests/mars-e2e-test/mars_e2e_imu_pose_update.cpp +++ b/source/tests/mars-e2e-test/mars_e2e_imu_pose_update.cpp @@ -124,6 +124,9 @@ TEST_F(mars_e2e_imu_pose_update, END_2_END_IMU_POSE_UPDATE) // create the CoreLogic and link the core states mars::CoreLogic core_logic(core_states_sptr); + core_logic.verbose_ = false; + core_logic.add_interm_buffer_entries_ = true; + // Open file for data export std::ofstream ofile_core; ofile_core.open("/tmp/mars_core_state.csv", std::ios::out); @@ -136,12 +139,12 @@ TEST_F(mars_e2e_imu_pose_update, END_2_END_IMU_POSE_UPDATE) // process data for (auto k : measurement_data) { - core_logic.ProcessMeasurement(k.sensor_, k.timestamp_, k.data_); + core_logic.ProcessMeasurement(k.sensor_handle_, k.timestamp_, k.data_); if (!core_logic.core_is_initialized_) { // Initialize the first time at which the propagation sensor occures - if (k.sensor_ == core_logic.core_states_->propagation_sensor_) + if (k.sensor_handle_ == core_logic.core_states_->propagation_sensor_) { Eigen::Vector3d p_wi_init(0, 0, 5); Eigen::Quaterniond q_wi_init = Eigen::Quaterniond::Identity(); @@ -154,21 +157,21 @@ TEST_F(mars_e2e_imu_pose_update, END_2_END_IMU_POSE_UPDATE) } // Store results in a csv file - if (k.sensor_ == core_logic.core_states_->propagation_sensor_) + if (k.sensor_handle_ == core_logic.core_states_->propagation_sensor_) { mars::BufferEntryType latest_result; core_logic.buffer_.get_latest_state(&latest_result); - mars::CoreStateType last_state = static_cast(latest_result.data_.core_.get())->state_; + mars::CoreStateType last_state = static_cast(latest_result.data_.core_state_.get())->state_; ofile_core << last_state.to_csv_string(latest_result.timestamp_.get_seconds()) << std::endl; } - if (k.sensor_ == pose_sensor_sptr) + if (k.sensor_handle_ == pose_sensor_sptr) { // Repropagation after an out of order update can cause the latest state to be different from the current update // sensor. Using get_latest_sensor_handle_state is the safest option. mars::BufferEntryType latest_result; core_logic.buffer_.get_latest_sensor_handle_state(pose_sensor_sptr, &latest_result); - mars::PoseSensorStateType last_state = pose_sensor_sptr->get_state(latest_result.data_.sensor_); + mars::PoseSensorStateType last_state = pose_sensor_sptr->get_state(latest_result.data_.sensor_state_); ofile_pose << last_state.to_csv_string(latest_result.timestamp_.get_seconds()) << std::endl; } } @@ -178,7 +181,7 @@ TEST_F(mars_e2e_imu_pose_update, END_2_END_IMU_POSE_UPDATE) mars::BufferEntryType latest_result; core_logic.buffer_.get_latest_state(&latest_result); - mars::CoreStateType last_state = static_cast(latest_result.data_.core_.get())->state_; + mars::CoreStateType last_state = static_cast(latest_result.data_.core_state_.get())->state_; std::cout << "Last State:" << std::endl; std::cout << last_state << std::endl; diff --git a/source/tests/mars-e2e-test/mars_e2e_imu_prop.cpp b/source/tests/mars-e2e-test/mars_e2e_imu_prop.cpp index 0bf6e37..6540e1a 100644 --- a/source/tests/mars-e2e-test/mars_e2e_imu_prop.cpp +++ b/source/tests/mars-e2e-test/mars_e2e_imu_prop.cpp @@ -87,12 +87,12 @@ TEST_F(mars_e2e_imu_prop, END_2_END_IMU_PROPAGATION) // process data for (auto k : measurement_data) { - core_logic.ProcessMeasurement(k.sensor_, k.timestamp_, k.data_); + core_logic.ProcessMeasurement(k.sensor_handle_, k.timestamp_, k.data_); if (!core_logic.core_is_initialized_) { // Initialize the first time at which the propagation sensor occures - if (k.sensor_ == core_logic.core_states_->propagation_sensor_) + if (k.sensor_handle_ == core_logic.core_states_->propagation_sensor_) { // Initialize with ground truth Eigen::Vector3d p_wi_init(0, 0, 5); @@ -118,8 +118,8 @@ TEST_F(mars_e2e_imu_prop, END_2_END_IMU_PROPAGATION) std::cout << "Timestamp: " << latest_result.timestamp_ << std::endl; - mars::CoreStateType last_state = static_cast(latest_result.data_.core_.get())->state_; - Eigen::MatrixXd last_state_cov = static_cast(latest_result.data_.core_.get())->cov_; + mars::CoreStateType last_state = static_cast(latest_result.data_.core_state_.get())->state_; + Eigen::MatrixXd last_state_cov = static_cast(latest_result.data_.core_state_.get())->cov_; std::cout << "Last State:" << std::endl; std::cout << last_state << std::endl; diff --git a/source/tests/mars-e2e-test/mars_e2e_imu_prop_empty_updates.cpp b/source/tests/mars-e2e-test/mars_e2e_imu_prop_empty_updates.cpp index 83de93d..7aad434 100644 --- a/source/tests/mars-e2e-test/mars_e2e_imu_prop_empty_updates.cpp +++ b/source/tests/mars-e2e-test/mars_e2e_imu_prop_empty_updates.cpp @@ -58,13 +58,12 @@ class mars_e2e_imu_prop_empty_update : public testing::Test std::vector measurement_data_empty; measurement_data_empty.resize(num_empty_meas); - mars::BufferDataType data; - data.set_sensor_data(std::make_shared(13)); + mars::BufferDataType data(std::make_shared(13)); for (int k = 0; k < num_empty_meas; k++) { mars::Time t_sensor(t_begin.get_seconds() + dt * k); - mars::BufferEntryType current_entry(t_sensor, data, sensor_sptr, mars::BufferMetadataType::measurement); + mars::BufferEntryType current_entry(t_sensor, data, sensor_sptr); measurement_data_empty.at(k) = current_entry; } @@ -174,12 +173,12 @@ TEST_F(mars_e2e_imu_prop_empty_update, END_2_END_IMU_PROPAGATION) // process data for (auto k : measurement_data) { - core_logic.ProcessMeasurement(k.sensor_, k.timestamp_, k.data_); + core_logic.ProcessMeasurement(k.sensor_handle_, k.timestamp_, k.data_); if (!core_logic.core_is_initialized_) { // Initialize the first time at which the propagation sensor occures - if (k.sensor_ == core_logic.core_states_->propagation_sensor_) + if (k.sensor_handle_ == core_logic.core_states_->propagation_sensor_) { // Initialize with ground truth Eigen::Vector3d p_wi_init(0, 0, 5); @@ -205,8 +204,8 @@ TEST_F(mars_e2e_imu_prop_empty_update, END_2_END_IMU_PROPAGATION) std::cout << "Timestamp: " << latest_result.timestamp_ << std::endl; - mars::CoreStateType last_state = static_cast(latest_result.data_.core_.get())->state_; - Eigen::MatrixXd last_state_cov = static_cast(latest_result.data_.core_.get())->cov_; + mars::CoreStateType last_state = static_cast(latest_result.data_.core_state_.get())->state_; + Eigen::MatrixXd last_state_cov = static_cast(latest_result.data_.core_state_.get())->cov_; std::cout << "Last State:" << std::endl; std::cout << last_state << std::endl; diff --git a/source/tests/mars-test/mars_buffer.cpp b/source/tests/mars-test/mars_buffer.cpp index d772b1c..fa800f2 100644 --- a/source/tests/mars-test/mars_buffer.cpp +++ b/source/tests/mars-test/mars_buffer.cpp @@ -24,6 +24,15 @@ class mars_buffer_test : public testing::Test { public: + mars::BufferDataType data_no_state_; + mars::BufferDataType data_with_state_; + + mars_buffer_test() + { + int core_dummy = 13; + int sensor_dummy = 15; + data_with_state_.set_states(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + } }; /// @@ -39,9 +48,12 @@ TEST_F(mars_buffer_test, CTOR) // Test setter of max buffer size mars::Buffer buffer_setter_1(100); + // Change trough setter method buffer_setter_1.set_max_buffer_size(200); + // Check setter result ASSERT_EQ(buffer_setter_1.get_max_buffer_size(), 200); buffer_setter_1.set_max_buffer_size(-1 * 200); + // Check negative values for setter ASSERT_EQ(buffer_setter_1.get_max_buffer_size(), 200); } @@ -67,7 +79,7 @@ TEST_F(mars_buffer_test, GETTER_EMPTY_BUFFER_RETURN) exit(EXIT_FAILURE); } - // Empty buffer return tests + // Check empty buffer returns mars::BufferEntryType latest_state; int latest_state_return = buffer.get_latest_state(&latest_state); ASSERT_EQ(latest_state_return, 0); @@ -118,34 +130,90 @@ TEST_F(mars_buffer_test, GETTER_EMPTY_BUFFER_RETURN) /// /// \brief Test that old entrys are removed if max_buffer_size is reached /// -// TBD(CHB): Waiting for new test implementation -// TEST_F(mars_buffer_test, STORAGE_MAX_ENTRY) -//{ -// // Force removal of overflowing buffer entrys -// const int num_test_entrys = 20; -// const int max_buffer_size = 10; -// mars::Buffer buffer(max_buffer_size); +TEST_F(mars_buffer_test, STORAGE_MAX_ENTRY) +{ + // Handle removal of overflowing buffer entrys -// std::shared_ptr core_states_sptr = std::make_shared(); -// std::shared_ptr pose_sensor_1_sptr = -// std::make_shared("Pose_1", core_states_sptr); + const int num_test_entrys = 20; + const int max_buffer_size = 10; + mars::Buffer buffer(max_buffer_size); -// mars::Time timestamp(0); -// int core_dummy = 13; -// int sensor_dummy = 15; -// mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + std::shared_ptr core_states_sptr = std::make_shared(); + std::shared_ptr pose_sensor_1_sptr = + std::make_shared("Pose_1", core_states_sptr); + std::shared_ptr pose_sensor_2_sptr = + std::make_shared("Pose_2", core_states_sptr); -// for (int k = num_test_entrys; k > 0; k--) -// { -// mars::BufferEntryType entry(timestamp + mars::Time(1), data, pose_sensor_1_sptr, 1); -// buffer.AddEntrySorted(entry); -// } + // Normal case + int core_dummy = 13; + int sensor_dummy = 15; + mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); -// std::cout << "Buffer Length: " << buffer.get_length() << std::endl; -// buffer.PrintBufferEntries(); + for (int k = 0; k < num_test_entrys; k++) + { + mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_1_sptr, 1); + buffer.AddEntrySorted(entry); + } -// ASSERT_EQ(buffer.get_length(), max_buffer_size); -//} + std::cout << "Buffer Length: " << buffer.get_length() << std::endl; + buffer.PrintBufferEntries(); + + ASSERT_EQ(buffer.get_length(), max_buffer_size); + + // Case where the last entry is also the last sensor handle + mars::BufferDataType data2(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + mars::Buffer buffer2(max_buffer_size); + + for (int k = 0; k < num_test_entrys; k++) + { + if (k == 0) + { + mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_1_sptr, 1); + buffer2.AddEntrySorted(entry); + } + else + { + mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_2_sptr, 1); + buffer2.AddEntrySorted(entry); + } + } + buffer2.PrintBufferEntries(); + ASSERT_EQ(buffer.get_length(), max_buffer_size); + + mars::BufferEntryType t2_idx0, t2_idx1; + buffer2.get_entry_at_idx(0, &t2_idx0); + buffer2.get_entry_at_idx(1, &t2_idx1); + + ASSERT_EQ(t2_idx0.timestamp_, 0); + ASSERT_EQ(t2_idx1.timestamp_, 11); + + // Case where the last entry becomes the last sensor handle while adding other entries + mars::BufferDataType data3(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + mars::Buffer buffer3(max_buffer_size); + + for (int k = 0; k < num_test_entrys; k++) + { + if (k == 0 || k == 5 || k == 9) + { + mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_1_sptr, 1); + buffer3.AddEntrySorted(entry); + } + else + { + mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_2_sptr, 1); + buffer3.AddEntrySorted(entry); + } + } + buffer3.PrintBufferEntries(); + ASSERT_EQ(buffer.get_length(), max_buffer_size); + + mars::BufferEntryType t3_idx0, t3_idx1; + buffer3.get_entry_at_idx(0, &t2_idx0); + buffer3.get_entry_at_idx(1, &t2_idx1); + + ASSERT_EQ(t2_idx0.timestamp_, 9); + ASSERT_EQ(t2_idx1.timestamp_, 11); +} TEST_F(mars_buffer_test, LATEST_ENTRY) { @@ -171,12 +239,12 @@ TEST_F(mars_buffer_test, LATEST_ENTRY) if (k % 2 == 0) { - mars::BufferEntryType entry(current_timestamp, data, pose_sensor_1_sptr, 1); + mars::BufferEntryType entry(current_timestamp, data, pose_sensor_1_sptr); buffer.AddEntrySorted(entry); } else { - mars::BufferEntryType entry(current_timestamp, data, pose_sensor_2_sptr, 4); + mars::BufferEntryType entry(current_timestamp, data, pose_sensor_2_sptr); buffer.AddEntrySorted(entry); } } @@ -193,6 +261,8 @@ TEST_F(mars_buffer_test, LATEST_ENTRY) TEST_F(mars_buffer_test, OLDEST_LATEST_STATE_RETURN) { + /// NEEDS REWRITING + const int num_test_entrys = 10; const int max_buffer_size = 100; mars::Buffer buffer(max_buffer_size); @@ -213,24 +283,24 @@ TEST_F(mars_buffer_test, OLDEST_LATEST_STATE_RETURN) std::shared_ptr pose_sensor_2_sptr = std::make_shared("Pose_2", core_states_sptr); - mars::Time timestamp(0); int core_dummy = 13; int sensor_dummy = 15; + int measurement_dummy = 12; for (int k = num_test_entrys; k > 0; k--) { - mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + mars::BufferDataType data; + data.set_measurement(std::make_shared(measurement_dummy)); + data.set_states(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); if (k % 2 == 0) { - mars::BufferEntryType entry(timestamp + mars::Time(1), data, pose_sensor_1_sptr, - mars::BufferMetadataType::sensor_state); + mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_1_sptr); buffer.AddEntrySorted(entry); } else { - mars::BufferEntryType entry(timestamp + mars::Time(1), data, pose_sensor_2_sptr, - mars::BufferMetadataType::measurement); + mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_2_sptr); buffer.AddEntrySorted(entry); } } @@ -241,14 +311,54 @@ TEST_F(mars_buffer_test, OLDEST_LATEST_STATE_RETURN) mars::BufferEntryType latest_state_full; int latest_filled_state_return = buffer.get_latest_state(&latest_state_full); ASSERT_EQ(latest_filled_state_return, 1); - ASSERT_EQ(latest_state_full.metadata_, mars::BufferMetadataType::sensor_state); - std::cout << "Picked:\n" << latest_state_full << std::endl; + ASSERT_EQ(latest_state_full.timestamp_, 10); + std::cout << "Pick for latest:\n" << latest_state_full << std::endl; mars::BufferEntryType oldest_state_full; int oldest_filled_state_return = buffer.get_oldest_state(&oldest_state_full); ASSERT_EQ(oldest_filled_state_return, 1); - ASSERT_EQ(oldest_state_full.metadata_, mars::BufferMetadataType::sensor_state); - std::cout << "Picked:\n" << oldest_state_full << std::endl; + ASSERT_EQ(oldest_state_full.timestamp_, 1); + std::cout << "Picked for oldest:\n" << oldest_state_full << std::endl; + + // Second case where the latest and newest do not have a state, only a measurement + mars::Buffer buffer2(max_buffer_size); + for (int k = num_test_entrys; k > 0; k--) + { + mars::BufferDataType data; + data.set_measurement(std::make_shared(measurement_dummy)); + data.set_states(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + + if (k == 1 || k == num_test_entrys) + { + data.ClearStates(); + } + + if (k % 2 == 0) + { + mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_1_sptr); + buffer2.AddEntrySorted(entry); + } + else + { + mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_2_sptr); + buffer2.AddEntrySorted(entry); + } + } + + std::cout << buffer2.get_length() << std::endl; + buffer2.PrintBufferEntries(); + + mars::BufferEntryType latest_state_full2; + int latest_filled_state_return2 = buffer2.get_latest_state(&latest_state_full); + ASSERT_EQ(latest_filled_state_return2, 1); + ASSERT_EQ(latest_state_full.timestamp_, 9); + std::cout << "Pick for latest:\n" << latest_state_full << std::endl; + + mars::BufferEntryType oldest_state_full2; + int oldest_filled_state_return2 = buffer2.get_oldest_state(&oldest_state_full); + ASSERT_EQ(oldest_filled_state_return2, 1); + ASSERT_EQ(oldest_state_full.timestamp_, 2); + std::cout << "Picked for oldest:\n" << oldest_state_full << std::endl; } /// @@ -266,7 +376,6 @@ TEST_F(mars_buffer_test, RESET_BUFFER) std::shared_ptr pose_sensor_2_sptr = std::make_shared("Pose_2", core_states_sptr); - mars::Time timestamp(0); int core_dummy = 13; int sensor_dummy = 15; @@ -276,12 +385,12 @@ TEST_F(mars_buffer_test, RESET_BUFFER) if (k % 2 == 0) { - mars::BufferEntryType entry(timestamp + mars::Time(1), data, pose_sensor_1_sptr, 1); + mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_1_sptr); buffer.AddEntrySorted(entry); } else { - mars::BufferEntryType entry(timestamp + mars::Time(1), data, pose_sensor_2_sptr, 1); + mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_2_sptr); buffer.AddEntrySorted(entry); } } @@ -291,6 +400,7 @@ TEST_F(mars_buffer_test, RESET_BUFFER) ASSERT_EQ(buffer.get_length(), num_test_entrys); ASSERT_EQ(buffer.IsEmpty(), 0); + std::cout << "Reset Buffer" << std::endl; buffer.ResetBufferData(); std::cout << buffer.get_length() << std::endl; @@ -309,21 +419,22 @@ TEST_F(mars_buffer_test, GET_ENTRY_METHODS) std::shared_ptr pose_sensor_2_sptr = std::make_shared("Pose_2", core_states_sptr); - int core_dummy = 13; - int sensor_dummy = 15; - mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); - - buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::init_state)); - buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_2_sptr, mars::BufferMetadataType::init_state)); - buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_1_sptr, mars::BufferMetadataType::core_state)); - buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_2_sptr, mars::BufferMetadataType::core_state)); - buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(6, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(8, data, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(9, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement_ooo)); - buffer.AddEntrySorted(mars::BufferEntryType(10, data, pose_sensor_2_sptr, mars::BufferMetadataType::measurement_ooo)); + mars::BufferDataType data_with_state(this->data_with_state_); + mars::BufferDataType data_no_state(this->data_no_state_); + + buffer.AddEntrySorted(mars::BufferEntryType(0, data_no_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(1, data_with_state, pose_sensor_1_sptr, mars::BufferMetadataType::init)); + buffer.AddEntrySorted(mars::BufferEntryType(2, data_with_state, pose_sensor_2_sptr, mars::BufferMetadataType::init)); + buffer.AddEntrySorted(mars::BufferEntryType(3, data_no_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(4, data_with_state, pose_sensor_2_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(5, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(6, data_no_state, pose_sensor_2_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(7, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(8, data_with_state, pose_sensor_2_sptr)); + buffer.AddEntrySorted( + mars::BufferEntryType(9, data_no_state, pose_sensor_1_sptr, mars::BufferMetadataType::out_of_order)); + buffer.AddEntrySorted( + mars::BufferEntryType(10, data_no_state, pose_sensor_2_sptr, mars::BufferMetadataType::out_of_order)); mars::BufferEntryType latest_entry_return; buffer.get_latest_entry(&latest_entry_return); @@ -335,7 +446,7 @@ TEST_F(mars_buffer_test, GET_ENTRY_METHODS) mars::BufferEntryType oldest_core_state_return; buffer.get_oldest_core_state(&oldest_core_state_return); - ASSERT_EQ(oldest_core_state_return.timestamp_, 3); + ASSERT_EQ(oldest_core_state_return.timestamp_, 1); mars::BufferEntryType latest_init_state_return; buffer.get_latest_init_state(&latest_init_state_return); @@ -343,25 +454,25 @@ TEST_F(mars_buffer_test, GET_ENTRY_METHODS) mars::BufferEntryType latest_state_return; buffer.get_latest_state(&latest_state_return); - ASSERT_EQ(latest_state_return.timestamp_, 6); + ASSERT_EQ(latest_state_return.timestamp_, 8); mars::BufferEntryType latest_sensor1_handle_state_return; buffer.get_latest_sensor_handle_state(pose_sensor_1_sptr, &latest_sensor1_handle_state_return); - ASSERT_EQ(latest_sensor1_handle_state_return.timestamp_, 5); + ASSERT_EQ(latest_sensor1_handle_state_return.timestamp_, 7); int latest_sensor1_handle_state_index; buffer.get_latest_sensor_handle_state(pose_sensor_1_sptr, &latest_sensor1_handle_state_return, &latest_sensor1_handle_state_index); - ASSERT_EQ(latest_sensor1_handle_state_index, 5); + ASSERT_EQ(latest_sensor1_handle_state_index, 7); mars::BufferEntryType latest_sensor2_handle_state_return; buffer.get_latest_sensor_handle_state(pose_sensor_2_sptr, &latest_sensor2_handle_state_return); - ASSERT_EQ(latest_sensor2_handle_state_return.timestamp_, 6); + ASSERT_EQ(latest_sensor2_handle_state_return.timestamp_, 8); int latest_sensor2_handle_state_index; buffer.get_latest_sensor_handle_state(pose_sensor_2_sptr, &latest_sensor2_handle_state_return, &latest_sensor2_handle_state_index); - ASSERT_EQ(latest_sensor2_handle_state_index, 6); + ASSERT_EQ(latest_sensor2_handle_state_index, 8); mars::BufferEntryType latest_sensor1_handle_measurement_return; buffer.get_latest_sensor_handle_measurement(pose_sensor_1_sptr, &latest_sensor1_handle_measurement_return); @@ -380,17 +491,16 @@ TEST_F(mars_buffer_test, GET_CLOSEST_STATE) std::shared_ptr pose_sensor_1_sptr = std::make_shared("Pose_1", core_states_sptr); - int core_dummy = 13; - int sensor_dummy = 15; - mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + mars::BufferDataType data_with_state(this->data_with_state_); + mars::BufferDataType data_no_state(this->data_no_state_); - buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // Fill buffer with measurements only + buffer.AddEntrySorted(mars::BufferEntryType(0, data_no_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(1, data_no_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(2, data_no_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(3, data_no_state, pose_sensor_1_sptr)); // Buffer is not empty but has no state, needs to return false - mars::BufferEntryType no_state_entry; int no_state_return = buffer.get_closest_state(2, &no_state_entry); ASSERT_EQ(no_state_return, 0); @@ -400,12 +510,12 @@ TEST_F(mars_buffer_test, GET_CLOSEST_STATE) buffer.get_closest_state(2, &no_state_entry, &closest_state_idx_no_state); ASSERT_EQ(closest_state_idx_no_state, -1); - buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::init_state)); - buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(6, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(8, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(9, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + buffer.AddEntrySorted(mars::BufferEntryType(4, data_with_state, pose_sensor_1_sptr, mars::BufferMetadataType::init)); + buffer.AddEntrySorted(mars::BufferEntryType(5, data_no_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(6, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(7, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(8, data_no_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(9, data_with_state, pose_sensor_1_sptr)); // Equal timestamp mars::Time search_timestamp(6); @@ -431,7 +541,7 @@ TEST_F(mars_buffer_test, GET_CLOSEST_STATE) buffer.get_closest_state(search_timestamp_4, &close_to_newer_state_return); ASSERT_EQ(close_to_newer_state_return.timestamp_, 7); - // Timestamp newer than newer state + // Timestamp newer than latest state mars::Time search_timestamp_5(10); mars::BufferEntryType newer_than_newer_state_return; buffer.get_closest_state(search_timestamp_5, &newer_than_newer_state_return); @@ -448,16 +558,19 @@ TEST_F(mars_buffer_test, GET_CLOSEST_STATE) TEST_F(mars_buffer_test, GET_CLOSEST_STATE_ONLY_ONE_STATE_IN_BUFFER) { mars::Buffer buffer; - mars::BufferDataType data; mars::Time timestamp(0.0); + mars::BufferDataType data_with_state(this->data_with_state_); + mars::BufferDataType data_no_state(this->data_no_state_); // setup propagation sensor std::shared_ptr imu_sensor_sptr = std::make_shared("IMU"); - mars::BufferEntryType meas_entry(timestamp, data, imu_sensor_sptr, mars::BufferMetadataType::measurement); + // Only measurement + mars::BufferEntryType meas_entry(timestamp, data_no_state, imu_sensor_sptr); buffer.AddEntrySorted(meas_entry); - mars::BufferEntryType state_entry(timestamp, data, imu_sensor_sptr, mars::BufferMetadataType::core_state); + // Measurement and state + mars::BufferEntryType state_entry(timestamp, data_with_state, imu_sensor_sptr); buffer.AddEntrySorted(state_entry); mars::BufferEntryType result; @@ -475,14 +588,13 @@ TEST_F(mars_buffer_test, GET_ENTRY_AT_INDEX) std::shared_ptr pose_sensor_1_sptr = std::make_shared("Pose_1", core_states_sptr); - int core_dummy = 13; - int sensor_dummy = 15; - mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + mars::BufferDataType data_with_state(this->data_with_state_); + mars::BufferDataType data_no_state(this->data_no_state_); - buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + buffer.AddEntrySorted(mars::BufferEntryType(0, data_no_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(1, data_no_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(2, data_no_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(3, data_no_state, pose_sensor_1_sptr)); // test return entry value mars::BufferEntryType entry_return; @@ -493,11 +605,11 @@ TEST_F(mars_buffer_test, GET_ENTRY_AT_INDEX) ASSERT_EQ(entry_return.timestamp_, k); } - // test return success status - + // Test return success status + // In valid range ASSERT_EQ(buffer.get_entry_at_idx(0, &entry_return), 1); ASSERT_EQ(buffer.get_entry_at_idx(3, &entry_return), 1); - + // Outside valid range ASSERT_EQ(buffer.get_entry_at_idx(-1, &entry_return), 0); ASSERT_EQ(buffer.get_entry_at_idx(4, &entry_return), 0); } @@ -511,17 +623,16 @@ TEST_F(mars_buffer_test, ADD_SORTED) std::shared_ptr pose_sensor_1_sptr = std::make_shared("Pose_1", core_states_sptr); - int core_dummy = 13; - int sensor_dummy = 15; - mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + mars::BufferDataType data_with_state(this->data_with_state_); + mars::BufferDataType data_no_state(this->data_no_state_); - buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(3.2, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(6, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + buffer.AddEntrySorted(mars::BufferEntryType(1, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(0, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(3.2, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(4, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(2, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(6, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(5, data_with_state, pose_sensor_1_sptr)); buffer.PrintBufferEntries(); ASSERT_EQ(buffer.IsSorted(), 1); @@ -530,81 +641,115 @@ TEST_F(mars_buffer_test, ADD_SORTED) TEST_F(mars_buffer_test, REMOVE_STATES_STARTING_AT_IDX) { - const int max_buffer_size = 10; - mars::Buffer buffer(max_buffer_size); + mars::Buffer buffer(100); std::shared_ptr core_states_sptr = std::make_shared(); std::shared_ptr pose_sensor_1_sptr = std::make_shared("Pose_1", core_states_sptr); - int core_dummy = 13; - int sensor_dummy = 15; - mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + mars::BufferDataType data_with_state(this->data_with_state_); + mars::BufferDataType data_no_state(this->data_no_state_); - buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(6, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(8, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(9, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + buffer.AddEntrySorted(mars::BufferEntryType(0, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(1, data_no_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(2, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(3, data_no_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(4, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(5, data_no_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(6, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(7, data_no_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(8, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(9, data_no_state, pose_sensor_1_sptr)); buffer.PrintBufferEntries(); - buffer.DeleteStatesStartingAtIdx(4); + buffer.ClearStatesStartingAtIdx(4); buffer.PrintBufferEntries(); - ASSERT_EQ(buffer.get_length(), 6); -} - -TEST_F(mars_buffer_test, MULTI_SENSOR_TYPE_SETUP) -{ - const int max_buffer_size = 100; - mars::Buffer buffer(max_buffer_size); + // Size needs to remain the same because only states are removed + ASSERT_EQ(buffer.get_length(), 10); - std::shared_ptr core_states_sptr = std::make_shared(); - std::shared_ptr pose_sensor_1_sptr = - std::make_shared("Pose_1", core_states_sptr); - std::shared_ptr position_sensor_1_sptr = - std::make_shared("Position_1", core_states_sptr); + for (int i = 4; i < buffer.get_length(); i++) + { + mars::BufferEntryType entry_return; + buffer.get_entry_at_idx(i, &entry_return); - int core_dummy = 13; - int sensor_dummy = 15; - mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + ASSERT_EQ(entry_return.HasStates(), false); + } - buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::init_state)); - buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + // Check that auto entries are removed entirely + buffer.AddEntrySorted( + mars::BufferEntryType(10, data_no_state, pose_sensor_1_sptr, mars::BufferMetadataType::auto_add)); - buffer.AddEntrySorted(mars::BufferEntryType(3, data, position_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(3, data, position_sensor_1_sptr, mars::BufferMetadataType::init_state)); - buffer.AddEntrySorted(mars::BufferEntryType(4, data, position_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(4, data, position_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + buffer.PrintBufferEntries(); + mars::BufferEntryType entry_return; + buffer.get_latest_entry(&entry_return); - buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + // Sanity Check that auto added state exists + EXPECT_TRUE(entry_return.timestamp_ == 10); + ASSERT_EQ(buffer.get_length(), 11); - buffer.AddEntrySorted(mars::BufferEntryType(6, data, position_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(6, data, position_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + // Clear buffer states and auto entries again + buffer.ClearStatesStartingAtIdx(4); - buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + // Expect that the auto added entry is removed + buffer.get_latest_entry(&entry_return); + EXPECT_TRUE(entry_return.timestamp_ == 9); + ASSERT_EQ(buffer.get_length(), 10); buffer.PrintBufferEntries(); - ASSERT_EQ(buffer.IsSorted(), 1); - ASSERT_EQ(buffer.get_length(), 14); - - // If the new timestamp matches with an existing one, then the new entry is added after the existing one - mars::BufferEntryType entry_return_0; - buffer.get_entry_at_idx(0, &entry_return_0); - ASSERT_EQ(entry_return_0.metadata_, mars::BufferMetadataType::measurement); +} - mars::BufferEntryType entry_return_1; - buffer.get_entry_at_idx(1, &entry_return_1); - ASSERT_EQ(entry_return_1.metadata_, mars::BufferMetadataType::init_state); +TEST_F(mars_buffer_test, MULTI_SENSOR_TYPE_SETUP) +{ + // const int max_buffer_size = 100; + // mars::Buffer buffer(max_buffer_size); + + // std::shared_ptr core_states_sptr = std::make_shared(); + // std::shared_ptr pose_sensor_1_sptr = + // std::make_shared("Pose_1", core_states_sptr); + // std::shared_ptr position_sensor_1_sptr = + // std::make_shared("Position_1", core_states_sptr); + + // mars::BufferDataType data_with_state(this->data_with_state_); + // mars::BufferDataType data_no_state(this->data_no_state_); + + // buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::init_state)); + // buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_1_sptr, + // mars::BufferMetadataType::sensor_state)); + + // buffer.AddEntrySorted(mars::BufferEntryType(3, data, position_sensor_1_sptr, + // mars::BufferMetadataType::measurement)); buffer.AddEntrySorted(mars::BufferEntryType(3, data, + // position_sensor_1_sptr, mars::BufferMetadataType::init_state)); buffer.AddEntrySorted(mars::BufferEntryType(4, + // data, position_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(4, data, position_sensor_1_sptr, + // mars::BufferMetadataType::sensor_state)); + + // buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, + // mars::BufferMetadataType::sensor_state)); + + // buffer.AddEntrySorted(mars::BufferEntryType(6, data, position_sensor_1_sptr, + // mars::BufferMetadataType::measurement)); buffer.AddEntrySorted(mars::BufferEntryType(6, data, + // position_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + + // buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_1_sptr, + // mars::BufferMetadataType::sensor_state)); + + // buffer.PrintBufferEntries(); + // ASSERT_EQ(buffer.IsSorted(), 1); + // ASSERT_EQ(buffer.get_length(), 14); + + // // If the new timestamp matches with an existing one, then the new entry is added after the existing one + // mars::BufferEntryType entry_return_0; + // buffer.get_entry_at_idx(0, &entry_return_0); + // ASSERT_EQ(entry_return_0.metadata_, mars::BufferMetadataType::measurement); + + // mars::BufferEntryType entry_return_1; + // buffer.get_entry_at_idx(1, &entry_return_1); + // ASSERT_EQ(entry_return_1.metadata_, mars::BufferMetadataType::init_state); } TEST_F(mars_buffer_test, INSERT_DATA_IDX_TEST) @@ -615,32 +760,27 @@ TEST_F(mars_buffer_test, INSERT_DATA_IDX_TEST) std::shared_ptr pose_sensor_1_sptr = std::make_shared("Pose_1", core_states_sptr); - int core_dummy = 13; - int sensor_dummy = 15; - mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + mars::BufferDataType data_with_state(this->data_with_state_); + mars::BufferDataType data_no_state(this->data_no_state_); - buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::init_state)); - buffer.AddEntrySorted(mars::BufferEntryType(6, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + buffer.AddEntrySorted(mars::BufferEntryType(4, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(5, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(6, data_with_state, pose_sensor_1_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(7, data_with_state, pose_sensor_1_sptr)); // New entry is newer than newest existing entry and needs to be inserted at idx 4 - int idx_newer = buffer.InsertDataAtTimestamp( - mars::BufferEntryType(8, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + int idx_newer = buffer.InsertDataAtTimestamp(mars::BufferEntryType(8, data_with_state, pose_sensor_1_sptr)); ASSERT_EQ(4, idx_newer); // add entry in the middle of the buffer - int idx_mid = buffer.InsertDataAtTimestamp( - mars::BufferEntryType(5.3, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + int idx_mid = buffer.InsertDataAtTimestamp(mars::BufferEntryType(5.3, data_with_state, pose_sensor_1_sptr)); ASSERT_EQ(2, idx_mid); - idx_mid = buffer.InsertDataAtTimestamp( - mars::BufferEntryType(5.6, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + idx_mid = buffer.InsertDataAtTimestamp(mars::BufferEntryType(5.6, data_with_state, pose_sensor_1_sptr)); ASSERT_EQ(3, idx_mid); // New entry is older than oldest existing entry and needs to be inserted at idx 0 - int idx_older = buffer.InsertDataAtTimestamp( - mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + int idx_older = buffer.InsertDataAtTimestamp(mars::BufferEntryType(1, data_with_state, pose_sensor_1_sptr)); ASSERT_EQ(int(0), idx_older); } @@ -685,51 +825,73 @@ TEST_F(mars_buffer_test, CHECK_LAST_SENSOR_HANDLE) std::make_shared("Pose_1", core_states_sptr); std::shared_ptr pose_sensor_2_sptr = std::make_shared("Pose_2", core_states_sptr); + std::shared_ptr pose_sensor_3_sptr = + std::make_shared("Pose_3", core_states_sptr); - mars::BufferDataType data; + mars::BufferDataType data_with_state(this->data_with_state_); + mars::BufferDataType data_no_state(this->data_no_state_); mars::Time timestamp(1); - buffer.AddEntrySorted( - mars::BufferEntryType(timestamp, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted( - mars::BufferEntryType(timestamp, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - EXPECT_TRUE(buffer.CheckForLastSensorHandlePair(pose_sensor_1_sptr)); - // Return false on non-existing state of sensor 2 - EXPECT_FALSE(buffer.CheckForLastSensorHandlePair(pose_sensor_2_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(timestamp, data_no_state, pose_sensor_1_sptr)); + timestamp = timestamp + mars::Time(1); + buffer.AddEntrySorted(mars::BufferEntryType(timestamp, data_no_state, pose_sensor_1_sptr)); + timestamp = timestamp + mars::Time(1); + buffer.AddEntrySorted(mars::BufferEntryType(timestamp, data_no_state, pose_sensor_1_sptr)); + // Only non-state entries + EXPECT_FALSE(buffer.CheckForLastSensorHandleWithState(pose_sensor_1_sptr)); + EXPECT_FALSE(buffer.CheckForLastSensorHandleWithState(pose_sensor_2_sptr)); + EXPECT_FALSE(buffer.CheckForLastSensorHandleWithState(pose_sensor_3_sptr)); timestamp = timestamp + mars::Time(1); - buffer.AddEntrySorted( - mars::BufferEntryType(timestamp, data, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted( - mars::BufferEntryType(timestamp, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); - EXPECT_TRUE(buffer.CheckForLastSensorHandlePair(pose_sensor_2_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(timestamp, data_no_state, pose_sensor_2_sptr)); + timestamp = timestamp + mars::Time(1); + buffer.AddEntrySorted(mars::BufferEntryType(timestamp, data_no_state, pose_sensor_2_sptr)); + EXPECT_FALSE(buffer.CheckForLastSensorHandleWithState(pose_sensor_1_sptr)); + EXPECT_FALSE(buffer.CheckForLastSensorHandleWithState(pose_sensor_2_sptr)); + EXPECT_FALSE(buffer.CheckForLastSensorHandleWithState(pose_sensor_3_sptr)); + // One Pose 1 State timestamp = timestamp + mars::Time(1); - buffer.AddEntrySorted( - mars::BufferEntryType(timestamp, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted( - mars::BufferEntryType(timestamp, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - EXPECT_TRUE(buffer.CheckForLastSensorHandlePair(pose_sensor_1_sptr)); - EXPECT_TRUE(buffer.CheckForLastSensorHandlePair(pose_sensor_2_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(timestamp, data_with_state, pose_sensor_1_sptr)); + EXPECT_TRUE(buffer.CheckForLastSensorHandleWithState(pose_sensor_1_sptr)); + EXPECT_FALSE(buffer.CheckForLastSensorHandleWithState(pose_sensor_2_sptr)); + EXPECT_FALSE(buffer.CheckForLastSensorHandleWithState(pose_sensor_3_sptr)); + // Two Pose 1 States and one Pose 2 state timestamp = timestamp + mars::Time(1); - buffer.AddEntrySorted( - mars::BufferEntryType(timestamp, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted( - mars::BufferEntryType(timestamp, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - // Three entrys of sensor 1 exist - EXPECT_FALSE(buffer.CheckForLastSensorHandlePair(pose_sensor_1_sptr)); - EXPECT_TRUE(buffer.CheckForLastSensorHandlePair(pose_sensor_2_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(timestamp, data_with_state, pose_sensor_1_sptr)); + timestamp = timestamp + mars::Time(1); + buffer.AddEntrySorted(mars::BufferEntryType(timestamp, data_with_state, pose_sensor_2_sptr)); + EXPECT_FALSE(buffer.CheckForLastSensorHandleWithState(pose_sensor_1_sptr)); + EXPECT_TRUE(buffer.CheckForLastSensorHandleWithState(pose_sensor_2_sptr)); + EXPECT_FALSE(buffer.CheckForLastSensorHandleWithState(pose_sensor_3_sptr)); + + // Three Pose 1 States, two Pose 2 state, one Pose 3 state + timestamp = timestamp + mars::Time(1); + buffer.AddEntrySorted(mars::BufferEntryType(timestamp, data_with_state, pose_sensor_1_sptr)); + timestamp = timestamp + mars::Time(1); + buffer.AddEntrySorted(mars::BufferEntryType(timestamp, data_with_state, pose_sensor_2_sptr)); + timestamp = timestamp + mars::Time(1); + buffer.AddEntrySorted(mars::BufferEntryType(timestamp, data_with_state, pose_sensor_3_sptr)); + + EXPECT_FALSE(buffer.CheckForLastSensorHandleWithState(pose_sensor_1_sptr)); + EXPECT_FALSE(buffer.CheckForLastSensorHandleWithState(pose_sensor_2_sptr)); + EXPECT_TRUE(buffer.CheckForLastSensorHandleWithState(pose_sensor_3_sptr)); + + // Three Pose 1 States, two Pose 2 state, two Pose 3 state + timestamp = timestamp + mars::Time(1); + buffer.AddEntrySorted(mars::BufferEntryType(timestamp, data_with_state, pose_sensor_3_sptr)); + + EXPECT_FALSE(buffer.CheckForLastSensorHandleWithState(pose_sensor_1_sptr)); + EXPECT_FALSE(buffer.CheckForLastSensorHandleWithState(pose_sensor_2_sptr)); + EXPECT_FALSE(buffer.CheckForLastSensorHandleWithState(pose_sensor_3_sptr)); } -/// -/// \brief Tests if getting all measurements from a single sensor from buffer works. -/// -/// \author Martin Scheiber -/// -TEST_F(mars_buffer_test, GET_SENSOR_MEASUREMENTS) +TEST_F(mars_buffer_test, REMOVE_SENSOR_FROM_BUFFER) { - const int max_buffer_size = 20; + // Generate sensor entries + const int num_test_entrys = 100; + const int max_buffer_size = 110; mars::Buffer buffer(max_buffer_size); std::shared_ptr core_states_sptr = std::make_shared(); @@ -737,116 +899,164 @@ TEST_F(mars_buffer_test, GET_SENSOR_MEASUREMENTS) std::make_shared("Pose_1", core_states_sptr); std::shared_ptr pose_sensor_2_sptr = std::make_shared("Pose_2", core_states_sptr); - std::shared_ptr position_sensor_1_sptr = - std::make_shared("Position", core_states_sptr); - std::shared_ptr imu_sensor_sptr = std::make_shared("IMU"); - - int core_dummy = 13; - int sensor_dummy = 15; - mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); - - mars::PoseMeasurementType meas_pose1(Eigen::Vector3d(1, 1, 1), Eigen::Quaterniond::Identity()); - std::shared_ptr meas_pose1_ptr = std::make_shared(meas_pose1); - mars::BufferDataType data_pose1; - data_pose1.set_sensor_data(meas_pose1_ptr); - mars::PoseMeasurementType meas_pose2(Eigen::Vector3d(5, 4, 3), Eigen::Quaterniond::Identity()); - std::shared_ptr meas_pose2_ptr = std::make_shared(meas_pose2); - mars::BufferDataType data_pose2; - data_pose2.set_sensor_data(meas_pose2_ptr); - - buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::init_state)); - buffer.AddEntrySorted( - mars::BufferEntryType(5, data_pose1, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(6, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted( - mars::BufferEntryType(8, data_pose1, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(9, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - - buffer.AddEntrySorted(mars::BufferEntryType(11, data, pose_sensor_2_sptr, mars::BufferMetadataType::init_state)); - buffer.AddEntrySorted( - mars::BufferEntryType(9, data_pose2, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(8, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted( - mars::BufferEntryType(5, data_pose2, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted( - mars::BufferEntryType(1, data_pose2, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); - - buffer.AddEntrySorted(mars::BufferEntryType(3, data, position_sensor_1_sptr, mars::BufferMetadataType::init_state)); - buffer.AddEntrySorted(mars::BufferEntryType(4, data, position_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - - // test return measurements size1 - std::vector entries_return; - buffer.get_sensor_handle_measurements(pose_sensor_1_sptr, &entries_return); - ASSERT_EQ(entries_return.size(), 2); - - // iterate over buffer and test elements1 - int ts = 5; - for (const auto& it : entries_return) + for (int k = num_test_entrys; k > 0; k--) { - mars::PoseMeasurementType meas = *static_cast(it->data_.sensor_.get()); - std::cout << "pose1_meas: " << meas.position_.transpose() << std::endl; - // value - ASSERT_EQ((meas.position_ - Eigen::Vector3d(1, 1, 1)).norm(), 0); - // timestamp - ASSERT_EQ(it->timestamp_, ts); - ts += 3; - // check if pointer is corresponding to correct one - ASSERT_EQ(it->data_.sensor_, meas_pose1_ptr); - } - - // test return measurements size2 - buffer.get_sensor_handle_measurements(pose_sensor_2_sptr, &entries_return); - - ASSERT_EQ(entries_return.size(), 3); + mars::BufferDataType data(std::make_shared(13), std::make_shared(15)); - // iterate over buffer and test elements2 - ts = 1; - for (const auto& it : entries_return) - { - mars::PoseMeasurementType meas = *static_cast(it->data_.sensor_.get()); - std::cout << "pose2_meas: " << meas.position_.transpose() << std::endl; - ASSERT_EQ((meas.position_ - Eigen::Vector3d(5, 4, 3)).norm(), 0); - // timestamp - ASSERT_EQ(it->timestamp_, ts); - ts += 4; - // check if pointer is corresponding to correct one - ASSERT_EQ(it->data_.sensor_, meas_pose2_ptr); + if (k % 2 == 0 || k == 1 || k == 2) + { + mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_1_sptr); + buffer.AddEntrySorted(entry); + } + else + { + mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_2_sptr); + buffer.AddEntrySorted(entry); + } } - // change value one item, retreive values again, should still be the same - mars::PoseMeasurementType meas = *static_cast(entries_return.at(1)->data_.sensor_.get()); - meas.position_ = Eigen::Vector3d(1, 1, 1); - ASSERT_EQ((meas.position_ - Eigen::Vector3d(1, 1, 1)).norm(), 0); + mars::BufferEntryType data_sink; - // get all entries again and rerun above test case - buffer.get_sensor_handle_measurements(pose_sensor_2_sptr, &entries_return); - ASSERT_EQ(entries_return.size(), 3); - ts = 1; - for (const auto& it : entries_return) - { - mars::PoseMeasurementType meas = *static_cast(it->data_.sensor_.get()); - std::cout << "pose2_meas: " << meas.position_.transpose() << std::endl; - ASSERT_EQ((meas.position_ - Eigen::Vector3d(5, 4, 3)).norm(), 0); - ASSERT_EQ(it->timestamp_, ts); - ts += 4; - } - // if this succeeds then the entries are unchangable in the buffer (as they should be)! + // Check that both sensor instances are present + EXPECT_TRUE(buffer.get_latest_sensor_handle_measurement(pose_sensor_1_sptr, &data_sink)); + EXPECT_TRUE(buffer.get_latest_sensor_handle_measurement(pose_sensor_2_sptr, &data_sink)); - // test return measurements - buffer.get_sensor_handle_measurements(position_sensor_1_sptr, &entries_return); + // Remove sensor 1 from buffer + buffer.RemoveSensorFromBuffer(pose_sensor_1_sptr); - ASSERT_EQ(entries_return.size(), 0); - ASSERT_TRUE(entries_return.empty()); + // Check that sensor 1 is removed and sensor 2 still exists + EXPECT_FALSE(buffer.get_latest_sensor_handle_measurement(pose_sensor_1_sptr, &data_sink)); + EXPECT_TRUE(buffer.get_latest_sensor_handle_measurement(pose_sensor_2_sptr, &data_sink)); +} - // test return success status - ASSERT_EQ(buffer.get_sensor_handle_measurements(pose_sensor_1_sptr, &entries_return), 1); - ASSERT_EQ(buffer.get_sensor_handle_measurements(pose_sensor_2_sptr, &entries_return), 1); - ASSERT_EQ(buffer.get_sensor_handle_measurements(position_sensor_1_sptr, &entries_return), 0); - ASSERT_EQ(buffer.get_sensor_handle_measurements(imu_sensor_sptr, &entries_return), 0); +/// +/// \brief Tests if getting all measurements from a single sensor from buffer works. +/// +/// \author Martin Scheiber +/// +TEST_F(mars_buffer_test, GET_SENSOR_MEASUREMENTS) +{ + // const int max_buffer_size = 20; + // mars::Buffer buffer(max_buffer_size); + + // std::shared_ptr core_states_sptr = std::make_shared(); + // std::shared_ptr pose_sensor_1_sptr = + // std::make_shared("Pose_1", core_states_sptr); + // std::shared_ptr pose_sensor_2_sptr = + // std::make_shared("Pose_2", core_states_sptr); + // std::shared_ptr position_sensor_1_sptr = + // std::make_shared("Position", core_states_sptr); + // std::shared_ptr imu_sensor_sptr = std::make_shared("IMU"); + + // int core_dummy = 13; + // int sensor_dummy = 15; + // mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + + // mars::PoseMeasurementType meas_pose1(Eigen::Vector3d(1, 1, 1), Eigen::Quaterniond::Identity()); + // std::shared_ptr meas_pose1_ptr = + // std::make_shared(meas_pose1); mars::BufferDataType data_pose1; + // data_pose1.set_sensor_state(meas_pose1_ptr); + // mars::PoseMeasurementType meas_pose2(Eigen::Vector3d(5, 4, 3), Eigen::Quaterniond::Identity()); + // std::shared_ptr meas_pose2_ptr = + // std::make_shared(meas_pose2); mars::BufferDataType data_pose2; + // data_pose2.set_sensor_state(meas_pose2_ptr); + + // buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::init_state)); + // buffer.AddEntrySorted( + // mars::BufferEntryType(5, data_pose1, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(6, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + // buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + // buffer.AddEntrySorted( + // mars::BufferEntryType(8, data_pose1, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(9, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + + // buffer.AddEntrySorted(mars::BufferEntryType(11, data, pose_sensor_2_sptr, mars::BufferMetadataType::init_state)); + // buffer.AddEntrySorted( + // mars::BufferEntryType(9, data_pose2, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(8, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); + // buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); + // buffer.AddEntrySorted( + // mars::BufferEntryType(5, data_pose2, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); + // buffer.AddEntrySorted( + // mars::BufferEntryType(1, data_pose2, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); + + // buffer.AddEntrySorted(mars::BufferEntryType(3, data, position_sensor_1_sptr, + // mars::BufferMetadataType::init_state)); buffer.AddEntrySorted(mars::BufferEntryType(4, data, + // position_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + + // // test return measurements size1 + // std::vector entries_return; + // buffer.get_sensor_handle_measurements(pose_sensor_1_sptr, &entries_return); + + // ASSERT_EQ(entries_return.size(), 2); + + // // iterate over buffer and test elements1 + // int ts = 5; + // for (const auto& it : entries_return) + // { + // mars::PoseMeasurementType meas = *static_cast(it->data_.sensor_state_.get()); + // std::cout << "pose1_meas: " << meas.position_.transpose() << std::endl; + // // value + // ASSERT_EQ((meas.position_ - Eigen::Vector3d(1, 1, 1)).norm(), 0); + // // timestamp + // ASSERT_EQ(it->timestamp_, ts); + // ts += 3; + // // check if pointer is corresponding to correct one + // ASSERT_EQ(it->data_.sensor_state_, meas_pose1_ptr); + // } + + // // test return measurements size2 + // buffer.get_sensor_handle_measurements(pose_sensor_2_sptr, &entries_return); + + // ASSERT_EQ(entries_return.size(), 3); + + // // iterate over buffer and test elements2 + // ts = 1; + // for (const auto& it : entries_return) + // { + // mars::PoseMeasurementType meas = *static_cast(it->data_.sensor_state_.get()); + // std::cout << "pose2_meas: " << meas.position_.transpose() << std::endl; + // ASSERT_EQ((meas.position_ - Eigen::Vector3d(5, 4, 3)).norm(), 0); + // // timestamp + // ASSERT_EQ(it->timestamp_, ts); + // ts += 4; + // // check if pointer is corresponding to correct one + // ASSERT_EQ(it->data_.sensor_state_, meas_pose2_ptr); + // } + + // // change value one item, retreive values again, should still be the same + // mars::PoseMeasurementType meas = + // *static_cast(entries_return.at(1)->data_.sensor_state_.get()); + // meas.position_ = Eigen::Vector3d(1, 1, 1); + // ASSERT_EQ((meas.position_ - Eigen::Vector3d(1, 1, 1)).norm(), 0); + + // // get all entries again and rerun above test case + // buffer.get_sensor_handle_measurements(pose_sensor_2_sptr, &entries_return); + // ASSERT_EQ(entries_return.size(), 3); + // ts = 1; + // for (const auto& it : entries_return) + // { + // mars::PoseMeasurementType meas = *static_cast(it->data_.sensor_state_.get()); + // std::cout << "pose2_meas: " << meas.position_.transpose() << std::endl; + // ASSERT_EQ((meas.position_ - Eigen::Vector3d(5, 4, 3)).norm(), 0); + // ASSERT_EQ(it->timestamp_, ts); + // ts += 4; + // } + // // if this succeeds then the entries are unchangable in the buffer (as they should be)! + + // // test return measurements + // buffer.get_sensor_handle_measurements(position_sensor_1_sptr, &entries_return); + + // ASSERT_EQ(entries_return.size(), 0); + // ASSERT_TRUE(entries_return.empty()); + + // // test return success status + // ASSERT_EQ(buffer.get_sensor_handle_measurements(pose_sensor_1_sptr, &entries_return), 1); + // ASSERT_EQ(buffer.get_sensor_handle_measurements(pose_sensor_2_sptr, &entries_return), 1); + // ASSERT_EQ(buffer.get_sensor_handle_measurements(position_sensor_1_sptr, &entries_return), 0); + // ASSERT_EQ(buffer.get_sensor_handle_measurements(imu_sensor_sptr, &entries_return), 0); } /// @@ -857,48 +1067,48 @@ TEST_F(mars_buffer_test, GET_SENSOR_MEASUREMENTS) /// TEST_F(mars_buffer_test, ADD_AUTOREMOVE_ENTRIES) { - // Only delete measurement and state pairs + // // Only delete measurement and state pairs - const int max_buffer_size = 5; - mars::Buffer buffer(max_buffer_size); - buffer.set_keep_last_sensor_handle(true); + // const int max_buffer_size = 5; + // mars::Buffer buffer(max_buffer_size); + // buffer.set_keep_last_sensor_handle(true); - std::shared_ptr core_states_sptr = std::make_shared(); - std::shared_ptr pose_sensor_1_sptr = - std::make_shared("Pose_1", core_states_sptr); - std::shared_ptr pose_sensor_2_sptr = - std::make_shared("Pose_2", core_states_sptr); + // std::shared_ptr core_states_sptr = std::make_shared(); + // std::shared_ptr pose_sensor_1_sptr = + // std::make_shared("Pose_1", core_states_sptr); + // std::shared_ptr pose_sensor_2_sptr = + // std::make_shared("Pose_2", core_states_sptr); - int core_dummy = 13; - int sensor_dummy = 15; - mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + // int core_dummy = 13; + // int sensor_dummy = 15; + // mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); - buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + // buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + // buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); + // buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - // Measurement only, since we only allow to delete pairs, this results in no entry removal - buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // // Measurement only, since we only allow to delete pairs, this results in no entry removal + // buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - ASSERT_EQ(buffer.get_length(), 7); - mars::BufferEntryType oldest_entry_return; - buffer.get_oldest_state(&oldest_entry_return); - ASSERT_EQ(oldest_entry_return.timestamp_, 1); + // ASSERT_EQ(buffer.get_length(), 7); + // mars::BufferEntryType oldest_entry_return; + // buffer.get_oldest_state(&oldest_entry_return); + // ASSERT_EQ(oldest_entry_return.timestamp_, 1); - buffer.PrintBufferEntries(); + // buffer.PrintBufferEntries(); - // Add state for the corresponding message - // Now the oldest sensor handle is not the last one anymore and is deleted - buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + // // Add state for the corresponding message + // // Now the oldest sensor handle is not the last one anymore and is deleted + // buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - ASSERT_EQ(buffer.get_length(), 6); - buffer.get_oldest_state(&oldest_entry_return); - ASSERT_EQ(oldest_entry_return.timestamp_, 3); + // ASSERT_EQ(buffer.get_length(), 6); + // buffer.get_oldest_state(&oldest_entry_return); + // ASSERT_EQ(oldest_entry_return.timestamp_, 3); - buffer.PrintBufferEntries(); + // buffer.PrintBufferEntries(); } /// @@ -911,80 +1121,80 @@ TEST_F(mars_buffer_test, ADD_AUTOREMOVE_ENTRIES) /// TEST_F(mars_buffer_test, ADD_AUTOREMOVE_ENTRIES_W_OOO) { - const int max_buffer_size = 8; - mars::Buffer buffer(max_buffer_size); - buffer.set_keep_last_sensor_handle(true); - - std::shared_ptr core_states_sptr = std::make_shared(); - std::shared_ptr pose_sensor_1_sptr = - std::make_shared("Pose_1", core_states_sptr); - std::shared_ptr pose_sensor_2_sptr = - std::make_shared("Pose_2", core_states_sptr); - - int core_dummy = 13; - int sensor_dummy = 15; - mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); - - buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - - buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); - - buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); - - buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - - // This should trigger an overflow removal and out of order integration - buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); - - std::vector buffer_entry_data; - - for (int k = 0; k < 9; k++) - { - mars::BufferEntryType buffer_entry_data_tmp; - buffer.get_entry_at_idx(k, &buffer_entry_data_tmp); - buffer_entry_data.push_back(buffer_entry_data_tmp); - } - - std::vector result_vec; - result_vec.push_back(buffer_entry_data[0].sensor_ == pose_sensor_1_sptr); - result_vec.push_back(buffer_entry_data[0].metadata_ == mars::BufferMetadataType::measurement); - result_vec.push_back(buffer_entry_data[1].sensor_ == pose_sensor_1_sptr); - result_vec.push_back(buffer_entry_data[1].metadata_ == mars::BufferMetadataType::sensor_state); - - result_vec.push_back(buffer_entry_data[2].sensor_ == pose_sensor_2_sptr); - result_vec.push_back(buffer_entry_data[2].metadata_ == mars::BufferMetadataType::measurement); - result_vec.push_back(buffer_entry_data[3].sensor_ == pose_sensor_2_sptr); - result_vec.push_back(buffer_entry_data[3].metadata_ == mars::BufferMetadataType::sensor_state); - - result_vec.push_back(buffer_entry_data[4].sensor_ == pose_sensor_2_sptr); - result_vec.push_back(buffer_entry_data[4].metadata_ == mars::BufferMetadataType::measurement); - result_vec.push_back(buffer_entry_data[5].sensor_ == pose_sensor_2_sptr); - result_vec.push_back(buffer_entry_data[5].metadata_ == mars::BufferMetadataType::sensor_state); - - result_vec.push_back(buffer_entry_data[6].sensor_ == pose_sensor_2_sptr); - result_vec.push_back(buffer_entry_data[6].metadata_ == mars::BufferMetadataType::measurement); - - result_vec.push_back(buffer_entry_data[7].sensor_ == pose_sensor_1_sptr); - result_vec.push_back(buffer_entry_data[7].metadata_ == mars::BufferMetadataType::measurement); - result_vec.push_back(buffer_entry_data[8].sensor_ == pose_sensor_1_sptr); - result_vec.push_back(buffer_entry_data[8].metadata_ == mars::BufferMetadataType::sensor_state); - - bool final_result = true; - for (auto k : result_vec) - { - if (!k) - { - final_result = false; - buffer.PrintBufferEntries(); - break; - } - } - - EXPECT_TRUE(final_result); + // const int max_buffer_size = 8; + // mars::Buffer buffer(max_buffer_size); + // buffer.set_keep_last_sensor_handle(true); + + // std::shared_ptr core_states_sptr = std::make_shared(); + // std::shared_ptr pose_sensor_1_sptr = + // std::make_shared("Pose_1", core_states_sptr); + // std::shared_ptr pose_sensor_2_sptr = + // std::make_shared("Pose_2", core_states_sptr); + + // int core_dummy = 13; + // int sensor_dummy = 15; + // mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + + // buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + + // buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); + + // buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); + + // buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + + // // This should trigger an overflow removal and out of order integration + // buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); + + // std::vector buffer_entry_data; + + // for (int k = 0; k < 9; k++) + // { + // mars::BufferEntryType buffer_entry_data_tmp; + // buffer.get_entry_at_idx(k, &buffer_entry_data_tmp); + // buffer_entry_data.push_back(buffer_entry_data_tmp); + // } + + // std::vector result_vec; + // result_vec.push_back(buffer_entry_data[0].sensor_handle_ == pose_sensor_1_sptr); + // result_vec.push_back(buffer_entry_data[0].metadata_ == mars::BufferMetadataType::measurement); + // result_vec.push_back(buffer_entry_data[1].sensor_handle_ == pose_sensor_1_sptr); + // result_vec.push_back(buffer_entry_data[1].metadata_ == mars::BufferMetadataType::sensor_state); + + // result_vec.push_back(buffer_entry_data[2].sensor_handle_ == pose_sensor_2_sptr); + // result_vec.push_back(buffer_entry_data[2].metadata_ == mars::BufferMetadataType::measurement); + // result_vec.push_back(buffer_entry_data[3].sensor_handle_ == pose_sensor_2_sptr); + // result_vec.push_back(buffer_entry_data[3].metadata_ == mars::BufferMetadataType::sensor_state); + + // result_vec.push_back(buffer_entry_data[4].sensor_handle_ == pose_sensor_2_sptr); + // result_vec.push_back(buffer_entry_data[4].metadata_ == mars::BufferMetadataType::measurement); + // result_vec.push_back(buffer_entry_data[5].sensor_handle_ == pose_sensor_2_sptr); + // result_vec.push_back(buffer_entry_data[5].metadata_ == mars::BufferMetadataType::sensor_state); + + // result_vec.push_back(buffer_entry_data[6].sensor_handle_ == pose_sensor_2_sptr); + // result_vec.push_back(buffer_entry_data[6].metadata_ == mars::BufferMetadataType::measurement); + + // result_vec.push_back(buffer_entry_data[7].sensor_handle_ == pose_sensor_1_sptr); + // result_vec.push_back(buffer_entry_data[7].metadata_ == mars::BufferMetadataType::measurement); + // result_vec.push_back(buffer_entry_data[8].sensor_handle_ == pose_sensor_1_sptr); + // result_vec.push_back(buffer_entry_data[8].metadata_ == mars::BufferMetadataType::sensor_state); + + // bool final_result = true; + // for (auto k : result_vec) + // { + // if (!k) + // { + // final_result = false; + // buffer.PrintBufferEntries(); + // break; + // } + // } + + // EXPECT_TRUE(final_result); } /// @@ -994,39 +1204,40 @@ TEST_F(mars_buffer_test, ADD_AUTOREMOVE_ENTRIES_W_OOO) /// TEST_F(mars_buffer_test, ADD_INDEX_TEST) { - const int max_buffer_size = 5; - mars::Buffer buffer(max_buffer_size); - buffer.set_keep_last_sensor_handle(true); - - std::shared_ptr core_states_sptr = std::make_shared(); - std::shared_ptr pose_sensor_1_sptr = - std::make_shared("Pose_1", core_states_sptr); - std::shared_ptr pose_sensor_2_sptr = - std::make_shared("Pose_2", core_states_sptr); - std::shared_ptr pose_sensor_3_sptr = - std::make_shared("Pose_3", core_states_sptr); - - int core_dummy = 13; - int sensor_dummy = 15; - mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); - - buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); - - int idx = - buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_3_sptr, mars::BufferMetadataType::measurement)); - - ASSERT_EQ(buffer.get_length(), 7); - - mars::BufferEntryType entry_01; - buffer.get_entry_at_idx(1, &entry_01); - - ASSERT_EQ(entry_01.timestamp_, 0); - ASSERT_EQ(idx, 2); + // const int max_buffer_size = 5; + // mars::Buffer buffer(max_buffer_size); + // buffer.set_keep_last_sensor_handle(true); + + // std::shared_ptr core_states_sptr = std::make_shared(); + // std::shared_ptr pose_sensor_1_sptr = + // std::make_shared("Pose_1", core_states_sptr); + // std::shared_ptr pose_sensor_2_sptr = + // std::make_shared("Pose_2", core_states_sptr); + // std::shared_ptr pose_sensor_3_sptr = + // std::make_shared("Pose_3", core_states_sptr); + + // int core_dummy = 13; + // int sensor_dummy = 15; + // mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + + // buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + // buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); + // buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_2_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); + + // int idx = + // buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_3_sptr, + // mars::BufferMetadataType::measurement)); + + // ASSERT_EQ(buffer.get_length(), 7); + + // mars::BufferEntryType entry_01; + // buffer.get_entry_at_idx(1, &entry_01); + + // ASSERT_EQ(entry_01.timestamp_, 0); + // ASSERT_EQ(idx, 2); } /// @@ -1039,135 +1250,134 @@ TEST_F(mars_buffer_test, ADD_INDEX_TEST) /// TEST_F(mars_buffer_test, SIZE_TEST) { - const int max_buffer_size = 2; - mars::Buffer buffer(max_buffer_size); - buffer.set_keep_last_sensor_handle(true); + // const int max_buffer_size = 2; + // mars::Buffer buffer(max_buffer_size); + // buffer.set_keep_last_sensor_handle(true); - std::shared_ptr core_states_sptr = std::make_shared(); - std::shared_ptr pose_sensor_1_sptr = - std::make_shared("Pose_1", core_states_sptr); - std::shared_ptr pose_sensor_2_sptr = - std::make_shared("Pose_2", core_states_sptr); - std::shared_ptr pose_sensor_3_sptr = - std::make_shared("Pose_3", core_states_sptr); + // std::shared_ptr core_states_sptr = std::make_shared(); + // std::shared_ptr pose_sensor_1_sptr = + // std::make_shared("Pose_1", core_states_sptr); + // std::shared_ptr pose_sensor_2_sptr = + // std::make_shared("Pose_2", core_states_sptr); + // std::shared_ptr pose_sensor_3_sptr = + // std::make_shared("Pose_3", core_states_sptr); - int core_dummy = 13; - int sensor_dummy = 15; - mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + // int core_dummy = 13; + // int sensor_dummy = 15; + // mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); - buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); + // buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + // buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_3_sptr, mars::BufferMetadataType::sensor_state)); + // buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_3_sptr, mars::BufferMetadataType::sensor_state)); - ASSERT_EQ(buffer.get_length(), 3); - buffer.PrintBufferEntries(); + // ASSERT_EQ(buffer.get_length(), 3); + // buffer.PrintBufferEntries(); - // Check that last state entry is still pose sensor 2 - mars::BufferEntryType last_state; - buffer.get_oldest_state(&last_state); + // // Check that last state entry is still pose sensor 2 + // mars::BufferEntryType last_state; + // buffer.get_oldest_state(&last_state); - ASSERT_EQ(pose_sensor_1_sptr, last_state.sensor_); + // ASSERT_EQ(pose_sensor_1_sptr, last_state.sensor_handle_); } TEST_F(mars_buffer_test, BUFFER_INSERT_INTERMEDIATE_DATA) { - const int max_buffer_size = 10; - mars::Buffer buffer(max_buffer_size); - buffer.set_keep_last_sensor_handle(true); - - std::shared_ptr core_states_sptr = std::make_shared(); - std::shared_ptr pose_sensor_1_sptr = - std::make_shared("Pose_1", core_states_sptr); - std::shared_ptr imu_sensor_1_sptr = - std::make_shared("imu"); - - int core_dummy = 13; - int sensor_dummy = 15; - mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); - - buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - - mars::BufferEntryType meas(4, data, imu_sensor_1_sptr, mars::BufferMetadataType::measurement); - mars::BufferEntryType state(4, data, imu_sensor_1_sptr, mars::BufferMetadataType::core_state); - - EXPECT_FALSE(buffer.InsertIntermediateData(state, state)); - // Confirm no change in buffersize - EXPECT_TRUE(buffer.get_length() == 5); - EXPECT_FALSE(buffer.InsertIntermediateData(meas, meas)); - // Confirm no change in buffersize - EXPECT_TRUE(buffer.get_length() == 5); - - buffer.PrintBufferEntries(); - - EXPECT_TRUE(buffer.InsertIntermediateData(meas, state)); - EXPECT_TRUE(buffer.get_length() == 7); - - buffer.PrintBufferEntries(); - - mars::BufferEntryType entry_04; - buffer.get_entry_at_idx(4, &entry_04); - mars::BufferEntryType entry_05; - buffer.get_entry_at_idx(5, &entry_05); - - EXPECT_TRUE(entry_04.sensor_ == imu_sensor_1_sptr); - EXPECT_EQ(entry_04.timestamp_, 4); - EXPECT_TRUE(entry_04.metadata_ == mars::BufferMetadataType::measurement_auto); - - EXPECT_TRUE(entry_05.sensor_ == imu_sensor_1_sptr); - EXPECT_EQ(entry_05.timestamp_, 4); - EXPECT_TRUE(entry_05.metadata_ == mars::BufferMetadataType::core_state_auto); + // const int max_buffer_size = 10; + // mars::Buffer buffer(max_buffer_size); + // buffer.set_keep_last_sensor_handle(true); + + // std::shared_ptr core_states_sptr = std::make_shared(); + // std::shared_ptr pose_sensor_1_sptr = + // std::make_shared("Pose_1", core_states_sptr); + // std::shared_ptr imu_sensor_1_sptr = std::make_shared("imu"); + + // int core_dummy = 13; + // int sensor_dummy = 15; + // mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + + // buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + // buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + // buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + + // mars::BufferEntryType meas(4, data, imu_sensor_1_sptr, mars::BufferMetadataType::measurement); + // mars::BufferEntryType state(4, data, imu_sensor_1_sptr, mars::BufferMetadataType::core_state); + + // EXPECT_FALSE(buffer.InsertIntermediateData(state, state)); + // // Confirm no change in buffersize + // EXPECT_TRUE(buffer.get_length() == 5); + // EXPECT_FALSE(buffer.InsertIntermediateData(meas, meas)); + // // Confirm no change in buffersize + // EXPECT_TRUE(buffer.get_length() == 5); + + // buffer.PrintBufferEntries(); + + // EXPECT_TRUE(buffer.InsertIntermediateData(meas, state)); + // EXPECT_TRUE(buffer.get_length() == 7); + + // buffer.PrintBufferEntries(); + + // mars::BufferEntryType entry_04; + // buffer.get_entry_at_idx(4, &entry_04); + // mars::BufferEntryType entry_05; + // buffer.get_entry_at_idx(5, &entry_05); + + // EXPECT_TRUE(entry_04.sensor_handle_ == imu_sensor_1_sptr); + // EXPECT_EQ(entry_04.timestamp_, 4); + // EXPECT_TRUE(entry_04.metadata_ == mars::BufferMetadataType::measurement_auto); + + // EXPECT_TRUE(entry_05.sensor_handle_ == imu_sensor_1_sptr); + // EXPECT_EQ(entry_05.timestamp_, 4); + // EXPECT_TRUE(entry_05.metadata_ == mars::BufferMetadataType::core_state_auto); } TEST_F(mars_buffer_test, BUFFER_GET_INTERMEDIATE_ENTRY_PAIR) { - const int max_buffer_size = 10; - mars::Buffer buffer(max_buffer_size); - buffer.set_keep_last_sensor_handle(true); + // const int max_buffer_size = 10; + // mars::Buffer buffer(max_buffer_size); + // buffer.set_keep_last_sensor_handle(true); - std::shared_ptr core_states_sptr = std::make_shared(); - std::shared_ptr pose_sensor_1_sptr = - std::make_shared("Pose_1", core_states_sptr); - std::shared_ptr imu_sensor_1_sptr = std::make_shared("imu"); + // std::shared_ptr core_states_sptr = std::make_shared(); + // std::shared_ptr pose_sensor_1_sptr = + // std::make_shared("Pose_1", core_states_sptr); + // std::shared_ptr imu_sensor_1_sptr = std::make_shared("imu"); - int core_dummy = 13; - int sensor_dummy = 15; - mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); + // int core_dummy = 13; + // int sensor_dummy = 15; + // mars::BufferDataType data(std::make_shared(core_dummy), std::make_shared(sensor_dummy)); - buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(3, data, imu_sensor_1_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(3, data, imu_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + // buffer.AddEntrySorted(mars::BufferEntryType(3, data, imu_sensor_1_sptr, mars::BufferMetadataType::measurement)); + // buffer.AddEntrySorted(mars::BufferEntryType(3, data, imu_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + // buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement)); - mars::BufferEntryType state_tmp; - EXPECT_FALSE(buffer.get_intermediate_entry_pair(pose_sensor_1_sptr, &state_tmp, &state_tmp)); + // mars::BufferEntryType state_tmp; + // EXPECT_FALSE(buffer.get_intermediate_entry_pair(pose_sensor_1_sptr, &state_tmp, &state_tmp)); - mars::BufferEntryType meas(5, data, imu_sensor_1_sptr, mars::BufferMetadataType::measurement); - mars::BufferEntryType state(5, data, imu_sensor_1_sptr, mars::BufferMetadataType::core_state); + // mars::BufferEntryType meas(5, data, imu_sensor_1_sptr, mars::BufferMetadataType::measurement); + // mars::BufferEntryType state(5, data, imu_sensor_1_sptr, mars::BufferMetadataType::core_state); - buffer.InsertIntermediateData(meas, state); + // buffer.InsertIntermediateData(meas, state); - // Pretend the Sensor update was finished and a state was added - buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); + // // Pretend the Sensor update was finished and a state was added + // buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state)); - mars::BufferEntryType pose_state_at_5; - mars::BufferEntryType imu_state_at_5; + // mars::BufferEntryType pose_state_at_5; + // mars::BufferEntryType imu_state_at_5; - EXPECT_TRUE(buffer.get_intermediate_entry_pair(pose_sensor_1_sptr, &imu_state_at_5, &pose_state_at_5)); + // EXPECT_TRUE(buffer.get_intermediate_entry_pair(pose_sensor_1_sptr, &imu_state_at_5, &pose_state_at_5)); - EXPECT_TRUE(pose_state_at_5.sensor_ == pose_sensor_1_sptr); - EXPECT_TRUE(imu_state_at_5.sensor_ == imu_sensor_1_sptr); + // EXPECT_TRUE(pose_state_at_5.sensor_handle_ == pose_sensor_1_sptr); + // EXPECT_TRUE(imu_state_at_5.sensor_handle_ == imu_sensor_1_sptr); - EXPECT_TRUE(pose_state_at_5.IsState()); - EXPECT_TRUE(imu_state_at_5.IsState()); + // EXPECT_TRUE(pose_state_at_5.IsState()); + // EXPECT_TRUE(imu_state_at_5.IsState()); - EXPECT_EQ(pose_state_at_5.timestamp_, 5); - EXPECT_EQ(imu_state_at_5.timestamp_, 5); + // EXPECT_EQ(pose_state_at_5.timestamp_, 5); + // EXPECT_EQ(imu_state_at_5.timestamp_, 5); } TEST_F(mars_buffer_test, INSERT_DATA_AT_IDX) diff --git a/source/tests/mars-test/mars_buffer_type.cpp b/source/tests/mars-test/mars_buffer_type.cpp index 955a5db..ee282c1 100644 --- a/source/tests/mars-test/mars_buffer_type.cpp +++ b/source/tests/mars-test/mars_buffer_type.cpp @@ -69,12 +69,44 @@ TEST_F(mars_buffer_type_test, DATA_STORAGE) // the unique pointer ensures deletion when leaving the scope { std::unique_ptr data(new mars::BufferDataType); - data->set_sensor_data(std::make_shared(imu_meas)); - buffer_entry = mars::BufferEntryType(1, *data, imu_sensor_sptr, mars::BufferMetadataType::measurement); + data->set_sensor_state(std::make_shared(imu_meas)); + buffer_entry = mars::BufferEntryType(1, *data, imu_sensor_sptr); } - mars::IMUMeasurementType imu_meas_return = *static_cast(buffer_entry.data_.sensor_.get()); + mars::IMUMeasurementType imu_meas_return = + *static_cast(buffer_entry.data_.sensor_state_.get()); ASSERT_EQ(imu_meas, imu_meas_return); - ASSERT_EQ(imu_sensor_sptr.get(), buffer_entry.sensor_.get()); + ASSERT_EQ(imu_sensor_sptr.get(), buffer_entry.sensor_handle_.get()); +} + +TEST_F(mars_buffer_type_test, SET_RESET_STATE_ELEMENT) +{ + // Plane instance + mars::BufferDataType data_empty; + ASSERT_FALSE(data_empty.HasCoreStates()); + ASSERT_FALSE(data_empty.HasSensorStates()); + ASSERT_FALSE(data_empty.HasStates()); + + // Instance init with data + mars::BufferDataType data_w_state(std::make_shared(12), std::make_shared(13)); + + ASSERT_TRUE(data_w_state.HasCoreStates()); + ASSERT_TRUE(data_w_state.HasSensorStates()); + + data_w_state.ClearStates(); + ASSERT_FALSE(data_w_state.HasCoreStates()); + ASSERT_FALSE(data_w_state.HasSensorStates()); + + // Only set core state + data_w_state.set_core_state(std::make_shared(12)); + + ASSERT_TRUE(data_w_state.HasCoreStates()); + ASSERT_FALSE(data_w_state.HasSensorStates()); + + // Set sensor state separately + data_w_state.set_sensor_state(std::make_shared(12)); + + ASSERT_TRUE(data_w_state.HasCoreStates()); + ASSERT_TRUE(data_w_state.HasSensorStates()); } diff --git a/source/tests/mars-test/mars_core_logic.cpp b/source/tests/mars-test/mars_core_logic.cpp index 502f160..6399192 100644 --- a/source/tests/mars-test/mars_core_logic.cpp +++ b/source/tests/mars-test/mars_core_logic.cpp @@ -55,18 +55,18 @@ TEST_F(mars_core_logic_test, INITIALIZE) // Call on empty buffer ASSERT_FALSE(core_logic.Initialize(p_wi_init, q_wi_init)); + // Add measurement to pre-buffer int timestamp = 0; mars::IMUMeasurementType imu_meas(Eigen::Vector3d(1, 2, 3), Eigen::Vector3d(4, 5, 6)); mars::BufferDataType data; - data.set_sensor_data(std::make_shared(imu_meas)); + data.set_measurement(std::make_shared(imu_meas)); - mars::BufferEntryType buffer_entry(timestamp, data, imu_sensor_sptr, mars::BufferMetadataType::measurement); + mars::BufferEntryType buffer_entry(timestamp, data, imu_sensor_sptr); core_logic.buffer_prior_core_init_.AddEntrySorted(buffer_entry); // Call on non-empty buffer - ASSERT_TRUE(core_logic.Initialize(p_wi_init, q_wi_init)); } @@ -93,9 +93,9 @@ TEST_F(mars_core_logic_test, GENERATE_STATE_TRANSITION_BLOCK) double timestamp = k; // Generate buffer entrys mars::BufferDataType data; - data.set_core_data(std::make_shared(state_transition_test_data[k])); + data.set_core_state(std::make_shared(state_transition_test_data[k])); - mars::BufferEntryType buffer_entry(timestamp, data, imu_sensor_sptr, mars::BufferMetadataType::core_state); + mars::BufferEntryType buffer_entry(timestamp, data, imu_sensor_sptr); core_logic.buffer_.AddEntrySorted(buffer_entry); } @@ -142,15 +142,14 @@ TEST_F(mars_core_logic_test, PERFORM_SENSOR_UPDATE) std::shared_ptr sensor_sptr = std::make_shared("Pose", core_states_sptr); - // Prepare Dummy data - int sensor_dummy = 15; - mars::BufferDataType data(std::make_shared(), std::make_shared(sensor_dummy)); - std::shared_ptr data_sptr = std::make_shared(data); + // Prepare Dummy entry + mars::BufferEntryType new_buffer_entry; + new_buffer_entry.data_.set_states(std::make_shared(), std::make_shared(13)); + new_buffer_entry.data_.set_measurement(std::make_shared(13)); // Call sensor update with prepared objects int sensor_update_status; - mars::BufferEntryType new_buffer_entry; - sensor_update_status = core_logic.PerformSensorUpdate(&new_buffer_entry, sensor_sptr, 10, data_sptr); + sensor_update_status = core_logic.PerformSensorUpdate(sensor_sptr, 10, &new_buffer_entry); ASSERT_EQ(sensor_update_status, 0); } diff --git a/source/tests/mars-test/mars_gps_utils.cpp b/source/tests/mars-test/mars_gps_utils.cpp index f913a89..905ddc6 100644 --- a/source/tests/mars-test/mars_gps_utils.cpp +++ b/source/tests/mars-test/mars_gps_utils.cpp @@ -59,13 +59,13 @@ TEST_F(mars_gps_utils_test, AVG_CALC) mars::GpsMeasurementType meas_gps2(-5.0, 4.0, 3.0); mars::GpsMeasurementType meas_gps3(3.0, 5.5, 3.0); mars::BufferDataType data_gps1, data_gps2, data_gps3; - data_gps1.set_sensor_data(std::make_shared(meas_gps1)); - data_gps2.set_sensor_data(std::make_shared(meas_gps2)); - data_gps3.set_sensor_data(std::make_shared(meas_gps3)); + data_gps1.set_measurement(std::make_shared(meas_gps1)); + data_gps2.set_measurement(std::make_shared(meas_gps2)); + data_gps3.set_measurement(std::make_shared(meas_gps3)); // add measurements to buffer - buffer.AddEntrySorted(mars::BufferEntryType(1, data_dummy, pose1_sensor_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(2, data_dummy, pose1_sensor_sptr, mars::BufferMetadataType::measurement)); + buffer.AddEntrySorted(mars::BufferEntryType(1, data_dummy, pose1_sensor_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(2, data_dummy, pose1_sensor_sptr)); // perform update without measurements in buffer mars::GpsCoordinates gps_mean = gps_init.get_gps_mean(gps1_sensor_sptr, buffer, meas_gps1.coordinates_, 1); @@ -78,9 +78,9 @@ TEST_F(mars_gps_utils_test, AVG_CALC) gps_init.Reset(); // continue to fill buffer - buffer.AddEntrySorted(mars::BufferEntryType(1, data_gps1, gps1_sensor_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(3, data_gps2, gps1_sensor_sptr, mars::BufferMetadataType::measurement)); - buffer.AddEntrySorted(mars::BufferEntryType(4, data_dummy, pose1_sensor_sptr, mars::BufferMetadataType::measurement)); + buffer.AddEntrySorted(mars::BufferEntryType(1, data_gps1, gps1_sensor_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(3, data_gps2, gps1_sensor_sptr)); + buffer.AddEntrySorted(mars::BufferEntryType(4, data_dummy, pose1_sensor_sptr)); // test mean again gps_mean = gps_init.get_gps_mean(gps1_sensor_sptr, buffer, meas_gps3.coordinates_, 5); From 578d9b15385bfffd2bf544dc0129bd7130d0be7f Mon Sep 17 00:00:00 2001 From: Christian Brommer Date: Wed, 29 May 2024 11:44:18 +0200 Subject: [PATCH 3/5] add: performance test case --- source/tests/mars-e2e-test/CMakeLists.txt | 1 + .../mars_e2e_imu_pose_update_perf.cpp | 224 ++++++++++++++++++ 2 files changed, 225 insertions(+) create mode 100644 source/tests/mars-e2e-test/mars_e2e_imu_pose_update_perf.cpp diff --git a/source/tests/mars-e2e-test/CMakeLists.txt b/source/tests/mars-e2e-test/CMakeLists.txt index 3a46b29..ae5436d 100644 --- a/source/tests/mars-e2e-test/CMakeLists.txt +++ b/source/tests/mars-e2e-test/CMakeLists.txt @@ -23,6 +23,7 @@ set(sources main.cpp mars_e2e_imu_prop.cpp mars_e2e_imu_pose_update.cpp + mars_e2e_imu_pose_update_perf.cpp mars_e2e_imu_prop_empty_updates.cpp ) diff --git a/source/tests/mars-e2e-test/mars_e2e_imu_pose_update_perf.cpp b/source/tests/mars-e2e-test/mars_e2e_imu_pose_update_perf.cpp new file mode 100644 index 0000000..0e5850e --- /dev/null +++ b/source/tests/mars-e2e-test/mars_e2e_imu_pose_update_perf.cpp @@ -0,0 +1,224 @@ +// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria. +// +// All rights reserved. +// +// This software is licensed under the terms of the BSD-2-Clause-License with +// no commercial use allowed, the full terms of which are made available +// in the LICENSE file. No license in patents is granted. +// +// You can contact the author at + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "../include_local/test_data_settings.h" + +/// +/// \brief mars_e2e_imu_pose_update End to end test with imu and pose measurements without noise +/// +class mars_e2e_imu_pose_update : public testing::Test +{ +public: + std::string test_data_path; + YAML::Node config; + std::string traj_file_name; + std::string pose_file_name; + + std::shared_ptr imu_sensor_sptr; + std::shared_ptr core_states_sptr; + mars::CoreLogic core_logic; + + std::shared_ptr pose_sensor_sptr; + + std::vector measurement_data; + + mars_e2e_imu_pose_update() + { + LoadInstancesAndParams(); + + measurement_data = LoadData(); + } + + void LoadInstancesAndParams() + { + test_data_path = std::string(MARS_LIB_TEST_DATA_PATH); + + // get config + config = YAML::LoadFile(test_data_path + "parameter.yaml"); + + traj_file_name = config["traj_file_name"].as(); + std::cout << "Trajectory File: " << traj_file_name << std::endl; + + pose_file_name = config["pose_file_name"].as(); + std::cout << "Pose File: " << pose_file_name << std::endl; + + std::vector imu_n_w; + std::vector imu_n_bw; + std::vector imu_n_a; + std::vector imu_n_ba; + + std::cout << "IMU Noise Parameter: " << std::endl; + read_yaml_vec_3(&imu_n_w, "imu_n_w", config); + read_yaml_vec_3(&imu_n_bw, "imu_n_bw", config); + read_yaml_vec_3(&imu_n_a, "imu_n_a", config); + read_yaml_vec_3(&imu_n_ba, "imu_n_ba", config); + + // setup propagation sensor + imu_sensor_sptr = std::make_shared("IMU"); + + // setup the core definition + core_states_sptr = std::make_shared(); + core_states_sptr.get()->set_propagation_sensor(imu_sensor_sptr); + core_states_sptr.get()->set_noise_std(Eigen::Vector3d(imu_n_w.data()), Eigen::Vector3d(imu_n_bw.data()), + Eigen::Vector3d(imu_n_a.data()), Eigen::Vector3d(imu_n_ba.data())); + + core_logic = mars::CoreLogic(core_states_sptr); + + // setup additional sensors + // Pose sensor + pose_sensor_sptr = std::make_shared("Pose", core_states_sptr); + pose_sensor_sptr->const_ref_to_nav_ = + true; // TODO is set here for now but will be managed by core logic in later versions + + // Define measurement noise + Eigen::Matrix pose_meas_std; + pose_meas_std << 0.02, 0.02, 0.02, 2 * (M_PI / 180), 2 * (M_PI / 180), 2 * (M_PI / 180); + pose_sensor_sptr->R_ = pose_meas_std.cwiseProduct(pose_meas_std); + + // Define initial calibration and covariance + mars::PoseSensorData pose_init_cal; + pose_init_cal.state_.p_ip_ = Eigen::Vector3d::Zero(); + pose_init_cal.state_.q_ip_ = Eigen::Quaterniond::Identity(); + + // The covariance should enclose the initialization with a 3 Sigma bound + Eigen::Matrix std; + std << 0.1, 0.1, 0.1, (10 * M_PI / 180), (10 * M_PI / 180), (10 * M_PI / 180); + pose_init_cal.sensor_cov_ = std.cwiseProduct(std).asDiagonal(); + + pose_sensor_sptr->set_initial_calib(std::make_shared(pose_init_cal)); + } + + std::vector LoadData() + { + std::vector measurement_data; + + std::vector measurement_data_imu; + mars::ReadSimData(&measurement_data_imu, imu_sensor_sptr, test_data_path + traj_file_name); + + std::vector measurement_data_pose; + mars::ReadPoseData(&measurement_data_pose, pose_sensor_sptr, test_data_path + pose_file_name, 1e-13); + + measurement_data.insert(measurement_data.end(), measurement_data_imu.begin(), measurement_data_imu.end()); + measurement_data.insert(measurement_data.end(), measurement_data_pose.begin(), measurement_data_pose.end()); + + std::sort(measurement_data.begin(), measurement_data.end()); + + return measurement_data; + } + + bool read_yaml_vec_3(std::vector* value, const std::string& parameter, YAML::Node config) + { + if (config[parameter]) + { + *value = config[parameter].as>(); + + std::cout << parameter << ": \t ["; + for (auto const& i : *value) + std::cout << i << " "; + + std::cout << " ]" << std::endl; + return true; + } + return false; + } + + void RunFilter() + { + // process data + for (auto k : measurement_data) + { + core_logic.ProcessMeasurement(k.sensor_handle_, k.timestamp_, k.data_); + + if (!core_logic.core_is_initialized_) + { + // Initialize the first time at which the propagation sensor occures + if (k.sensor_handle_ == core_logic.core_states_->propagation_sensor_) + { + Eigen::Vector3d p_wi_init(0, 0, 5); + Eigen::Quaterniond q_wi_init = Eigen::Quaterniond::Identity(); + core_logic.Initialize(p_wi_init, q_wi_init); + } + else + { + continue; + } + } + } + } + + void Reset() + { + core_logic.core_is_initialized_ = false; + core_logic.buffer_.ResetBufferData(); + pose_sensor_sptr->is_initialized_ = false; + } +}; + +TEST_F(mars_e2e_imu_pose_update, END_2_END_IMU_POSE_UPDATE_PERF) +{ + // generate a single out of order measurement + // std::swap(measurement_data[31],measurement_data[32]); + + core_logic.verbose_ = false; + core_logic.add_interm_buffer_entries_ = true; + + int num_iteration = 5; + double time_accum = 0; + + for (int k = 0; k < num_iteration; k++) + { + Reset(); + double t_start = mars::Time::get_time_now().get_seconds(); + RunFilter(); + double t_end = mars::Time::get_time_now().get_seconds(); + + double t_duration = t_end - t_start; + std::cout << "Single itteration " << k << " time: " << t_duration << std::endl; + + time_accum += t_duration; + } + + std::cout << "Average processing time over " << num_iteration << " iterations:" << time_accum / num_iteration + << std::endl; + + mars::BufferEntryType latest_result; + core_logic.buffer_.get_latest_state(&latest_result); + mars::CoreStateType last_state = static_cast(latest_result.data_.core_state_.get())->state_; + + std::cout << "Last State:" << std::endl; + std::cout << last_state << std::endl; + + // Define final ground truth values + Eigen::Vector3d true_p_wi(-20946.817372738657, -3518.039994126535, 8631.1520460773336); + Eigen::Vector3d true_v_wi(15.924719563070044, -20.483884216740151, 11.455154466026718); + Eigen::Quaterniond true_q_wi(0.98996033625708202, 0.048830414166879263, -0.02917972697860232, -0.12939345742158029); + + std::cout << "p_wi error [m]: [" << (last_state.p_wi_ - true_p_wi).transpose() << " ]" << std::endl; + std::cout << "v_wi error [m/s]: [" << (last_state.v_wi_ - true_v_wi).transpose() << " ]" << std::endl; + + Eigen::Quaterniond q_wi_error(last_state.q_wi_.conjugate() * true_q_wi); + std::cout << "q_wi error [w,x,y,z]: [" << q_wi_error.w() << " " << q_wi_error.vec().transpose() << " ]" << std::endl; + + EXPECT_TRUE(last_state.p_wi_.isApprox(true_p_wi, 1e-5)); + EXPECT_TRUE(last_state.v_wi_.isApprox(true_v_wi, 1e-5)); + EXPECT_TRUE(last_state.q_wi_.coeffs().isApprox(true_q_wi.coeffs(), 1e-5)); +} From 041eaecbb89e64abb61ca8a9380fe335eb60a6ef Mon Sep 17 00:00:00 2001 From: Christian Brommer Date: Mon, 3 Jun 2024 16:48:21 +0200 Subject: [PATCH 4/5] adapted reworking of the buffer for ooo --- source/mars/include/mars/buffer.h | 11 +- source/mars/include/mars/core_logic.h | 3 + source/mars/source/buffer.cpp | 41 ++-- source/mars/source/buffer_entry_type.cpp | 4 +- source/mars/source/core_logic.cpp | 145 ++++++------ source/tests/mars-e2e-test/CMakeLists.txt | 1 + .../mars_e2e_imu_pose_ooo_rework.cpp | 216 ++++++++++++++++++ source/tests/mars-test/mars_buffer.cpp | 13 +- 8 files changed, 324 insertions(+), 110 deletions(-) create mode 100644 source/tests/mars-e2e-test/mars_e2e_imu_pose_ooo_rework.cpp diff --git a/source/mars/include/mars/buffer.h b/source/mars/include/mars/buffer.h index 9c9c1cd..652d1c0 100644 --- a/source/mars/include/mars/buffer.h +++ b/source/mars/include/mars/buffer.h @@ -79,7 +79,10 @@ class Buffer /// \brief get_length /// \return current number of elements stored in the buffer /// - int get_length() const; + inline int get_length() const + { + return static_cast(data_.size()); + } /// /// \brief PrintBufferEntries prints all buffer entries in a formatted way @@ -199,7 +202,7 @@ class Buffer /// \return Index of the added entry. The index is -1 if the entry was removed because the max_buffer_size was /// reached. /// - int AddEntrySorted(const BufferEntryType& new_entry); + int AddEntrySorted(const BufferEntryType& new_entry, const bool& after = true); /// /// \brief FindClosestTimestamp Returns the index of the entry which is the closest to the specified 'timestamp' when @@ -231,14 +234,14 @@ class Buffer /// The method finds the closest timestamp based on the shortest 'time' distance between the new and existing buffer /// elements. It then determines if the entry needs to be added before or after the closest entry. /// - int InsertDataAtTimestamp(const BufferEntryType& new_entry); + int InsertDataAtTimestamp(const BufferEntryType& new_entry, const bool& after = true); /// /// \brief InsertDataAtIndex Adds 'entry' at buffer position 'index' /// \param new_entry Entry buffer entry to be added /// \param index position at which the entry is added /// - bool InsertDataAtIndex(const BufferEntryType& new_entry, const int& index); + bool OverwriteDataAtIndex(const BufferEntryType& new_entry, const int& index); /// /// \brief get_latest_interm_entrie Get last state pair of imu prop and sensor update diff --git a/source/mars/include/mars/core_logic.h b/source/mars/include/mars/core_logic.h index 076c52a..e9d9bfd 100644 --- a/source/mars/include/mars/core_logic.h +++ b/source/mars/include/mars/core_logic.h @@ -73,6 +73,9 @@ class CoreLogic /// \brief PerformSensorUpdate Returns new state with corrected state and updated covariance /// bool PerformSensorUpdate(std::shared_ptr sensor, const Time& timestamp, BufferEntryType* sensor_data); + bool PerformSensorUpdate(std::shared_ptr sensor, const Time& timestamp, BufferEntryType* sensor_data, + bool* added_interm_state); + /// /// \brief PerformCoreStatePropagation Propagates the core state and returns the new state entry /// diff --git a/source/mars/source/buffer.cpp b/source/mars/source/buffer.cpp index f129f6d..09c12df 100644 --- a/source/mars/source/buffer.cpp +++ b/source/mars/source/buffer.cpp @@ -52,11 +52,6 @@ bool Buffer::IsEmpty() const return data_.empty(); } -int Buffer::get_length() const -{ - return static_cast(data_.size()); -} - void Buffer::PrintBufferEntries() const { std::cout << "Idx" @@ -368,15 +363,9 @@ bool Buffer::RemoveSensorFromBuffer(const std::shared_ptr& senso return true; } -int Buffer::AddEntrySorted(const BufferEntryType& new_entry) +int Buffer::AddEntrySorted(const BufferEntryType& new_entry, const bool& after) { - int index = InsertDataAtTimestamp(new_entry); - - int del_idx = RemoveOverflowEntrys(); - if (del_idx > index) - { - index = -1; - } + int index = InsertDataAtTimestamp(new_entry, after); return index; } @@ -428,7 +417,7 @@ bool Buffer::IsSorted() const return std::is_sorted(data_.begin(), data_.end()); } -int Buffer::InsertDataAtTimestamp(const BufferEntryType& new_entry) +int Buffer::InsertDataAtTimestamp(const BufferEntryType& new_entry, const bool& after) { if (this->IsEmpty()) { @@ -458,7 +447,13 @@ int Buffer::InsertDataAtTimestamp(const BufferEntryType& new_entry) if (current_time_distance.get_seconds() >= 0) { - int insert_idx = k + 1; + int insert_idx = k; + + if (after) + { + insert_idx += 1; + } + data_.insert(data_.begin() + insert_idx, new_entry); return insert_idx; // return entry index } @@ -469,17 +464,15 @@ int Buffer::InsertDataAtTimestamp(const BufferEntryType& new_entry) return 0; // push front adds element at index 0 } -bool Buffer::InsertDataAtIndex(const BufferEntryType& new_entry, const int& index) +bool Buffer::OverwriteDataAtIndex(const BufferEntryType& new_entry, const int& index) { - if (this->get_length() - 1 < index) + if (index < (this->get_length())) { - // required index is beyond buffersize, append at the end of the buffer - data_.push_back(new_entry); + data_[index] = new_entry; return true; } - data_.insert(data_.begin() + index, new_entry); - return true; + return false; } bool Buffer::get_intermediate_entry_pair(const std::shared_ptr& sensor_handle, @@ -532,7 +525,11 @@ int Buffer::RemoveOverflowEntrys() else { it = data_.erase(it); - break; + + if (this->get_length() <= this->max_buffer_size_) + { + break; + } } } diff --git a/source/mars/source/buffer_entry_type.cpp b/source/mars/source/buffer_entry_type.cpp index fab8a85..afcb60a 100644 --- a/source/mars/source/buffer_entry_type.cpp +++ b/source/mars/source/buffer_entry_type.cpp @@ -71,7 +71,7 @@ std::ostream& operator<<(std::ostream& out, const BufferEntryType& entry) out << BufferEntryType::get_metadata_label(entry.metadata_) << '\t'; out << "Core"; - if (entry.HasStates()) + if (entry.data_.HasCoreStates()) { out << "[x]"; } @@ -81,7 +81,7 @@ std::ostream& operator<<(std::ostream& out, const BufferEntryType& entry) } out << " Sensor "; - if (entry.HasStates()) + if (entry.data_.HasSensorStates()) { out << "[x]"; } diff --git a/source/mars/source/core_logic.cpp b/source/mars/source/core_logic.cpp index 9c8099a..a984a22 100644 --- a/source/mars/source/core_logic.cpp +++ b/source/mars/source/core_logic.cpp @@ -125,6 +125,13 @@ Eigen::MatrixXd CoreLogic::PropagateSensorCrossCov(const Eigen::MatrixXd& sensor bool CoreLogic::PerformSensorUpdate(std::shared_ptr sensor, const Time& timestamp, BufferEntryType* sensor_data) +{ + bool dummy; + return PerformSensorUpdate(sensor, timestamp, sensor_data, &dummy); +} + +bool CoreLogic::PerformSensorUpdate(std::shared_ptr sensor, const Time& timestamp, + BufferEntryType* sensor_data, bool* added_interm_state) { if (verbose_) { @@ -197,7 +204,8 @@ bool CoreLogic::PerformSensorUpdate(std::shared_ptr sensor, cons if (add_interm_buffer_entries_) { // Intermediate IMU Measurement and propergated state - buffer_.AddEntrySorted(interm_buffer_entry); + buffer_.AddEntrySorted(interm_buffer_entry, false); + *added_interm_state = true; if (verbose_) { @@ -238,10 +246,6 @@ bool CoreLogic::PerformSensorUpdate(std::shared_ptr sensor, cons { sensor_data->data_.set_states(corrected_state_data.core_state_, corrected_state_data.sensor_state_); - PoseMeasurementType* meas = static_cast(sensor_data->data_.measurement_.get()); - mars::PoseSensorData* prior_sensor_data = - static_cast(sensor_data->data_.sensor_state_.get()); - if (verbose_) { std::cout << "[CoreLogic]: Perform Sensor Update (Successfull) - DONE" << std::endl; @@ -295,84 +299,69 @@ void CoreLogic::PerformCoreStatePropagation(std::shared_ptr sens void CoreLogic::ReworkBufferStartingAtIndex(const int& index) { - // if (verbose_) - // { - // std::cout << "[CoreLogic]: Rework Buffer Starting At Index " << index << std::endl; - // } - - // assert(index >= 0); - - // buffer_.ClearStatesStartingAtIdx(index); - - // // The buffer grows by one index for each additional state, - // // index_offset corrects this - // int index_offset = 0; - - // // The buffer size can change during the reiteration. The reiteration needs to be done with the initial size - // // (current_buffer_length). - // const int current_buffer_length = buffer_.get_length(); - // for (int k = index; k < current_buffer_length; k++) - // { - // // All states after the "out of order" index have been deleted - // // that's why getting the latest state returns the latest valid - // // state before the out of order measurement at this point - - // // get next measurement - // int measurement_entry_idx = k + index_offset; - // int state_insertion_idx = measurement_entry_idx + 1; - - // BufferEntryType next_measurement_buffer_entry; - // buffer_.get_entry_at_idx(measurement_entry_idx, &next_measurement_buffer_entry); - - // std::shared_ptr sensor_handle = next_measurement_buffer_entry.sensor_handle_; - // Time timestamp = next_measurement_buffer_entry.timestamp_; - // BufferDataType buffer_data = next_measurement_buffer_entry.data_; - - // // Propagate State with System Input from Propagation Sensor - // if (sensor_handle == core_states_->propagation_sensor_) - // { - // // Since the measurement was not out of order, get latest state is valid - // mars::BufferEntryType latest_state_buffer_entry; - // buffer_.get_latest_state(&latest_state_buffer_entry); - - // // Zero-order hold for IMU measurement. Copy imu measurement from latest state to current sensor state - // mars::BufferEntryType latest_prop_meas_entry; - // buffer_.get_latest_sensor_handle_state(core_states_->propagation_sensor_, &latest_prop_meas_entry); - - // latest_state_buffer_entry.data_.sensor_state_ = latest_prop_meas_entry.data_.sensor_state_; - - // mars::BufferEntryType new_core_state_entry; - // new_core_state_entry = - // PerformCoreStatePropagation(sensor_handle, timestamp, std::make_shared(buffer_data), - // std::make_shared(latest_state_buffer_entry)); - - // buffer_.InsertDataAtIndex(new_core_state_entry, state_insertion_idx); - // } - // else - // { - // // non-propagation sensor information - // mars::BufferEntryType new_state_buffer_entry; - // PerformSensorUpdate(&new_state_buffer_entry, sensor_handle, timestamp, - // std::make_shared(buffer_data)); - - // buffer_.InsertDataAtIndex(new_state_buffer_entry, state_insertion_idx); - // } - - // index_offset = index_offset + 1; - // } - - // // delete last buffer entry if the max. buffer size is reached - // buffer_.RemoveOverflowEntrys(); - - // if (verbose_) - // { - // std::cout << "[CoreLogic]: Rework Buffer Starting At Index - DONE" << std::endl; - // } + if (verbose_) + { + std::cout << "[CoreLogic]: Rework Buffer Starting At Index " << index << std::endl; + } + + assert(index >= 0); + + buffer_.ClearStatesStartingAtIdx(index); + + // get running current index + int current_state_entry_idx = index; + + // The buffer size can change during the reiteration. + // Thats why we need to compare for each iteration + while (current_state_entry_idx < buffer_.get_length()) + { + // All states after the "out of order" index have been deleted + // that's why getting the latest state returns the latest valid + // state before the out of order measurement at this point + + BufferEntryType current_measurement_buffer_entry; + buffer_.get_entry_at_idx(current_state_entry_idx, ¤t_measurement_buffer_entry); + std::shared_ptr sensor_handle = current_measurement_buffer_entry.sensor_handle_; + Time timestamp = current_measurement_buffer_entry.timestamp_; + + // Propagate State with System Input from Propagation Sensor + if (sensor_handle == core_states_->propagation_sensor_) + { + mars::BufferEntryType latest_state_buffer_entry; + buffer_.get_latest_state(&latest_state_buffer_entry); + + PerformCoreStatePropagation(sensor_handle, timestamp, latest_state_buffer_entry, + ¤t_measurement_buffer_entry); + + buffer_.OverwriteDataAtIndex(current_measurement_buffer_entry, current_state_entry_idx); + current_state_entry_idx++; + } + else + { + // Processing update sensors + bool added_interm_state = false; + PerformSensorUpdate(sensor_handle, timestamp, ¤t_measurement_buffer_entry, &added_interm_state); + if (added_interm_state) + { + current_state_entry_idx++; + } + + buffer_.OverwriteDataAtIndex(current_measurement_buffer_entry, current_state_entry_idx); + current_state_entry_idx++; + } + } + + if (verbose_) + { + std::cout << "[CoreLogic]: Rework Buffer Starting At Index - DONE" << std::endl; + } } bool CoreLogic::ProcessMeasurement(std::shared_ptr sensor, const Time& timestamp, const BufferDataType& data) { + buffer_.RemoveOverflowEntrys(); + if (verbose_) { std::cout << "[CoreLogic]: Process Measurement (" << sensor->name_ << ")"; diff --git a/source/tests/mars-e2e-test/CMakeLists.txt b/source/tests/mars-e2e-test/CMakeLists.txt index ae5436d..d959ac3 100644 --- a/source/tests/mars-e2e-test/CMakeLists.txt +++ b/source/tests/mars-e2e-test/CMakeLists.txt @@ -23,6 +23,7 @@ set(sources main.cpp mars_e2e_imu_prop.cpp mars_e2e_imu_pose_update.cpp + mars_e2e_imu_pose_ooo_rework.cpp mars_e2e_imu_pose_update_perf.cpp mars_e2e_imu_prop_empty_updates.cpp ) diff --git a/source/tests/mars-e2e-test/mars_e2e_imu_pose_ooo_rework.cpp b/source/tests/mars-e2e-test/mars_e2e_imu_pose_ooo_rework.cpp new file mode 100644 index 0000000..8601c09 --- /dev/null +++ b/source/tests/mars-e2e-test/mars_e2e_imu_pose_ooo_rework.cpp @@ -0,0 +1,216 @@ +// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria. +// +// All rights reserved. +// +// This software is licensed under the terms of the BSD-2-Clause-License with +// no commercial use allowed, the full terms of which are made available +// in the LICENSE file. No license in patents is granted. +// +// You can contact the author at + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "../include_local/test_data_settings.h" + +/// +/// \brief mars_e2e_imu_pose_update End to end test with imu and pose measurements without noise +/// +class mars_e2e_imu_pose_rework_ooo : public testing::Test +{ +public: + std::string test_data_path; + YAML::Node config; + std::string traj_file_name; + std::string pose_file_name; + + std::shared_ptr imu_sensor_sptr; + std::shared_ptr core_states_sptr; + mars::CoreLogic core_logic; + + std::shared_ptr pose_sensor_sptr; + + std::vector measurement_data; + + mars_e2e_imu_pose_rework_ooo() + { + LoadInstancesAndParams(); + + measurement_data = LoadData(); + } + + void LoadInstancesAndParams() + { + test_data_path = std::string(MARS_LIB_TEST_DATA_PATH); + + // get config + config = YAML::LoadFile(test_data_path + "parameter.yaml"); + + traj_file_name = config["traj_file_name"].as(); + std::cout << "Trajectory File: " << traj_file_name << std::endl; + + pose_file_name = config["pose_file_name"].as(); + std::cout << "Pose File: " << pose_file_name << std::endl; + + std::vector imu_n_w; + std::vector imu_n_bw; + std::vector imu_n_a; + std::vector imu_n_ba; + + std::cout << "IMU Noise Parameter: " << std::endl; + read_yaml_vec_3(&imu_n_w, "imu_n_w", config); + read_yaml_vec_3(&imu_n_bw, "imu_n_bw", config); + read_yaml_vec_3(&imu_n_a, "imu_n_a", config); + read_yaml_vec_3(&imu_n_ba, "imu_n_ba", config); + + // setup propagation sensor + imu_sensor_sptr = std::make_shared("IMU"); + + // setup the core definition + core_states_sptr = std::make_shared(); + core_states_sptr.get()->set_propagation_sensor(imu_sensor_sptr); + core_states_sptr.get()->set_noise_std(Eigen::Vector3d(imu_n_w.data()), Eigen::Vector3d(imu_n_bw.data()), + Eigen::Vector3d(imu_n_a.data()), Eigen::Vector3d(imu_n_ba.data())); + + core_logic = mars::CoreLogic(core_states_sptr); + + // setup additional sensors + // Pose sensor + pose_sensor_sptr = std::make_shared("Pose", core_states_sptr); + pose_sensor_sptr->const_ref_to_nav_ = + true; // TODO is set here for now but will be managed by core logic in later versions + + // Define measurement noise + Eigen::Matrix pose_meas_std; + pose_meas_std << 0.02, 0.02, 0.02, 2 * (M_PI / 180), 2 * (M_PI / 180), 2 * (M_PI / 180); + pose_sensor_sptr->R_ = pose_meas_std.cwiseProduct(pose_meas_std); + + // Define initial calibration and covariance + mars::PoseSensorData pose_init_cal; + pose_init_cal.state_.p_ip_ = Eigen::Vector3d::Zero(); + pose_init_cal.state_.q_ip_ = Eigen::Quaterniond::Identity(); + + // The covariance should enclose the initialization with a 3 Sigma bound + Eigen::Matrix std; + std << 0.1, 0.1, 0.1, (10 * M_PI / 180), (10 * M_PI / 180), (10 * M_PI / 180); + pose_init_cal.sensor_cov_ = std.cwiseProduct(std).asDiagonal(); + + pose_sensor_sptr->set_initial_calib(std::make_shared(pose_init_cal)); + } + + std::vector LoadData() + { + std::vector measurement_data; + + std::vector measurement_data_imu; + mars::ReadSimData(&measurement_data_imu, imu_sensor_sptr, test_data_path + traj_file_name); + + std::vector measurement_data_pose; + mars::ReadPoseData(&measurement_data_pose, pose_sensor_sptr, test_data_path + pose_file_name, 1e-13); + + measurement_data.insert(measurement_data.end(), measurement_data_imu.begin(), measurement_data_imu.end()); + measurement_data.insert(measurement_data.end(), measurement_data_pose.begin(), measurement_data_pose.end()); + + std::sort(measurement_data.begin(), measurement_data.end()); + + for (int k = measurement_data.size() - 1; k >= 0; k--) + { + if (measurement_data[k].sensor_handle_ == pose_sensor_sptr && + measurement_data[k].timestamp_ > mars::Time(5 * 60) && (k < (measurement_data.size() - 11))) + { + // Swap pose measurements older than 300 seconds by 3 positions to the past to generate ooo entries + std::rotate(measurement_data.begin() + k, measurement_data.begin() + k + 1, + measurement_data.begin() + k + 1 + 3); + } + } + + return measurement_data; + } + + bool read_yaml_vec_3(std::vector* value, const std::string& parameter, YAML::Node config) + { + if (config[parameter]) + { + *value = config[parameter].as>(); + + std::cout << parameter << ": \t ["; + for (auto const& i : *value) + std::cout << i << " "; + + std::cout << " ]" << std::endl; + return true; + } + return false; + } + + void RunFilter() + { + // process data + for (auto k : measurement_data) + { + core_logic.ProcessMeasurement(k.sensor_handle_, k.timestamp_, k.data_); + + if (!core_logic.core_is_initialized_) + { + // Initialize the first time at which the propagation sensor occures + if (k.sensor_handle_ == core_logic.core_states_->propagation_sensor_) + { + Eigen::Vector3d p_wi_init(0, 0, 5); + Eigen::Quaterniond q_wi_init = Eigen::Quaterniond::Identity(); + core_logic.Initialize(p_wi_init, q_wi_init); + } + else + { + continue; + } + } + } + } + + void Reset() + { + core_logic.core_is_initialized_ = false; + core_logic.buffer_.ResetBufferData(); + pose_sensor_sptr->is_initialized_ = false; + } +}; + +TEST_F(mars_e2e_imu_pose_rework_ooo, END_2_END_IMU_POSE_UPDATE_OOO) +{ + core_logic.verbose_ = false; + core_logic.add_interm_buffer_entries_ = true; + + Reset(); + RunFilter(); + + mars::BufferEntryType latest_result; + core_logic.buffer_.get_latest_state(&latest_result); + mars::CoreStateType last_state = static_cast(latest_result.data_.core_state_.get())->state_; + + std::cout << "Last State:" << std::endl; + std::cout << last_state << std::endl; + + // Define final ground truth values + Eigen::Vector3d true_p_wi(-20946.817372738657, -3518.039994126535, 8631.1520460773336); + Eigen::Vector3d true_v_wi(15.924719563070044, -20.483884216740151, 11.455154466026718); + Eigen::Quaterniond true_q_wi(0.98996033625708202, 0.048830414166879263, -0.02917972697860232, -0.12939345742158029); + + std::cout << "p_wi error [m]: [" << (last_state.p_wi_ - true_p_wi).transpose() << " ]" << std::endl; + std::cout << "v_wi error [m/s]: [" << (last_state.v_wi_ - true_v_wi).transpose() << " ]" << std::endl; + + Eigen::Quaterniond q_wi_error(last_state.q_wi_.conjugate() * true_q_wi); + std::cout << "q_wi error [w,x,y,z]: [" << q_wi_error.w() << " " << q_wi_error.vec().transpose() << " ]" << std::endl; + + EXPECT_TRUE(last_state.p_wi_.isApprox(true_p_wi, 1e-5)); + EXPECT_TRUE(last_state.v_wi_.isApprox(true_v_wi, 1e-5)); + EXPECT_TRUE(last_state.q_wi_.coeffs().isApprox(true_q_wi.coeffs(), 1e-5)); +} diff --git a/source/tests/mars-test/mars_buffer.cpp b/source/tests/mars-test/mars_buffer.cpp index fa800f2..0c2bf16 100644 --- a/source/tests/mars-test/mars_buffer.cpp +++ b/source/tests/mars-test/mars_buffer.cpp @@ -153,6 +153,7 @@ TEST_F(mars_buffer_test, STORAGE_MAX_ENTRY) { mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_1_sptr, 1); buffer.AddEntrySorted(entry); + buffer.RemoveOverflowEntrys(); } std::cout << "Buffer Length: " << buffer.get_length() << std::endl; @@ -170,11 +171,13 @@ TEST_F(mars_buffer_test, STORAGE_MAX_ENTRY) { mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_1_sptr, 1); buffer2.AddEntrySorted(entry); + buffer2.RemoveOverflowEntrys(); } else { mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_2_sptr, 1); buffer2.AddEntrySorted(entry); + buffer2.RemoveOverflowEntrys(); } } buffer2.PrintBufferEntries(); @@ -197,22 +200,24 @@ TEST_F(mars_buffer_test, STORAGE_MAX_ENTRY) { mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_1_sptr, 1); buffer3.AddEntrySorted(entry); + buffer3.RemoveOverflowEntrys(); } else { mars::BufferEntryType entry(mars::Time(k), data, pose_sensor_2_sptr, 1); buffer3.AddEntrySorted(entry); + buffer3.RemoveOverflowEntrys(); } } buffer3.PrintBufferEntries(); ASSERT_EQ(buffer.get_length(), max_buffer_size); mars::BufferEntryType t3_idx0, t3_idx1; - buffer3.get_entry_at_idx(0, &t2_idx0); - buffer3.get_entry_at_idx(1, &t2_idx1); + buffer3.get_entry_at_idx(0, &t3_idx0); + buffer3.get_entry_at_idx(1, &t3_idx1); - ASSERT_EQ(t2_idx0.timestamp_, 9); - ASSERT_EQ(t2_idx1.timestamp_, 11); + ASSERT_EQ(t3_idx0.timestamp_, 9); + ASSERT_EQ(t3_idx1.timestamp_, 11); } TEST_F(mars_buffer_test, LATEST_ENTRY) From 03205e7c527360e14a3dfdc34d5c177f03d6618a Mon Sep 17 00:00:00 2001 From: Christian Brommer Date: Mon, 3 Jun 2024 17:36:52 +0200 Subject: [PATCH 5/5] Final clean after new buffer implementation - Moved InsertEntryAtTimestamp to AddEntrySorted - Enabeled Chi2 test by default - Added e2e Chi2 test - Minor cleanups e.g. removal of unnecessary include --- source/mars/include/mars/buffer.h | 12 +- .../mars/include/mars/data_utils/write_csv.h | 1 - source/mars/include/mars/ekf.h | 2 +- source/mars/source/buffer.cpp | 93 ++++--- source/mars/source/buffer_entry_type.cpp | 2 +- source/mars/source/core_logic.cpp | 6 +- source/mars/source/time.cpp | 3 +- source/tests/mars-e2e-test/CMakeLists.txt | 1 + .../mars_e2e_imu_pose_outlier.cpp | 231 ++++++++++++++++++ source/tests/mars-test/mars_buffer.cpp | 10 +- 10 files changed, 290 insertions(+), 71 deletions(-) create mode 100644 source/tests/mars-e2e-test/mars_e2e_imu_pose_outlier.cpp diff --git a/source/mars/include/mars/buffer.h b/source/mars/include/mars/buffer.h index 652d1c0..aec9b0b 100644 --- a/source/mars/include/mars/buffer.h +++ b/source/mars/include/mars/buffer.h @@ -202,6 +202,9 @@ class Buffer /// \return Index of the added entry. The index is -1 if the entry was removed because the max_buffer_size was /// reached. /// + /// The method finds the closest timestamp based on the shortest 'time' distance between the new and existing buffer + /// elements. It then determines if the entry needs to be added before or after the closest entry. + /// int AddEntrySorted(const BufferEntryType& new_entry, const bool& after = true); /// @@ -227,15 +230,6 @@ class Buffer /// bool IsSorted() const; - /// - /// \brief Inserting new element before the element at the specified position - /// \param new_entry ntry that is added to the buffer - /// - /// The method finds the closest timestamp based on the shortest 'time' distance between the new and existing buffer - /// elements. It then determines if the entry needs to be added before or after the closest entry. - /// - int InsertDataAtTimestamp(const BufferEntryType& new_entry, const bool& after = true); - /// /// \brief InsertDataAtIndex Adds 'entry' at buffer position 'index' /// \param new_entry Entry buffer entry to be added diff --git a/source/mars/include/mars/data_utils/write_csv.h b/source/mars/include/mars/data_utils/write_csv.h index da3fd8b..1ea0c07 100644 --- a/source/mars/include/mars/data_utils/write_csv.h +++ b/source/mars/include/mars/data_utils/write_csv.h @@ -23,7 +23,6 @@ class WriteCsv inline static std::string vec_to_csv(const Eigen::VectorXd& a) { std::stringstream os; - // TODO(chb) option to pre or post add comma for (int k = 0; k < a.size(); k++) { os << ", " << a(k); diff --git a/source/mars/include/mars/ekf.h b/source/mars/include/mars/ekf.h index d632ef7..bde5c7d 100644 --- a/source/mars/include/mars/ekf.h +++ b/source/mars/include/mars/ekf.h @@ -80,7 +80,7 @@ class Chi2 int dof_{ 3 }; /// Degrees of freedom for the setup double chi_value_{ 0.05 }; /// Chi value for the confidence intervall (0.05 represents 95% test) double ucv_; /// Upper critival value - bool do_test_{ false }; /// Determine if the test is performed or not + bool do_test_{ true }; /// Determine if the test is performed or not bool passed_{ false }; /// Shows if the test passed or not (true=passed) private: diff --git a/source/mars/source/buffer.cpp b/source/mars/source/buffer.cpp index 09c12df..6adf391 100644 --- a/source/mars/source/buffer.cpp +++ b/source/mars/source/buffer.cpp @@ -1,4 +1,4 @@ -// Copyright (C) 2021 Christian Brommer and Martin Scheiber, Control of Networked Systems, University of Klagenfurt, +// Copyright (C) 2024 Christian Brommer and Martin Scheiber, Control of Networked Systems, University of Klagenfurt, // Austria. // // All rights reserved. @@ -365,9 +365,49 @@ bool Buffer::RemoveSensorFromBuffer(const std::shared_ptr& senso int Buffer::AddEntrySorted(const BufferEntryType& new_entry, const bool& after) { - int index = InsertDataAtTimestamp(new_entry, after); + if (this->IsEmpty()) + { + data_.push_back(new_entry); + // entry is added at idx 0, buffer was empty + return 0; + } + + BufferEntryType latest_entry; + this->get_latest_entry(&latest_entry); + if (latest_entry <= new_entry) + { + data_.push_back(new_entry); + // get index based on iterator + return static_cast(data_.end() - data_.begin()) - 1; + } + + Time previous_time_distance(1e100); + const Time timestamp = new_entry.timestamp_; + + // iterate backwards and start with latest entry + // find the first entry at which (state_entry_stamp - new_stamp) is >=0 + // the new entry is entered after this index (idx+1) + for (int k = data_.size() - 1; k >= 0; --k) + { + Time current_time_distance = timestamp - data_[k].timestamp_; + + if (current_time_distance.get_seconds() >= 0) + { + int insert_idx = k; + + if (after) + { + insert_idx += 1; + } + + data_.insert(data_.begin() + insert_idx, new_entry); + return insert_idx; // return entry index + } + } - return index; + // If the buffer has only one element and the new entry is older then the existing entry + data_.push_front(new_entry); + return 0; // push front adds element at index 0 } int Buffer::FindClosestTimestamp(const Time& /*timestamp*/) const @@ -417,53 +457,6 @@ bool Buffer::IsSorted() const return std::is_sorted(data_.begin(), data_.end()); } -int Buffer::InsertDataAtTimestamp(const BufferEntryType& new_entry, const bool& after) -{ - if (this->IsEmpty()) - { - data_.push_back(new_entry); - // entry is added at idx 0, buffer was empty - return 0; - } - - BufferEntryType latest_entry; - this->get_latest_entry(&latest_entry); - if (latest_entry <= new_entry) - { - data_.push_back(new_entry); - // get index based on iterator - return static_cast(data_.end() - data_.begin()) - 1; - } - - Time previous_time_distance(1e100); - const Time timestamp = new_entry.timestamp_; - - // iterate backwards and start with latest entry - // find the first entry at which (state_entry_stamp - new_stamp) is >=0 - // the new entry is entered after this index (idx+1) - for (int k = data_.size() - 1; k >= 0; --k) - { - Time current_time_distance = timestamp - data_[k].timestamp_; - - if (current_time_distance.get_seconds() >= 0) - { - int insert_idx = k; - - if (after) - { - insert_idx += 1; - } - - data_.insert(data_.begin() + insert_idx, new_entry); - return insert_idx; // return entry index - } - } - - // If the buffer has only one element and the new entry is older then the existing entry - data_.push_front(new_entry); - return 0; // push front adds element at index 0 -} - bool Buffer::OverwriteDataAtIndex(const BufferEntryType& new_entry, const int& index) { if (index < (this->get_length())) diff --git a/source/mars/source/buffer_entry_type.cpp b/source/mars/source/buffer_entry_type.cpp index afcb60a..9e0d15a 100644 --- a/source/mars/source/buffer_entry_type.cpp +++ b/source/mars/source/buffer_entry_type.cpp @@ -1,4 +1,4 @@ -// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria. +// Copyright (C) 2024 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria. // // All rights reserved. // diff --git a/source/mars/source/core_logic.cpp b/source/mars/source/core_logic.cpp index a984a22..97a77b6 100644 --- a/source/mars/source/core_logic.cpp +++ b/source/mars/source/core_logic.cpp @@ -155,7 +155,7 @@ bool CoreLogic::PerformSensorUpdate(std::shared_ptr sensor, cons BufferDataType init_data = sensor->Initialize(timestamp, sensor_data->data_.measurement_, std::make_shared(latest_core_data)); - // TODO the init function should directly receive the state entry and return it + // TODO (CHB) the init function should directly receive the state entry and return it sensor_data->data_.set_states(init_data.core_state_, init_data.sensor_state_); sensor_data->metadata_ = BufferMetadataType::init; @@ -239,12 +239,13 @@ bool CoreLogic::PerformSensorUpdate(std::shared_ptr sensor, cons sensor->CalcUpdate(timestamp, sensor_data->data_.measurement_, prior_core_data.state_, prior_sensor_state_entry.data_.sensor_state_, corrected_cov, &corrected_state_data); - // TODO: This should also happen inside the update class or a preset object should be given that already has the + // TODO(CHB): This should also happen inside the update class or a preset object should be given that already has the // measurement if (successful_update) { sensor_data->data_.set_states(corrected_state_data.core_state_, corrected_state_data.sensor_state_); + sensor_data->metadata_ = mars::BufferMetadataType::invalid; if (verbose_) { @@ -341,6 +342,7 @@ void CoreLogic::ReworkBufferStartingAtIndex(const int& index) // Processing update sensors bool added_interm_state = false; PerformSensorUpdate(sensor_handle, timestamp, ¤t_measurement_buffer_entry, &added_interm_state); + if (added_interm_state) { current_state_entry_idx++; diff --git a/source/mars/source/time.cpp b/source/mars/source/time.cpp index 2da0adf..9ae1188 100644 --- a/source/mars/source/time.cpp +++ b/source/mars/source/time.cpp @@ -12,7 +12,6 @@ #include #include #include -#include namespace mars { @@ -28,7 +27,7 @@ Time Time::get_time_now() { using namespace std::chrono; auto sys_time_now = system_clock::now(); - double sys_sec = double(duration_cast(sys_time_now.time_since_epoch()).count())/1e3; + double sys_sec = double(duration_cast(sys_time_now.time_since_epoch()).count()) / 1e3; // Cast to millisecond precision only // auto sys_ms = (duration_cast(sys_time_now.time_since_epoch()) - diff --git a/source/tests/mars-e2e-test/CMakeLists.txt b/source/tests/mars-e2e-test/CMakeLists.txt index d959ac3..a821b90 100644 --- a/source/tests/mars-e2e-test/CMakeLists.txt +++ b/source/tests/mars-e2e-test/CMakeLists.txt @@ -24,6 +24,7 @@ set(sources mars_e2e_imu_prop.cpp mars_e2e_imu_pose_update.cpp mars_e2e_imu_pose_ooo_rework.cpp + mars_e2e_imu_pose_outlier.cpp mars_e2e_imu_pose_update_perf.cpp mars_e2e_imu_prop_empty_updates.cpp ) diff --git a/source/tests/mars-e2e-test/mars_e2e_imu_pose_outlier.cpp b/source/tests/mars-e2e-test/mars_e2e_imu_pose_outlier.cpp new file mode 100644 index 0000000..6007ac5 --- /dev/null +++ b/source/tests/mars-e2e-test/mars_e2e_imu_pose_outlier.cpp @@ -0,0 +1,231 @@ +// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria. +// +// All rights reserved. +// +// This software is licensed under the terms of the BSD-2-Clause-License with +// no commercial use allowed, the full terms of which are made available +// in the LICENSE file. No license in patents is granted. +// +// You can contact the author at + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "../include_local/test_data_settings.h" + +/// +/// \brief mars_e2e_imu_pose_update End to end test with imu and pose measurements without noise +/// +class mars_e2e_imu_pose_outlier : public testing::Test +{ +public: + std::string test_data_path; + YAML::Node config; + std::string traj_file_name; + std::string pose_file_name; + + std::shared_ptr imu_sensor_sptr; + std::shared_ptr core_states_sptr; + mars::CoreLogic core_logic; + + std::shared_ptr pose_sensor_sptr; + + std::vector measurement_data; + + mars_e2e_imu_pose_outlier() + { + LoadInstancesAndParams(); + + measurement_data = LoadData(); + } + + void LoadInstancesAndParams() + { + test_data_path = std::string(MARS_LIB_TEST_DATA_PATH); + + // get config + config = YAML::LoadFile(test_data_path + "parameter.yaml"); + + traj_file_name = config["traj_file_name"].as(); + std::cout << "Trajectory File: " << traj_file_name << std::endl; + + pose_file_name = config["pose_file_name"].as(); + std::cout << "Pose File: " << pose_file_name << std::endl; + + std::vector imu_n_w; + std::vector imu_n_bw; + std::vector imu_n_a; + std::vector imu_n_ba; + + std::cout << "IMU Noise Parameter: " << std::endl; + read_yaml_vec_3(&imu_n_w, "imu_n_w", config); + read_yaml_vec_3(&imu_n_bw, "imu_n_bw", config); + read_yaml_vec_3(&imu_n_a, "imu_n_a", config); + read_yaml_vec_3(&imu_n_ba, "imu_n_ba", config); + + // setup propagation sensor + imu_sensor_sptr = std::make_shared("IMU"); + + // setup the core definition + core_states_sptr = std::make_shared(); + core_states_sptr.get()->set_propagation_sensor(imu_sensor_sptr); + core_states_sptr.get()->set_noise_std(Eigen::Vector3d(imu_n_w.data()), Eigen::Vector3d(imu_n_bw.data()), + Eigen::Vector3d(imu_n_a.data()), Eigen::Vector3d(imu_n_ba.data())); + + core_logic = mars::CoreLogic(core_states_sptr); + + // setup additional sensors + // Pose sensor + pose_sensor_sptr = std::make_shared("Pose", core_states_sptr); + // TODO is set here for now but will be managed by core logic in later versions + pose_sensor_sptr->const_ref_to_nav_ = true; + + // Define measurement noise + Eigen::Matrix pose_meas_std; + pose_meas_std << 0.02, 0.02, 0.02, 2 * (M_PI / 180), 2 * (M_PI / 180), 2 * (M_PI / 180); + pose_sensor_sptr->R_ = pose_meas_std.cwiseProduct(pose_meas_std); + + // Define initial calibration and covariance + mars::PoseSensorData pose_init_cal; + pose_init_cal.state_.p_ip_ = Eigen::Vector3d::Zero(); + pose_init_cal.state_.q_ip_ = Eigen::Quaterniond::Identity(); + + // The covariance should enclose the initialization with a 3 Sigma bound + Eigen::Matrix std; + std << 0.1, 0.1, 0.1, (10 * M_PI / 180), (10 * M_PI / 180), (10 * M_PI / 180); + pose_init_cal.sensor_cov_ = std.cwiseProduct(std).asDiagonal(); + + pose_sensor_sptr->set_initial_calib(std::make_shared(pose_init_cal)); + } + + std::vector LoadData() + { + std::vector measurement_data; + + std::vector measurement_data_imu; + mars::ReadSimData(&measurement_data_imu, imu_sensor_sptr, test_data_path + traj_file_name); + + std::vector measurement_data_pose; + mars::ReadPoseData(&measurement_data_pose, pose_sensor_sptr, test_data_path + pose_file_name, 1e-13); + + measurement_data.insert(measurement_data.end(), measurement_data_imu.begin(), measurement_data_imu.end()); + measurement_data.insert(measurement_data.end(), measurement_data_pose.begin(), measurement_data_pose.end()); + + std::sort(measurement_data.begin(), measurement_data.end()); + + for (auto it = measurement_data.begin(); it != measurement_data.end(); it++) + { + if (it->sensor_handle_ == pose_sensor_sptr && it->timestamp_ > mars::Time(5 * 60) && + (it - measurement_data.begin()) % 2) + { + mars::PoseMeasurementType* meas = static_cast(it->data_.measurement_.get()); + meas->position_ = Eigen::Vector3d(1e5, 1e5, 1e5); + + it->data_.set_measurement(std::make_shared(*meas)); + } + } + + return measurement_data; + } + + bool read_yaml_vec_3(std::vector* value, const std::string& parameter, YAML::Node config) + { + if (config[parameter]) + { + *value = config[parameter].as>(); + + std::cout << parameter << ": \t ["; + for (auto const& i : *value) + std::cout << i << " "; + + std::cout << " ]" << std::endl; + return true; + } + return false; + } + + void RunFilter() + { + // process data + for (auto k : measurement_data) + { + core_logic.ProcessMeasurement(k.sensor_handle_, k.timestamp_, k.data_); + + if (!core_logic.core_is_initialized_) + { + // Initialize the first time at which the propagation sensor occures + if (k.sensor_handle_ == core_logic.core_states_->propagation_sensor_) + { + Eigen::Vector3d p_wi_init(0, 0, 5); + Eigen::Quaterniond q_wi_init = Eigen::Quaterniond::Identity(); + core_logic.Initialize(p_wi_init, q_wi_init); + } + else + { + continue; + } + } + } + } + + void Reset() + { + core_logic.core_is_initialized_ = false; + core_logic.buffer_.ResetBufferData(); + pose_sensor_sptr->is_initialized_ = false; + } +}; + +TEST_F(mars_e2e_imu_pose_outlier, END_2_END_IMU_POSE_UPDATE_OUTLIER) +{ + core_logic.verbose_ = false; + core_logic.add_interm_buffer_entries_ = true; + + Reset(); + RunFilter(); + + mars::BufferEntryType latest_result; + core_logic.buffer_.get_latest_state(&latest_result); + mars::CoreStateType last_state = static_cast(latest_result.data_.core_state_.get())->state_; + + std::cout << "Last State:" << std::endl; + std::cout << last_state << std::endl; + + // Define final ground truth values + Eigen::Vector3d true_p_wi(-20946.817372738657, -3518.039994126535, 8631.1520460773336); + Eigen::Vector3d true_v_wi(15.924719563070044, -20.483884216740151, 11.455154466026718); + Eigen::Quaterniond true_q_wi(0.98996033625708202, 0.048830414166879263, -0.02917972697860232, -0.12939345742158029); + + std::cout << "p_wi error [m]: [" << (last_state.p_wi_ - true_p_wi).transpose() << " ]" << std::endl; + std::cout << "v_wi error [m/s]: [" << (last_state.v_wi_ - true_v_wi).transpose() << " ]" << std::endl; + + Eigen::Quaterniond q_wi_error(last_state.q_wi_.conjugate() * true_q_wi); + std::cout << "q_wi error [w,x,y,z]: [" << q_wi_error.w() << " " << q_wi_error.vec().transpose() << " ]" << std::endl; + + EXPECT_TRUE(last_state.p_wi_.isApprox(true_p_wi, 1e-5)); + EXPECT_TRUE(last_state.v_wi_.isApprox(true_v_wi, 1e-5)); + EXPECT_TRUE(last_state.q_wi_.coeffs().isApprox(true_q_wi.coeffs(), 1e-5)); + + // core_logic.buffer_.PrintBufferEntries(); +} + +TEST_F(mars_e2e_imu_pose_outlier, END_2_END_IMU_POSE_UPDATE_OUTLIER_TAGGING) +{ + mars::BufferEntryType latest_result; + core_logic.buffer_.get_entry_at_idx(295, &latest_result); + EXPECT_TRUE(latest_result.metadata_ == mars::BufferMetadataType::invalid); + core_logic.buffer_.get_entry_at_idx(284, &latest_result); + EXPECT_TRUE(latest_result.metadata_ == mars::BufferMetadataType::invalid); + core_logic.buffer_.get_entry_at_idx(273, &latest_result); + EXPECT_TRUE(latest_result.metadata_ == mars::BufferMetadataType::invalid); +} diff --git a/source/tests/mars-test/mars_buffer.cpp b/source/tests/mars-test/mars_buffer.cpp index 0c2bf16..016e80f 100644 --- a/source/tests/mars-test/mars_buffer.cpp +++ b/source/tests/mars-test/mars_buffer.cpp @@ -774,18 +774,18 @@ TEST_F(mars_buffer_test, INSERT_DATA_IDX_TEST) buffer.AddEntrySorted(mars::BufferEntryType(7, data_with_state, pose_sensor_1_sptr)); // New entry is newer than newest existing entry and needs to be inserted at idx 4 - int idx_newer = buffer.InsertDataAtTimestamp(mars::BufferEntryType(8, data_with_state, pose_sensor_1_sptr)); + int idx_newer = buffer.AddEntrySorted(mars::BufferEntryType(8, data_with_state, pose_sensor_1_sptr)); ASSERT_EQ(4, idx_newer); // add entry in the middle of the buffer - int idx_mid = buffer.InsertDataAtTimestamp(mars::BufferEntryType(5.3, data_with_state, pose_sensor_1_sptr)); + int idx_mid = buffer.AddEntrySorted(mars::BufferEntryType(5.3, data_with_state, pose_sensor_1_sptr)); ASSERT_EQ(2, idx_mid); - idx_mid = buffer.InsertDataAtTimestamp(mars::BufferEntryType(5.6, data_with_state, pose_sensor_1_sptr)); + idx_mid = buffer.AddEntrySorted(mars::BufferEntryType(5.6, data_with_state, pose_sensor_1_sptr)); ASSERT_EQ(3, idx_mid); // New entry is older than oldest existing entry and needs to be inserted at idx 0 - int idx_older = buffer.InsertDataAtTimestamp(mars::BufferEntryType(1, data_with_state, pose_sensor_1_sptr)); + int idx_older = buffer.AddEntrySorted(mars::BufferEntryType(1, data_with_state, pose_sensor_1_sptr)); ASSERT_EQ(int(0), idx_older); } @@ -1385,7 +1385,7 @@ TEST_F(mars_buffer_test, BUFFER_GET_INTERMEDIATE_ENTRY_PAIR) // EXPECT_EQ(imu_state_at_5.timestamp_, 5); } -TEST_F(mars_buffer_test, INSERT_DATA_AT_IDX) +TEST_F(mars_buffer_test, OVERWRITE_ENTRY_AT_INDEX) { // TODO }