From c045ec7b076c2280df23c8496dd563eddde672da Mon Sep 17 00:00:00 2001 From: jcasals <jcasals@iri.upc.edu> Date: Tue, 24 Mar 2020 16:29:36 +0100 Subject: [PATCH] Add processor dim & problem dim compatibility check --- include/core/processor/processor_base.h | 5 ++++- src/processor/processor_base.cpp | 7 +++++++ src/processor/processor_motion.cpp | 8 +++++++- test/gtest_emplace.cpp | 2 +- 4 files changed, 19 insertions(+), 3 deletions(-) diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index 55c74707c..d0a250aa1 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -261,6 +261,10 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce unsigned int id() const; protected: + /** + * \brief set the pointer to problem + */ + virtual void setProblem(ProblemPtr) override; /** \brief process an incoming capture * * Each derived processor should implement this function. It will be called if: @@ -397,7 +401,6 @@ inline void ProcessorBase::setTimeTolerance(double _time_tolerance) params_->time_tolerance = _time_tolerance; } - template<typename classType, typename... T> std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all) { diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index a35097f2c..954531df6 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -22,6 +22,13 @@ ProcessorBase::~ProcessorBase() // WOLF_DEBUG("destructed -p" , id()); } +void ProcessorBase::setProblem(ProblemPtr _problem) +{ + std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D"; + assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str()); + + NodeBase::setProblem(_problem); +} bool ProcessorBase::permittedKeyFrame() { return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this()); diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 49bbbbf41..1818c2fbb 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -621,7 +621,13 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep() void ProcessorMotion::setProblem(ProblemPtr _problem) { - if (_problem == nullptr || _problem == this->getProblem()) + if (_problem == nullptr) + return; + + std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D"; + assert(dim_ == 0 or dim_ == _problem->getDim() && str.c_str()); + + if (_problem == this->getProblem()) return; NodeBase::setProblem(_problem); diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp index dd2965da5..e60531daf 100644 --- a/test/gtest_emplace.cpp +++ b/test/gtest_emplace.cpp @@ -48,7 +48,7 @@ TEST(Emplace, Frame) TEST(Emplace, Processor) { - ProblemPtr P = Problem::create("POV", 3); + ProblemPtr P = Problem::create("PO", 2); auto sen = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false); auto prc = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen, std::make_shared<ProcessorParamsOdom2d>()); -- GitLab