Merge pull request #1 from locusrobotics/issue-300-prior-on-last-stamp

[Issue-300] Handle prior on last stamp

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/src/timestamp_manager.cpp

@@ -48,7 +48,6 @@
 #include <utility>
 #include <vector>
 
-
 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<std::pair<ros::Time, ros::Time>> 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.

fuse_core/test/example_variable.h

@@ -58,10 +58,22 @@ class ExampleVariable : public fuse_core::Variable
   {
   }
 
+  explicit 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);