Added unit test to illustrate the bug from Issue #300

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}

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

fuse_core/test/test_timestamp_manager.cpp

@@ -37,6 +37,7 @@
 #include <fuse_core/variable.h>
 #include <ros/duration.h>
 #include <ros/time.h>
+#include <test/example_variable.h>
 
 #include <gtest/gtest.h>
 
@@ -80,9 +81,17 @@ class TimestampManagerTestFixture : public ::testing::Test
     const ros::Time& beginning_stamp,
     const ros::Time& ending_stamp,
     std::vector<fuse_core::Constraint::SharedPtr>& /*constraints*/,
-    std::vector<fuse_core::Variable::SharedPtr>& /*variables*/)
+    std::vector<fuse_core::Variable::SharedPtr>& 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);