diff --git a/src/problem.cpp b/src/problem.cpp index 7dbe0b736b93720437f8b126612f261be38d7320..5e646aef19ee5e8f2e91c9fd47343c142f81f043 100644 --- a/src/problem.cpp +++ b/src/problem.cpp @@ -286,10 +286,8 @@ void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& state) { assert(state.size() == getFrameStructureSize() && "Problem::getStateAtTimeStamp: bad state size"); - if (processor_motion_ptr_ != nullptr) - processor_motion_ptr_->getState(_ts, state); - - else + // try to get the state from processor_motion if any, otherwise... + if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, state)) { FrameBasePtr closest_frame = trajectory_ptr_->closestKeyFrameToTimeStamp(_ts); if (closest_frame != nullptr) diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp index a89aa763c329051a347eda4c7530f67ad673f5f0..d15a942a738fd17dc62c75e12c2fba8d6f7e10ec 100644 --- a/src/processor_motion.cpp +++ b/src/processor_motion.cpp @@ -257,7 +257,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr) } -void ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x) +bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x) { CaptureMotionPtr capture_motion; if (origin_ptr_ && _ts >= origin_ptr_->getTimeStamp()) @@ -291,8 +291,9 @@ void ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x) // We could not find any CaptureMotion for the time stamp requested WOLF_ERROR("Could not find any Capture for the time stamp requested. "); WOLF_TRACE("Did you forget to call Problem::setPrior() in your application?") - throw std::runtime_error("Could not find any Capture for the time stamp requested. Did you forget to call Problem::setPrior() in your application?"); + return false; } + return true; } //CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const diff --git a/src/processor_motion.h b/src/processor_motion.h index a93a4bfb66191d61df34f3caafa3a8d95f2aea89..7087ef593e06106bd1ac8c2dcf21b470420a87d2 100644 --- a/src/processor_motion.h +++ b/src/processor_motion.h @@ -157,8 +157,9 @@ class ProcessorMotion : public ProcessorBase /** \brief Fill the state corresponding to the provided time-stamp * \param _ts the time stamp * \param _x the returned state + * \return if state in the provided time-stamp could be resolved */ - void getState(const TimeStamp& _ts, Eigen::VectorXs& _x); + bool getState(const TimeStamp& _ts, Eigen::VectorXs& _x); /** \brief Get the state corresponding to the provided time-stamp * \param _ts the time stamp