From 3e9691e29d3b5dfbcd8e7d3e4bcad59fd4b40d19 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Tue, 26 May 2020 13:48:53 +0200
Subject: [PATCH] Add problem::getTimeStamp()
---
include/core/problem/problem.h | 1 +
src/problem/problem.cpp | 19 +++++++++++++++++++
2 files changed, 20 insertions(+)
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 455d3b166..8f5bb0e50 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -312,6 +312,7 @@ class Problem : public std::enable_shared_from_this<Problem>
// State getters
VectorComposite getState ( ) const;
VectorComposite getState (const TimeStamp& _ts ) const;
+ TimeStamp getTimeStamp ( ) const;
// Zero state provider
Eigen::VectorXd zeroState ( ) const;
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 995d32238..d437e313b 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -415,6 +415,25 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
// _state = zeroState();
//}
+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
+ {
+ TimeStamp ts(0);
+ for (const auto& prc : processor_is_motion_list_)
+ if (prc->getTimeStamp() > ts)
+ ts = prc->getTimeStamp();
+ return ts;
+ }
+}
VectorComposite Problem::getState() const
{
--
GitLab