From f10a55d1ea9f2b71c330b65ab55788e7993072f1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sat, 11 Apr 2020 00:18:54 +0200
Subject: [PATCH] Add composite API to problem
---
include/core/problem/problem.h | 8 ++++----
src/problem/problem.cpp | 9 ++++-----
2 files changed, 8 insertions(+), 9 deletions(-)
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index e3c334661..86bef1d65 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -44,10 +44,10 @@ class Problem : public std::enable_shared_from_this<Problem>
// TODO move somewhere below
public:
- bool getStateComposite(VectorComposite& _state) const;
- bool getStateComposite(const TimeStamp& _ts, VectorComposite& _state) const;
- VectorComposite getStateComposite() const;
- VectorComposite getStateComposite(const TimeStamp& _ts) const;
+ bool getVectorComposite(VectorComposite& _state) const;
+ bool getVectorComposite(const TimeStamp& _ts, VectorComposite& _state) const;
+ VectorComposite getVectorComposite() const;
+ VectorComposite getVectorComposite(const TimeStamp& _ts) const;
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 91f2545b9..5be1c32af 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -376,7 +376,7 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) c
_state = zeroState();
}
-inline bool Problem::getStateComposite(VectorComposite& _state) const
+inline bool Problem::getVectorComposite(VectorComposite& _state) const
{
if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
{
@@ -389,17 +389,16 @@ inline bool Problem::getStateComposite(VectorComposite& _state) const
else // Compose from different processor motion
{
// get a timestamp where all processor motions have state
- std::list<TimeStamp> proc_motion_ts;
+ TimeStamp ts_min(1e99);
for (const auto& prc : processor_is_motion_list_)
{
- proc_motion_ts.push_back(prc->getCurrentTimeStamp());
+ ts_min = std::min(ts_min, prc->getCurrentTimeStamp());
}
- auto min_ts = *(std::min_element(proc_motion_ts.begin(), proc_motion_ts.end()));
// compose the states of all processor motions into one only state
for (const auto& prc : processor_is_motion_list_)
{
- for (const auto& pair_key_vec : prc->getStateComposite(min_ts))
+ for (const auto& pair_key_vec : prc->getStateComposite(ts_min))
{
if (_state.count(pair_key_vec.first) == 0)
_state.insert(pair_key_vec);
--
GitLab