diff --git a/include/iri_base_algorithm/iri_base_algorithm.h b/include/iri_base_algorithm/iri_base_algorithm.h index 5c02b06f02594a3ac04d279e409bea4ba18a27dc..84967adcbe2784a91935626fd5b5e97053b4391c 100644 --- a/include/iri_base_algorithm/iri_base_algorithm.h +++ b/include/iri_base_algorithm/iri_base_algorithm.h @@ -110,16 +110,6 @@ class IriBaseAlgorithm */ ros::NodeHandle private_node_handle_; - /** - * \brief main thread loop rate - * - * This value determines the loop frequency of the node main thread function - * mainThread() in HZ. It is initialized at construction time. This variable - * may be modified in the node implementation constructor if a desired - * frequency is required. - */ - ros::Rate loop_rate_; - /** * \brief default main thread frequency * @@ -179,7 +169,31 @@ class IriBaseAlgorithm */ dynamic_reconfigure::Server<Config> dsrv_; + /** + * \brief main thread loop rate + * + * This value determines the loop frequency of the node main thread function + * mainThread() in HZ. It is initialized at construction time. This variable + * may be modified in the node implementation constructor if a desired + * frequency is required. + */ + ros::Rate loop_rate_; + protected: + + /** + * \brief + * + */ + void setRate(double rate_hz); + + + /** + * \brief + * + */ + double getRate(void); + /** * \brief dynamic reconfigure server callback * @@ -273,6 +287,19 @@ IriBaseAlgorithm<Algorithm>::~IriBaseAlgorithm() pthread_join(this->thread,NULL); } +template <class Algorithm> +void IriBaseAlgorithm<Algorithm>::setRate(double rate_hz) +{ + this->loop_rate_=rate_hz; +} + +template <class Algorithm> +double IriBaseAlgorithm<Algorithm>::getRate(void) +{ + return this->loop_rate_; +} + + template <class Algorithm> void IriBaseAlgorithm<Algorithm>::reconfigureCallback(Config &config, uint32_t level) {