From 0ab718932a09c695dace64a09411713cb79a43e2 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Tue, 11 Apr 2023 21:44:16 -0700 Subject: [PATCH 1/3] Added unit test to illustrate the bug from Issue #300 --- fuse_core/CMakeLists.txt | 1 + fuse_core/test/example_variable.h | 14 ++- fuse_core/test/test_timestamp_manager.cpp | 120 +++++++++++++++++++++- 3 files changed, 133 insertions(+), 2 deletions(-) diff --git a/fuse_core/CMakeLists.txt b/fuse_core/CMakeLists.txt index 484a6ac99..fa18c8452 100644 --- a/fuse_core/CMakeLists.txt +++ b/fuse_core/CMakeLists.txt @@ -361,6 +361,7 @@ if(CATKIN_ENABLE_TESTING) target_include_directories(test_timestamp_manager PRIVATE include + ${CMAKE_CURRENT_SOURCE_DIR} ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} diff --git a/fuse_core/test/example_variable.h b/fuse_core/test/example_variable.h index 10600afb2..c825242a7 100644 --- a/fuse_core/test/example_variable.h +++ b/fuse_core/test/example_variable.h @@ -58,10 +58,22 @@ class ExampleVariable : public fuse_core::Variable { } + ExampleVariable(const ros::Time& stamp) : + fuse_core::Variable(fuse_core::uuid::generate("ExampleVariable", stamp)), + data_(0.0) + { + } + size_t size() const override { return 1; } const double* data() const override { return &data_; }; double* data() override { return &data_; }; - void print(std::ostream& /*stream = std::cout*/) const override {} + void print(std::ostream& stream = std::cout) const override + { + stream << type() << ":\n" + << " uuid: " << uuid() << "\n" + << " data:\n" + << " - [0]: " << data()[0] << "\n"; + } private: double data_; diff --git a/fuse_core/test/test_timestamp_manager.cpp b/fuse_core/test/test_timestamp_manager.cpp index e7858b6f2..27e2b1b14 100644 --- a/fuse_core/test/test_timestamp_manager.cpp +++ b/fuse_core/test/test_timestamp_manager.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include @@ -80,9 +81,17 @@ class TimestampManagerTestFixture : public ::testing::Test const ros::Time& beginning_stamp, const ros::Time& ending_stamp, std::vector& /*constraints*/, - std::vector& /*variables*/) + std::vector& variables) { generated_time_spans.emplace_back(beginning_stamp, ending_stamp); + + auto variable1 = ExampleVariable::make_shared(beginning_stamp); + variable1->data()[0] = beginning_stamp.toSec(); + variables.push_back(variable1); + + auto variable2 = ExampleVariable::make_shared(ending_stamp); + variable2->data()[0] = ending_stamp.toSec(); + variables.push_back(variable2); } fuse_core::TimestampManager manager; @@ -856,6 +865,115 @@ TEST_F(TimestampManagerTestFixture, SplitSameMultiple) EXPECT_EQ(ros::Time(30, 0), generated_time_spans[4].second); } +TEST_F(TimestampManagerTestFixture, PriorOnLast) +{ + // Test: + // Existing: |------111111112222222233333333-------> t + // Adding: |-----------------------------*-------> t + // Expected: |------111111112222222233333333-------> t + // In Issue 300, it was reported that adding a transaction that involves only the last variable does not properly + // update the variable value in the input transaction. + + populate(); + fuse_core::Transaction transaction; + transaction.addInvolvedStamp(ros::Time(40, 0)); + // Add a variable for the last timestamp and set the value to something known. We expect the query() to replace + // the variable value with the whatever was generated by the motion model. It this test case, the variable value + // will be the timestamp converted to a floating point value. + auto variable1 = ExampleVariable::make_shared(ros::Time(40, 0)); + variable1->data()[0] = -99.0; + transaction.addVariable(variable1); + // Request that the input variables be updated with the manager's variable values. + manager.query(transaction, true); + + // Verify the manager contains the expected timestamps + auto stamp_range = manager.stamps(); + ASSERT_EQ(4, std::distance(stamp_range.begin(), stamp_range.end())); + auto stamp_range_iter = stamp_range.begin(); + EXPECT_EQ(ros::Time(10, 0), *stamp_range_iter); + ++stamp_range_iter; + EXPECT_EQ(ros::Time(20, 0), *stamp_range_iter); + ++stamp_range_iter; + EXPECT_EQ(ros::Time(30, 0), *stamp_range_iter); + ++stamp_range_iter; + EXPECT_EQ(ros::Time(40, 0), *stamp_range_iter); + + // Verify no new motion model segments were generated + ASSERT_EQ(0ul, generated_time_spans.size()); + + // Verify the input variable value was updated to the timestamp manager's value (40.0 in this case) + for (auto&& variable : transaction.addedVariables()) + { + if (variable.uuid() == variable1->uuid()) + { + ASSERT_DOUBLE_EQ(40.0, variable.data()[0]) << "Transaction contains:\n" << transaction; + } + else + { + FAIL() << "Unexpected added variable:\n" << variable; + } + } +} + +TEST_F(TimestampManagerTestFixture, DeltaOnLast) +{ + // Test: + // Existing: |------111111112222222233333333-------> t + // Adding: |----------------------********-------> t + // Expected: |------111111112222222233333333-------> t + // In Issue 300, it was reported that adding a transaction that involves only the last variable does not properly + // update the variable value in the input transaction. Verify that adding a transaction that involves two timestamps + // does update the variable values as expected. + + populate(); + fuse_core::Transaction transaction; + transaction.addInvolvedStamp(ros::Time(30, 0)); + transaction.addInvolvedStamp(ros::Time(40, 0)); + // Add a variable for the last timestamp and set the value to something known. We expect the query() to replace + // the variable value with the whatever was generated by the motion model. It this test case, the variable value + // will be the timestamp converted to a floating point value. + auto variable1 = ExampleVariable::make_shared(ros::Time(30, 0)); + variable1->data()[0] = -98.0; + transaction.addVariable(variable1); + auto variable2 = ExampleVariable::make_shared(ros::Time(40, 0)); + variable2->data()[0] = -99.0; + transaction.addVariable(variable2); + // Request that the input variables be updated with the manager's variable values. + manager.query(transaction, true); + + // Verify the manager contains the expected timestamps + auto stamp_range = manager.stamps(); + ASSERT_EQ(4, std::distance(stamp_range.begin(), stamp_range.end())); + auto stamp_range_iter = stamp_range.begin(); + EXPECT_EQ(ros::Time(10, 0), *stamp_range_iter); + ++stamp_range_iter; + EXPECT_EQ(ros::Time(20, 0), *stamp_range_iter); + ++stamp_range_iter; + EXPECT_EQ(ros::Time(30, 0), *stamp_range_iter); + ++stamp_range_iter; + EXPECT_EQ(ros::Time(40, 0), *stamp_range_iter); + + // Verify no new motion model segments were generated + ASSERT_EQ(0ul, generated_time_spans.size()); + + // Verify the input variable values were updated to the timestamp manager's values (30.0 and 40.0 in this case) + for (auto&& variable : transaction.addedVariables()) + { + if (variable.uuid() == variable1->uuid()) + { + ASSERT_DOUBLE_EQ(30.0, variable.data()[0]) << "Transaction contains:\n" << transaction; + } + else if (variable.uuid() == variable2->uuid()) + { + ASSERT_DOUBLE_EQ(40.0, variable.data()[0]) << "Transaction contains:\n" << transaction; + } + else + { + FAIL() << "Unexpected added variable: " << variable; + } + } +} + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); From 9729c837b801bfc22e233d81be409f9ce2dc2920 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Wed, 26 Apr 2023 21:24:28 -0700 Subject: [PATCH 2/3] Move where the input variables are updated to the motion model values to ensure that transactions that involve only a single timestamp are correctly handled --- fuse_core/src/timestamp_manager.cpp | 52 ++++++++++++++++++----------- 1 file changed, 32 insertions(+), 20 deletions(-) diff --git a/fuse_core/src/timestamp_manager.cpp b/fuse_core/src/timestamp_manager.cpp index d8d00aacf..ffed2c2af 100644 --- a/fuse_core/src/timestamp_manager.cpp +++ b/fuse_core/src/timestamp_manager.cpp @@ -48,7 +48,6 @@ #include #include - namespace fuse_core { @@ -97,6 +96,38 @@ void TimestampManager::query( { augmented_stamps.insert(end->first); } + // If requested, add the motion model version of the variables involved in the input transaction. + // This ensures that the variables in the final transaction will be overwritten with the motion model version. + // Issue #300: Moved the variable update step outside the pair handling loop to ensure that transactions involving + // only a single stamp will still have their initial conditions updated. + if (update_variables) + { + // The motion model segments are indexed by the first timestamp of the segment. The very last entry in the + // motion model history is always an empty record assigned to the final timestamp of the final pair. + // If the first input timestamp references the very last entry, then \p begin will point at this last, empty + // entry in the motion model history. Check for that case, and back up one position if needed. + if (begin != motion_model_history_.begin()) + { + --begin; + } + auto transaction_variables = transaction.addedVariables(); + for (auto iter = begin; iter != end; ++iter) + { + for (const auto& variable : iter->second.variables) + { + if (std::any_of( + transaction_variables.begin(), + transaction_variables.end(), + [variable_uuid = variable->uuid()](const auto& input_variable) + { + return input_variable.uuid() == variable_uuid; + })) // NOLINT + { + motion_model_transaction.addVariable(variable, update_variables); + } + } + } + } } // Convert the sequence of stamps into stamp pairs that must be generated std::vector> stamp_pairs; @@ -113,25 +144,6 @@ void TimestampManager::query( (history_iter->second.beginning_stamp == previous_stamp) && (history_iter->second.ending_stamp == current_stamp)) { - if (update_variables) - { - // Add the motion model version of the variables involved in this motion model segment - // This ensures that the variables in the final transaction will be overwritten with the motion model version - auto transaction_variables = transaction.addedVariables(); - for (const auto& variable : history_iter->second.variables) - { - if (std::any_of( - transaction_variables.begin(), - transaction_variables.end(), - [variable_uuid = variable->uuid()](const auto& input_variable) - { - return input_variable.uuid() == variable_uuid; - })) // NOLINT - { - motion_model_transaction.addVariable(variable, update_variables); - } - } - } continue; } // Check if this stamp is in the middle of an existing entry. If so, delete it. From bbf3608863dacc99b1bc9b2922d200665865f3c3 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 7 May 2023 19:47:24 -0700 Subject: [PATCH 3/3] Fix linter --- fuse_core/test/example_variable.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_core/test/example_variable.h b/fuse_core/test/example_variable.h index c825242a7..107de9331 100644 --- a/fuse_core/test/example_variable.h +++ b/fuse_core/test/example_variable.h @@ -58,7 +58,7 @@ class ExampleVariable : public fuse_core::Variable { } - ExampleVariable(const ros::Time& stamp) : + explicit ExampleVariable(const ros::Time& stamp) : fuse_core::Variable(fuse_core::uuid::generate("ExampleVariable", stamp)), data_(0.0) {