From 8c213bca580f24b8d1b0495c935636b4be023c0e Mon Sep 17 00:00:00 2001 From: fherrero <fherrero@iri.upc.edu> Date: Mon, 4 May 2020 16:27:41 +0200 Subject: [PATCH] Fix get/setRate functions using ros::Rate API correctly --- include/iri_base_algorithm/iri_base_algorithm.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/include/iri_base_algorithm/iri_base_algorithm.h b/include/iri_base_algorithm/iri_base_algorithm.h index 84967ad..afde0e8 100644 --- a/include/iri_base_algorithm/iri_base_algorithm.h +++ b/include/iri_base_algorithm/iri_base_algorithm.h @@ -290,16 +290,15 @@ IriBaseAlgorithm<Algorithm>::~IriBaseAlgorithm() template <class Algorithm> void IriBaseAlgorithm<Algorithm>::setRate(double rate_hz) { - this->loop_rate_=rate_hz; + this->loop_rate_=ros::Rate(rate_hz); } template <class Algorithm> double IriBaseAlgorithm<Algorithm>::getRate(void) { - return this->loop_rate_; + return 1.0/this->loop_rate_.expectedCycleTime().toSec(); } - template <class Algorithm> void IriBaseAlgorithm<Algorithm>::reconfigureCallback(Config &config, uint32_t level) { -- GitLab