diff --git a/include/mrs_lib/impl/timer.hpp b/include/mrs_lib/impl/timer.hpp index ca383561..0708ff92 100644 --- a/include/mrs_lib/impl/timer.hpp +++ b/include/mrs_lib/impl/timer.hpp @@ -37,6 +37,7 @@ class ThreadTimer::Impl { void start(); void stop(); void setPeriod(const ros::Duration& duration, const bool reset = true); + void setCallback(const std::function& callback); friend class ThreadTimer; diff --git a/include/mrs_lib/timer.h b/include/mrs_lib/timer.h index a782666e..3b06b086 100644 --- a/include/mrs_lib/timer.h +++ b/include/mrs_lib/timer.h @@ -43,6 +43,15 @@ namespace mrs_lib */ virtual void setPeriod(const ros::Duration& duration, const bool reset = true) = 0; + /** + * @brief change the callback method + * + * Usable e.g. for running thread with a specific parameter if you bind it using std::bind + * + * @param callback callback method to be called. + */ + virtual void setCallback(const std::function& callback) = 0; + /** * @brief returns true if callbacks should be called * @@ -208,6 +217,15 @@ namespace mrs_lib * @param reset ignored in this implementation. */ virtual void setPeriod(const ros::Duration& duration, const bool reset = true) override; + + /** + * @brief change the callback method + * + * Usable e.g. for running thread with a specific parameter if you bind it using std::bind + * + * @param callback callback method to be called. + */ + virtual void setCallback(const std::function& callback) override; /** * @brief returns true if callbacks should be called diff --git a/src/timer/timer.cpp b/src/timer/timer.cpp index 4d1889f0..6f774adb 100644 --- a/src/timer/timer.cpp +++ b/src/timer/timer.cpp @@ -93,6 +93,16 @@ void ThreadTimer::setPeriod(const ros::Duration& duration, [[maybe_unused]] cons //} +/* ThreadTimer::setCallback() //{ */ + +void ThreadTimer::setCallback(const std::function& callback) { + if (impl_) { + impl_->setCallback(callback); + } +} + +//} + /* ThreadTimer::running() //{ */ bool ThreadTimer::running() @@ -159,6 +169,12 @@ void ThreadTimer::Impl::setPeriod(const ros::Duration& duration, [[maybe_unused] this->oneshot_ = true; } +void ThreadTimer::Impl::setCallback(const std::function& callback){ + std::scoped_lock lock(mutex_state_); + + callback_ = callback; +} + //} /* ThreadTimer::breakableSleep() method //{ */