Redesign Prb::getTimeStamp()

src/problem/problem.cpp

@@ -419,27 +419,27 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
 
 TimeStamp Problem::getTimeStamp ( ) const
 {
-    if (not processor_is_motion_list_.empty() ) // Use most recent Motion processor data
-    {
-        TimeStamp  ts(0);
-        for (const auto& prc : processor_is_motion_list_)
-            if (prc->getTimeStamp() > ts)
+    TimeStamp  ts = TimeStamp::Invalid();
+
+    for (const auto& prc : processor_is_motion_list_)
+        if (prc->getTimeStamp().ok())
+            if ( (not ts.ok() ) or prc->getTimeStamp() > ts)
                 ts = prc->getTimeStamp();
-        return ts;
-    }
-    else
+
+
+    if (not ts.ok())
     {
         const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
 
         if (last_kf_or_aux)
-            return last_kf_or_aux->getTimeStamp(); // Use last estimated frame's state
+            ts = last_kf_or_aux->getTimeStamp(); // Use last estimated frame's state
 
-        else
-        {
-            WOLF_WARN( "Problem has no Keyframe so no timestamp can be obtained!");
-            return TimeStamp::Invalid();
-        }
     }
+
+    if (not ts.ok())
+        WOLF_WARN( "Problem has nowhere to find a valid timestamp!");
+
+    return ts;
 }