Add processor dim & problem dim compatibility check

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)
 {

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());

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);

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>());