diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index d4a8662ddfb828e296151af514a45c17f2be97d9..2340cd8fc16c7da8db8767298b2337ba46a666ac 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -136,11 +136,6 @@ class ProcessorMotion : public ProcessorBase, public IsMotion - // TODO move somewhere below - virtual bool getStateComposite(VectorComposite& _state) const override; - virtual bool getStateComposite(const TimeStamp& _ts, VectorComposite& _state) const override; - using IsMotion::getStateComposite; - @@ -174,6 +169,11 @@ class ProcessorMotion : public ProcessorBase, public IsMotion // Queries to the processor: + virtual bool getVectorComposite(VectorComposite& _state) const override; + virtual bool getVectorComposite(const TimeStamp& _ts, VectorComposite& _state) const override; + using IsMotion::getVectorComposite; + + /** \brief Fill a reference to the state integrated so far * \param _x the returned state vector */ diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 4b34d54300e07c7c638d03fc77aefa6e8cbeb7cc..b31ae1e1b94dd7a33684684986c844fc0d09dc9f 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -398,16 +398,21 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const return true; } -bool ProcessorMotion::getStateComposite(VectorComposite& _state) const +bool ProcessorMotion::getVectorComposite(VectorComposite& _state) const { // TODO implement this if (last_ptr_ && last_ptr_->getFrame()) - return getLast()->getFrame()->getVectorComposite(_state); + return last_ptr_->getFrame()->getVectorComposite(_state); + else - return false; + if (origin_ptr_ && origin_ptr_->getFrame()) + return origin_ptr_->getFrame()->getVectorComposite(_state); + + else + return false; } -bool ProcessorMotion::getStateComposite(const TimeStamp& _ts, VectorComposite& _state) const +bool ProcessorMotion::getVectorComposite(const TimeStamp& _ts, VectorComposite& _state) const { // TODO implement this if (last_ptr_ && last_ptr_->getFrame())