From 0ab718932a09c695dace64a09411713cb79a43e2 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Tue, 11 Apr 2023 21:44:16 -0700 Subject: [PATCH] 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);