add mutex to ProcessorBase and use it in ProcessorMotion::keyFrameCallback

src/processor_base.h

@@ -40,6 +40,9 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
         static unsigned int processor_id_count_;
 
     public:
+
+        using mutex_t = std::recursive_timed_mutex;
+
         ProcessorBase(const std::string& _type, const Scalar& _time_tolerance = 0);
         virtual ~ProcessorBase();
         void remove();
@@ -92,6 +95,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
     protected:
         unsigned int processor_id_;
         Scalar time_tolerance_;         ///< self time tolerance for adding a capture into a frame
+
+        mutex_t mut_;
 };
 
 }

src/processor_motion.cpp

@@ -41,6 +41,8 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
     return;
   }
 
+  std::lock_guard<mutex_t> lock(mut_);
+
     if (status_ == IDLE)
     {
 //        std::cout << "PM: IDLE" << std::endl;
@@ -228,6 +230,18 @@ bool ProcessorMotion::keyFrameCallback(FrameBasePtr _new_keyframe, const Scalar&
     assert(_new_keyframe->getTrajectoryPtr() != nullptr
             && "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory.");
 
+    using rep_t = std::chrono::milliseconds::rep;
+
+    // Tying to self-lock the mutex for duration == _time_tol_other
+    if (!mut_.try_lock_for(std::chrono::milliseconds(rep_t(_time_tol_other))))
+    {
+      WOLF_DEBUG("ProcessorMotion::keyFrameCallback could not acquire the mutex on time.");
+      return false;
+    }
+
+    // Lock object takes ownership of the mutex
+    std::lock_guard<mutex_t> lock(mut_, std::adopt_lock);
+
     // get keyframe's time stamp
     TimeStamp new_ts = _new_keyframe->getTimeStamp();