diff --git a/src/processor_base.h b/src/processor_base.h index f87df2394d54ad7abf44637d971fa550b857968a..97c730db7728d510ec2c102e87c69ec1646aecfb 100644 --- a/src/processor_base.h +++ b/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_; }; } diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp index 227f476e0ecbacb3d20c2f248c120766e0014aab..f4dd03469f11e00cfddcd44b8bb2d46c7b9dc2e6 100644 --- a/src/processor_motion.cpp +++ b/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 (!timed_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(timed_mut_, std::adopt_lock); + // get keyframe's time stamp TimeStamp new_ts = _new_keyframe->getTimeStamp();