diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index e3c334661465ef9d4af07880bca10c7f0861a540..86bef1d6527d4f714cd455d0dc484771cd3faec0 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 91f2545b973c5eb26d75fb389dd2d239282b9e3d..5be1c32af58dc831d6c0cbaf1b549a4b3d33ad26 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);