Fix tests by ensuring non-repeated timestamps

test/gtest_frame_base.cpp

@@ -70,7 +70,7 @@ TEST(FrameBase, LinksToTree)
     intrinsics_odo.k_rot_to_rot = 1;
     auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
     auto F1 = FrameBase::emplaceKeyFrame<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
-    auto F2 = FrameBase::emplaceKeyFrame<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    auto F2 = FrameBase::emplaceKeyFrame<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
     auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr);
     WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
     auto p = std::make_shared<ProcessorOdom2d>(std::make_shared<ParamsProcessorOdom2d>());

test/gtest_processor_diff_drive.cpp

@@ -352,9 +352,11 @@ TEST_F(ProcessorDiffDriveTest, linear)
     data(0) = 100.0 ; // one turn of the wheels
     data(1) = 100.0 ;
 
+    t += 1.0;
     auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
 
     C->process();
+
     WOLF_TRACE("t = ", t, "; x = ", processor->getState().vector("PO").transpose());
 
     // radius is 1.0m, 100 ticks per revolution, so advanced distance is
@@ -377,6 +379,7 @@ TEST_F(ProcessorDiffDriveTest, angular)
     processor->setOrigin(F0);
 
     // Straight one turn of the wheels, in one go
+    t += 1.0;
     data(0) = -20.0 ; // one fifth of a turn of the left wheel, in reverse
     data(1) =  20.0 ; // one fifth of a turn of the right wheel, forward --> we'll turn left --> positive angle
 

test/gtest_solver_manager.cpp

@@ -569,14 +569,17 @@ TEST(SolverManager, MultiThreadingTruncatedNotifications)
 
     // loop emplacing and removing frames (window of 10 KF)
     auto start = std::chrono::high_resolution_clock::now();
+    TimeStamp ts(0);
     while (true)
     {
         // Emplace Frame, Capture, feature and factor pose 2d
-        FrameBasePtr        F = P->emplaceKeyFrame(TimeStamp(0), P->stateZero());
-        auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
+        FrameBasePtr        F = P->emplaceKeyFrame(ts, P->stateZero());
+        auto C = CaptureBase::emplace<CaptureVoid>(F, ts, nullptr);
         auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity());
         auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
 
+        ts += 1.0;
+
         if (P->getTrajectory()->getFrameList().size() > 10)
             (*P->getTrajectory()->begin())->remove();