From b8f54c3e0ff9a50ddad97459e606ec7a3227128b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 12 Jun 2020 09:56:08 +0200
Subject: [PATCH] Change order in Prb::getTimeStamp()

---
 src/problem/problem.cpp | 23 ++++++++++++++---------
 1 file changed, 14 insertions(+), 9 deletions(-)

diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 4d2462f72..74ea27dc4 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -419,15 +419,7 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
 
 TimeStamp Problem::getTimeStamp ( ) const
 {
-    if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
-    {
-        auto last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
-
-        assert(last_kf_or_aux != nullptr && "Problem has no Keyframe so no timestamp can be obtained!");
-
-        return last_kf_or_aux->getTimeStamp();
-    }
-    else
+    if (not processor_is_motion_list_.empty() ) // Use most recent Motion processor data
     {
         TimeStamp  ts(0);
         for (const auto& prc : processor_is_motion_list_)
@@ -435,6 +427,19 @@ TimeStamp Problem::getTimeStamp ( ) const
                 ts = prc->getTimeStamp();
         return ts;
     }
+    else
+    {
+        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
+
+        else
+        {
+            WOLF_WARN( "Problem has no Keyframe so no timestamp can be obtained!");
+            return TimeStamp::Invalid();
+        }
+    }
 }
 
 
-- 
GitLab