diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index 410b6dd9e23ea202df035e4d9ce4f5a1e5ac0acd..e749ec2140bad24d93c5cccf0fb25c91f7698346 100644
--- a/include/core/processor/is_motion.h
+++ b/include/core/processor/is_motion.h
@@ -24,7 +24,6 @@ class IsMotion
virtual ~IsMotion();
// Queries to the processor:
-// virtual bool isMotion() const {return true;}
/** \brief Fill a reference to the state integrated so far
* \param _x the returned state vector
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index a70ea3944185b92db0f8051137b6a59b0b606eb8..bd39ef49da82605edd6f48bb27337c9823ef8ed9 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -339,7 +339,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
public:
- virtual bool isMotion() const;
+ bool isMotion() const;
void setTimeTolerance(double _time_tolerance);
@@ -377,7 +377,8 @@ inline void ProcessorBase::setVotingAuxActive(bool _voting_active)
inline bool ProcessorBase::isMotion() const
{
- return false;
+ // check if this inherits from IsMotion
+ return (std::dynamic_pointer_cast<const IsMotion>(shared_from_this()) != nullptr);
}
inline unsigned int ProcessorBase::id() const
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 6e56fd99ba45bd203f88729b4f8d9b043b2b1b7c..4beca1b195d9f718405629a2c1f6a52c7e30cc61 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -160,7 +160,6 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
virtual void resetDerived();
// Queries to the processor:
- virtual bool isMotion() const override;
/** \brief Fill a reference to the state integrated so far
* \param _x the returned state vector
@@ -557,11 +556,6 @@ inline Motion ProcessorMotion::getMotion(const TimeStamp& _ts) const
return capture_ptr->getBuffer().getMotion(_ts);
}
-inline bool ProcessorMotion::isMotion() const
-{
- return true;
-}
-
inline double ProcessorMotion::updateDt()
{
return dt_ = incoming_ptr_->getTimeStamp() - getBuffer().get().back().ts_;
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index e5e61d3f097779e48a179a37b51c7be5b87038c8..ed07d3de55a3ec8e4b9181cb8c9b3af85ba4f1cb 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -110,9 +110,6 @@ class ProcessorTracker : public ProcessorBase
ProcessorParamsTrackerPtr _params_tracker);
virtual ~ProcessorTracker();
- virtual bool isMotion() const final override {return false; }
-
-
bool checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2);
bool checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts);
bool checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts);
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 1afab86dfb1267f1007e41de62e060f4e6f705a7..f3abb13a7d936b944be15eadf9f245a69b80ecd5 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -229,7 +229,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
// setting the main processor motion
if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
- processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr);
+ processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(prc_ptr);
return prc_ptr;
}
@@ -307,7 +307,7 @@ IsMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
if ((*prc_it)->isMotion()) // found, and it's motion!
{
std::cout << "Found processor '" << _processor_name << "', of type Motion, and set as the main motion processor." << std::endl;
- processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(*prc_it);
+ processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(*prc_it);
return processor_motion_ptr_;
}
else // found, but it's not motion!
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 17bc8fffa32576e34184279a5f8d0d70dd8747d3..3cf7743c4e673dc358f076db4dbc56376a71f46e 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -71,7 +71,7 @@ void ProcessorBase::remove()
if (isMotion())
{
ProblemPtr P = getProblem();
- if(P && this->isMotion() && P->getProcessorMotion() == std::dynamic_pointer_cast<IsMotion>(shared_from_this()))
+ if(P && (P->getProcessorMotion() == std::dynamic_pointer_cast<IsMotion>( shared_from_this() ) ) )
P->clearProcessorMotion();
}
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 2aef08f59cfcb594a34dce910a9f7f54eb389671..4d3dfffbf09b9d1582c330b1d3d37f7067fc8d3c 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -31,6 +31,41 @@ WOLF_REGISTER_PROCESSOR("TRACKER FEATURE DUMMY", ProcessorTrackerFeatureDummy)
} // namespace wolf
+TEST(ProcessorBase, IsMotion)
+{
+ using namespace wolf;
+ using std::shared_ptr;
+ using std::make_shared;
+ using std::static_pointer_cast;
+ using Eigen::Vector2d;
+
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ double dt = 0.01;
+
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ // Install tracker (sensor and processor)
+ auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ "SensorDummy",
+ nullptr,
+ nullptr,
+ nullptr,
+ 2);
+ auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", "dummy", sens_trk);
+
+ // Install odometer (sensor and processor)
+ SensorBasePtr sens_odo = problem->installSensor("SensorOdom2D", "odometer", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
+ ProcessorParamsOdom2DPtr proc_odo_params = make_shared<ProcessorParamsOdom2D>();
+ proc_odo_params->time_tolerance = dt/2;
+ ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2D", "odom processor", sens_odo, proc_odo_params);
+
+ ASSERT_FALSE(proc_trk->isMotion());
+ ASSERT_TRUE (proc_odo->isMotion());
+}
+
+
TEST(ProcessorBase, KeyFrameCallback)
{