From 68de2285899c4aa75846678d830b60c135fc3b8e Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Fri, 10 Mar 2023 10:37:28 +0300 Subject: [PATCH 01/18] Make Rate to select the clock to work with Add ROSRate respective with ROS time Signed-off-by: Alexey Merzlyakov --- rclcpp/include/rclcpp/rate.hpp | 83 +++++++++++++++++++++------------- 1 file changed, 52 insertions(+), 31 deletions(-) diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 55d3bbcb85..9be5c2c781 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -15,10 +15,10 @@ #ifndef RCLCPP__RATE_HPP_ #define RCLCPP__RATE_HPP_ -#include #include -#include +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" @@ -33,33 +33,32 @@ class RateBase virtual ~RateBase() {} virtual bool sleep() = 0; - virtual bool is_steady() const = 0; + virtual rcl_clock_type_t get_type() const = 0; virtual void reset() = 0; }; -using std::chrono::duration; -using std::chrono::duration_cast; -using std::chrono::nanoseconds; - -template -class GenericRate : public RateBase +class Rate : public RateBase { public: - RCLCPP_SMART_PTR_DEFINITIONS(GenericRate) + RCLCPP_SMART_PTR_DEFINITIONS(Rate) - explicit GenericRate(double rate) - : GenericRate( - duration_cast(duration(1.0 / rate))) + explicit Rate( + const double rate, + Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME)) + : Rate( + Duration::from_seconds(1.0 / rate), clock) {} - explicit GenericRate(std::chrono::nanoseconds period) - : period_(period), last_interval_(Clock::now()) + explicit Rate( + const Duration & period, + Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME)) + : clock_(clock), period_(period), last_interval_(clock_->now()) {} virtual bool sleep() { // Time coming into sleep - auto now = Clock::now(); + auto now = clock_->now(); // Time of next interval auto next_interval = last_interval_ + period_; // Detect backwards time flow @@ -67,12 +66,10 @@ class GenericRate : public RateBase // Best thing to do is to set the next_interval to now + period next_interval = now + period_; } - // Calculate the time to sleep - auto time_to_sleep = next_interval - now; // Update the interval last_interval_ += period_; // If the time_to_sleep is negative or zero, don't sleep - if (time_to_sleep <= std::chrono::seconds(0)) { + if (next_interval <= now) { // If an entire cycle was missed then reset next interval. // This might happen if the loop took more than a cycle. // Or if time jumps forward. @@ -82,38 +79,62 @@ class GenericRate : public RateBase // Either way do not sleep and return false return false; } + // Calculate the time to sleep + auto time_to_sleep = next_interval - now; // Sleep (will get interrupted by ctrl-c, may not sleep full time) - rclcpp::sleep_for(time_to_sleep); + clock_->sleep_for(time_to_sleep); return true; } - virtual bool - is_steady() const + virtual rcl_clock_type_t + get_type() const { - return Clock::is_steady; + return clock_->get_clock_type(); } virtual void reset() { - last_interval_ = Clock::now(); + last_interval_ = clock_->now(); } - std::chrono::nanoseconds period() const + Duration period() const { return period_; } private: - RCLCPP_DISABLE_COPY(GenericRate) + RCLCPP_DISABLE_COPY(Rate) + + Clock::SharedPtr clock_; + Duration period_; + Time last_interval_; +}; + +class WallRate : public Rate +{ +public: + explicit WallRate(const double rate) + : Rate(rate, std::make_shared(RCL_STEADY_TIME)) + {} - std::chrono::nanoseconds period_; - using ClockDurationNano = std::chrono::duration; - std::chrono::time_point last_interval_; + explicit WallRate(const Duration & period) + : Rate(period, std::make_shared(RCL_STEADY_TIME)) + {} }; -using Rate = GenericRate; -using WallRate = GenericRate; +class ROSRate : public Rate + +{ +public: + explicit ROSRate(const double rate) + : Rate(rate, std::make_shared(RCL_ROS_TIME)) + {} + + explicit ROSRate(const Duration & period) + : Rate(period, std::make_shared(RCL_ROS_TIME)) + {} +}; } // namespace rclcpp From 88e6d05351839d63bb6f5ad90ce34db2dc45acdc Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Mon, 13 Mar 2023 18:37:11 +0300 Subject: [PATCH 02/18] Make GenericRate class to be deprecated Signed-off-by: Alexey Merzlyakov --- rclcpp/include/rclcpp/rate.hpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 9be5c2c781..1e1d052491 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__RATE_HPP_ #define RCLCPP__RATE_HPP_ +#include #include #include "rclcpp/clock.hpp" @@ -37,6 +38,20 @@ class RateBase virtual void reset() = 0; }; +template +class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRate : public RateBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(GenericRate) + + explicit GenericRate(double) {} + explicit GenericRate(std::chrono::nanoseconds) {} + + virtual bool sleep() {return false;} + virtual rcl_clock_type_t get_type() const {return RCL_CLOCK_UNINITIALIZED;} + virtual void reset() {} +}; + class Rate : public RateBase { public: @@ -124,7 +139,6 @@ class WallRate : public Rate }; class ROSRate : public Rate - { public: explicit ROSRate(const double rate) From 423795668535271532bc7206250b19ffdf28a8bd Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Wed, 15 Mar 2023 17:22:52 +0300 Subject: [PATCH 03/18] Adjust test cases for new rates Signed-off-by: Alexey Merzlyakov --- rclcpp/test/rclcpp/test_rate.cpp | 137 ++++++++++++++++++++++++++++--- 1 file changed, 127 insertions(+), 10 deletions(-) diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index d6608d59f6..eaa430ccc3 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -22,6 +22,8 @@ Basic tests for the Rate and WallRate classes. */ TEST(TestRate, rate_basics) { + rclcpp::init(0, nullptr); + auto period = std::chrono::milliseconds(1000); auto offset = std::chrono::milliseconds(500); auto epsilon = std::chrono::milliseconds(100); @@ -29,8 +31,8 @@ TEST(TestRate, rate_basics) { auto start = std::chrono::system_clock::now(); rclcpp::Rate r(period); - EXPECT_EQ(period, r.period()); - ASSERT_FALSE(r.is_steady()); + EXPECT_EQ(rclcpp::Duration(period), r.period()); + ASSERT_EQ(RCL_SYSTEM_TIME, r.get_type()); ASSERT_TRUE(r.sleep()); auto one = std::chrono::system_clock::now(); auto delta = one - start; @@ -59,9 +61,13 @@ TEST(TestRate, rate_basics) { auto five = std::chrono::system_clock::now(); delta = five - four; ASSERT_TRUE(epsilon > delta); + + rclcpp::shutdown(); } TEST(TestRate, wall_rate_basics) { + rclcpp::init(0, nullptr); + auto period = std::chrono::milliseconds(100); auto offset = std::chrono::milliseconds(50); auto epsilon = std::chrono::milliseconds(1); @@ -69,8 +75,8 @@ TEST(TestRate, wall_rate_basics) { auto start = std::chrono::steady_clock::now(); rclcpp::WallRate r(period); - EXPECT_EQ(period, r.period()); - ASSERT_TRUE(r.is_steady()); + EXPECT_EQ(rclcpp::Duration(period), r.period()); + ASSERT_EQ(RCL_STEADY_TIME, r.get_type()); ASSERT_TRUE(r.sleep()); auto one = std::chrono::steady_clock::now(); auto delta = one - start; @@ -99,23 +105,134 @@ TEST(TestRate, wall_rate_basics) { auto five = std::chrono::steady_clock::now(); delta = five - four; EXPECT_GT(epsilon, delta); + + rclcpp::shutdown(); +} + +TEST(TestRate, ros_rate_basics) { + rclcpp::init(0, nullptr); + + auto period = std::chrono::milliseconds(100); + auto offset = std::chrono::milliseconds(50); + auto epsilon = std::chrono::milliseconds(1); + double overrun_ratio = 1.5; + + rclcpp::Clock clock(RCL_ROS_TIME); + + auto start = clock.now(); + rclcpp::ROSRate r(period); + EXPECT_EQ(rclcpp::Duration(period), r.period()); + ASSERT_EQ(RCL_ROS_TIME, r.get_type()); + ASSERT_TRUE(r.sleep()); + auto one = clock.now(); + auto delta = one - start; + EXPECT_LT(rclcpp::Duration(period), delta); + EXPECT_GT(rclcpp::Duration(period * overrun_ratio), delta); + + clock.sleep_for(offset); + ASSERT_TRUE(r.sleep()); + auto two = clock.now(); + delta = two - start; + EXPECT_LT(rclcpp::Duration(2 * period), delta + epsilon); + EXPECT_GT(rclcpp::Duration(2 * period * overrun_ratio), delta); + + clock.sleep_for(offset); + auto two_offset = clock.now(); + r.reset(); + ASSERT_TRUE(r.sleep()); + auto three = clock.now(); + delta = three - two_offset; + EXPECT_LT(rclcpp::Duration(period), delta); + EXPECT_GT(rclcpp::Duration(period * overrun_ratio), delta); + + clock.sleep_for(offset + period); + auto four = clock.now(); + ASSERT_FALSE(r.sleep()); + auto five = clock.now(); + delta = five - four; + EXPECT_GT(rclcpp::Duration(epsilon), delta); + + rclcpp::shutdown(); } TEST(TestRate, from_double) { { - rclcpp::WallRate rate(1.0); - EXPECT_EQ(std::chrono::seconds(1), rate.period()); + rclcpp::Rate rate(1.0); + EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(1000)), rate.period()); } { rclcpp::WallRate rate(2.0); - EXPECT_EQ(std::chrono::milliseconds(500), rate.period()); + EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(500)), rate.period()); } { rclcpp::WallRate rate(0.5); - EXPECT_EQ(std::chrono::seconds(2), rate.period()); + EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(2000)), rate.period()); + } + { + rclcpp::ROSRate rate(4.0); + EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(250)), rate.period()); + } +} + +TEST(TestRate, clock_types) { + { + rclcpp::Rate rate(1.0, std::make_shared(RCL_SYSTEM_TIME)); + EXPECT_EQ(RCL_SYSTEM_TIME, rate.get_type()); + } + { + rclcpp::Rate rate(1.0, std::make_shared(RCL_STEADY_TIME)); + EXPECT_EQ(RCL_STEADY_TIME, rate.get_type()); } { - rclcpp::WallRate rate(4.0); - EXPECT_EQ(std::chrono::milliseconds(250), rate.period()); + rclcpp::Rate rate(1.0, std::make_shared(RCL_ROS_TIME)); + EXPECT_EQ(RCL_ROS_TIME, rate.get_type()); + } +} + +TEST(TestRate, generic_rate_api) { + { +// suppress deprecated function warning +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + + rclcpp::GenericRate gr(1.0); + +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + + ASSERT_FALSE(gr.sleep()); + ASSERT_EQ(RCL_CLOCK_UNINITIALIZED, gr.get_type()); + } + + { +// suppress deprecated function warning +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + + rclcpp::GenericRate gr(std::chrono::seconds(1)); + +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + + ASSERT_FALSE(gr.sleep()); + ASSERT_EQ(RCL_CLOCK_UNINITIALIZED, gr.get_type()); } } From ac35e54ca5a428fa8ee59eb8866b6da5c2234566 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Wed, 15 Mar 2023 18:18:20 +0300 Subject: [PATCH 04/18] Fix GenericRate API to remain the same Signed-off-by: Alexey Merzlyakov --- rclcpp/include/rclcpp/rate.hpp | 6 ++++-- rclcpp/test/rclcpp/test_rate.cpp | 6 ++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 1e1d052491..4b27a1715e 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -39,17 +39,19 @@ class RateBase }; template -class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRate : public RateBase +class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRate { public: RCLCPP_SMART_PTR_DEFINITIONS(GenericRate) explicit GenericRate(double) {} explicit GenericRate(std::chrono::nanoseconds) {} + virtual ~GenericRate() {} virtual bool sleep() {return false;} - virtual rcl_clock_type_t get_type() const {return RCL_CLOCK_UNINITIALIZED;} + virtual bool is_steady() const {return false;} virtual void reset() {} + std::chrono::nanoseconds period() const {return std::chrono::seconds(1);} }; class Rate : public RateBase diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index eaa430ccc3..a976cda81e 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -210,7 +210,8 @@ TEST(TestRate, generic_rate_api) { #endif ASSERT_FALSE(gr.sleep()); - ASSERT_EQ(RCL_CLOCK_UNINITIALIZED, gr.get_type()); + ASSERT_FALSE(gr.is_steady()); + ASSERT_EQ(std::chrono::seconds(1), gr.period()); } { @@ -233,6 +234,7 @@ TEST(TestRate, generic_rate_api) { #endif ASSERT_FALSE(gr.sleep()); - ASSERT_EQ(RCL_CLOCK_UNINITIALIZED, gr.get_type()); + ASSERT_FALSE(gr.is_steady()); + ASSERT_EQ(std::chrono::seconds(1), gr.period()); } } From 76d7c44a27c6e2d82f2dd6295a9bb3750f1f1418 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Thu, 13 Apr 2023 13:24:58 +0300 Subject: [PATCH 05/18] Return back GenericRate logic Correct Rate::sleep() return value Signed-off-by: Alexey Merzlyakov --- rclcpp/include/rclcpp/rate.hpp | 83 +++++++++++++++-- rclcpp/test/rclcpp/test_rate.cpp | 152 +++++++++++++++++++++++-------- 2 files changed, 187 insertions(+), 48 deletions(-) diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 4b27a1715e..eee1944768 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" @@ -34,24 +35,85 @@ class RateBase virtual ~RateBase() {} virtual bool sleep() = 0; + virtual bool is_steady() const = 0; virtual rcl_clock_type_t get_type() const = 0; virtual void reset() = 0; }; +using std::chrono::duration; +using std::chrono::duration_cast; +using std::chrono::nanoseconds; + template class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRate { public: RCLCPP_SMART_PTR_DEFINITIONS(GenericRate) - explicit GenericRate(double) {} - explicit GenericRate(std::chrono::nanoseconds) {} + explicit GenericRate(double rate) + : period_(duration_cast(duration(1.0 / rate))), last_interval_(Clock::now()) + {} + explicit GenericRate(std::chrono::nanoseconds period) + : period_(period), last_interval_(Clock::now()) + {} + virtual ~GenericRate() {} - virtual bool sleep() {return false;} - virtual bool is_steady() const {return false;} - virtual void reset() {} - std::chrono::nanoseconds period() const {return std::chrono::seconds(1);} + virtual bool + sleep() + { + // Time coming into sleep + auto now = Clock::now(); + // Time of next interval + auto next_interval = last_interval_ + period_; + // Detect backwards time flow + if (now < last_interval_) { + // Best thing to do is to set the next_interval to now + period + next_interval = now + period_; + } + // Calculate the time to sleep + auto time_to_sleep = next_interval - now; + // Update the interval + last_interval_ += period_; + // If the time_to_sleep is negative or zero, don't sleep + if (time_to_sleep <= std::chrono::seconds(0)) { + // If an entire cycle was missed then reset next interval. + // This might happen if the loop took more than a cycle. + // Or if time jumps forward. + if (now > next_interval + period_) { + last_interval_ = now + period_; + } + // Either way do not sleep and return false + return false; + } + // Sleep (will get interrupted by ctrl-c, may not sleep full time) + rclcpp::sleep_for(time_to_sleep); + return true; + } + + virtual bool + is_steady() const + { + return Clock::is_steady; + } + + virtual void + reset() + { + last_interval_ = Clock::now(); + } + + std::chrono::nanoseconds period() const + { + return period_; + } + +private: + RCLCPP_DISABLE_COPY(GenericRate) + + std::chrono::nanoseconds period_; + using ClockDurationNano = std::chrono::duration; + std::chrono::time_point last_interval_; }; class Rate : public RateBase @@ -99,8 +161,13 @@ class Rate : public RateBase // Calculate the time to sleep auto time_to_sleep = next_interval - now; // Sleep (will get interrupted by ctrl-c, may not sleep full time) - clock_->sleep_for(time_to_sleep); - return true; + return clock_->sleep_for(time_to_sleep); + } + + virtual bool + is_steady() const + { + return clock_->get_clock_type() == RCL_STEADY_TIME; } virtual rcl_clock_type_t diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index a976cda81e..92ecedaa88 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -19,7 +19,7 @@ #include "rclcpp/rate.hpp" /* - Basic tests for the Rate and WallRate classes. + Basic tests for the Rate, WallRate and ROSRate classes. */ TEST(TestRate, rate_basics) { rclcpp::init(0, nullptr); @@ -32,6 +32,7 @@ TEST(TestRate, rate_basics) { auto start = std::chrono::system_clock::now(); rclcpp::Rate r(period); EXPECT_EQ(rclcpp::Duration(period), r.period()); + ASSERT_FALSE(r.is_steady()); ASSERT_EQ(RCL_SYSTEM_TIME, r.get_type()); ASSERT_TRUE(r.sleep()); auto one = std::chrono::system_clock::now(); @@ -76,6 +77,7 @@ TEST(TestRate, wall_rate_basics) { auto start = std::chrono::steady_clock::now(); rclcpp::WallRate r(period); EXPECT_EQ(rclcpp::Duration(period), r.period()); + ASSERT_TRUE(r.is_steady()); ASSERT_EQ(RCL_STEADY_TIME, r.get_type()); ASSERT_TRUE(r.sleep()); auto one = std::chrono::steady_clock::now(); @@ -122,6 +124,7 @@ TEST(TestRate, ros_rate_basics) { auto start = clock.now(); rclcpp::ROSRate r(period); EXPECT_EQ(rclcpp::Duration(period), r.period()); + ASSERT_FALSE(r.is_steady()); ASSERT_EQ(RCL_ROS_TIME, r.get_type()); ASSERT_TRUE(r.sleep()); auto one = clock.now(); @@ -155,42 +158,18 @@ TEST(TestRate, ros_rate_basics) { rclcpp::shutdown(); } -TEST(TestRate, from_double) { - { - rclcpp::Rate rate(1.0); - EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(1000)), rate.period()); - } - { - rclcpp::WallRate rate(2.0); - EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(500)), rate.period()); - } - { - rclcpp::WallRate rate(0.5); - EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(2000)), rate.period()); - } - { - rclcpp::ROSRate rate(4.0); - EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(250)), rate.period()); - } -} +/* + Basic test for the deprecated GenericRate class. + */ +TEST(TestRate, generic_rate) { + auto period = std::chrono::milliseconds(100); + auto offset = std::chrono::milliseconds(50); + auto epsilon = std::chrono::milliseconds(1); + double overrun_ratio = 1.5; -TEST(TestRate, clock_types) { - { - rclcpp::Rate rate(1.0, std::make_shared(RCL_SYSTEM_TIME)); - EXPECT_EQ(RCL_SYSTEM_TIME, rate.get_type()); - } { - rclcpp::Rate rate(1.0, std::make_shared(RCL_STEADY_TIME)); - EXPECT_EQ(RCL_STEADY_TIME, rate.get_type()); - } - { - rclcpp::Rate rate(1.0, std::make_shared(RCL_ROS_TIME)); - EXPECT_EQ(RCL_ROS_TIME, rate.get_type()); - } -} + auto start = std::chrono::system_clock::now(); -TEST(TestRate, generic_rate_api) { - { // suppress deprecated function warning #if !defined(_WIN32) # pragma GCC diagnostic push @@ -200,7 +179,7 @@ TEST(TestRate, generic_rate_api) { # pragma warning(disable: 4996) #endif - rclcpp::GenericRate gr(1.0); + rclcpp::GenericRate gr(period); // remove warning suppression #if !defined(_WIN32) @@ -209,12 +188,41 @@ TEST(TestRate, generic_rate_api) { # pragma warning(pop) #endif - ASSERT_FALSE(gr.sleep()); + EXPECT_EQ(period, gr.period()); ASSERT_FALSE(gr.is_steady()); - ASSERT_EQ(std::chrono::seconds(1), gr.period()); + ASSERT_TRUE(gr.sleep()); + auto one = std::chrono::system_clock::now(); + auto delta = one - start; + EXPECT_LT(period, delta + epsilon); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + ASSERT_TRUE(gr.sleep()); + auto two = std::chrono::system_clock::now(); + delta = two - start; + EXPECT_LT(2 * period, delta); + EXPECT_GT(2 * period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + auto two_offset = std::chrono::system_clock::now(); + gr.reset(); + ASSERT_TRUE(gr.sleep()); + auto three = std::chrono::system_clock::now(); + delta = three - two_offset; + EXPECT_LT(period, delta + epsilon); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset + period); + auto four = std::chrono::system_clock::now(); + ASSERT_FALSE(gr.sleep()); + auto five = std::chrono::system_clock::now(); + delta = five - four; + ASSERT_TRUE(epsilon > delta); } { + auto start = std::chrono::steady_clock::now(); + // suppress deprecated function warning #if !defined(_WIN32) # pragma GCC diagnostic push @@ -224,7 +232,7 @@ TEST(TestRate, generic_rate_api) { # pragma warning(disable: 4996) #endif - rclcpp::GenericRate gr(std::chrono::seconds(1)); + rclcpp::GenericRate gr(period); // remove warning suppression #if !defined(_WIN32) @@ -233,8 +241,72 @@ TEST(TestRate, generic_rate_api) { # pragma warning(pop) #endif + EXPECT_EQ(period, gr.period()); + ASSERT_TRUE(gr.is_steady()); + ASSERT_TRUE(gr.sleep()); + auto one = std::chrono::steady_clock::now(); + auto delta = one - start; + EXPECT_LT(period, delta); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + ASSERT_TRUE(gr.sleep()); + auto two = std::chrono::steady_clock::now(); + delta = two - start; + EXPECT_LT(2 * period, delta + epsilon); + EXPECT_GT(2 * period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + auto two_offset = std::chrono::steady_clock::now(); + gr.reset(); + ASSERT_TRUE(gr.sleep()); + auto three = std::chrono::steady_clock::now(); + delta = three - two_offset; + EXPECT_LT(period, delta); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset + period); + auto four = std::chrono::steady_clock::now(); ASSERT_FALSE(gr.sleep()); - ASSERT_FALSE(gr.is_steady()); - ASSERT_EQ(std::chrono::seconds(1), gr.period()); + auto five = std::chrono::steady_clock::now(); + delta = five - four; + EXPECT_GT(epsilon, delta); + } +} + +TEST(TestRate, from_double) { + { + rclcpp::Rate rate(1.0); + EXPECT_EQ(rclcpp::Duration(std::chrono::seconds(1)), rate.period()); + } + { + rclcpp::WallRate rate(2.0); + EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(500)), rate.period()); + } + { + rclcpp::WallRate rate(0.5); + EXPECT_EQ(rclcpp::Duration(std::chrono::seconds(2)), rate.period()); + } + { + rclcpp::ROSRate rate(4.0); + EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(250)), rate.period()); + } +} + +TEST(TestRate, clock_types) { + { + rclcpp::Rate rate(1.0, std::make_shared(RCL_SYSTEM_TIME)); + EXPECT_FALSE(rate.is_steady()); + EXPECT_EQ(RCL_SYSTEM_TIME, rate.get_type()); + } + { + rclcpp::Rate rate(1.0, std::make_shared(RCL_STEADY_TIME)); + EXPECT_TRUE(rate.is_steady()); + EXPECT_EQ(RCL_STEADY_TIME, rate.get_type()); + } + { + rclcpp::Rate rate(1.0, std::make_shared(RCL_ROS_TIME)); + EXPECT_FALSE(rate.is_steady()); + EXPECT_EQ(RCL_ROS_TIME, rate.get_type()); } } From beb00b1544927297b3031659cef8fceb3e481455 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Wed, 26 Jul 2023 13:17:48 +0300 Subject: [PATCH 06/18] Restore back RateBase -> GenericRate relationship is_steady() to be deprecated Signed-off-by: Alexey Merzlyakov --- rclcpp/include/rclcpp/rate.hpp | 13 +++-- rclcpp/test/rclcpp/test_rate.cpp | 96 ++++++++++++++++++++++++++++++-- 2 files changed, 101 insertions(+), 8 deletions(-) diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index eee1944768..cc7e5df678 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -35,7 +35,7 @@ class RateBase virtual ~RateBase() {} virtual bool sleep() = 0; - virtual bool is_steady() const = 0; + [[deprecated("use get_type() instead")]] virtual bool is_steady() const = 0; virtual rcl_clock_type_t get_type() const = 0; virtual void reset() = 0; }; @@ -45,7 +45,7 @@ using std::chrono::duration_cast; using std::chrono::nanoseconds; template -class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRate +class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRate : public RateBase { public: RCLCPP_SMART_PTR_DEFINITIONS(GenericRate) @@ -57,8 +57,6 @@ class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRat : period_(period), last_interval_(Clock::now()) {} - virtual ~GenericRate() {} - virtual bool sleep() { @@ -91,12 +89,18 @@ class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRat return true; } + [[deprecated("use get_type() instead")]] virtual bool is_steady() const { return Clock::is_steady; } + virtual rcl_clock_type_t get_type() const + { + return Clock::is_steady ? RCL_STEADY_TIME : RCL_SYSTEM_TIME; + } + virtual void reset() { @@ -164,6 +168,7 @@ class Rate : public RateBase return clock_->sleep_for(time_to_sleep); } + [[deprecated("use get_type() instead")]] virtual bool is_steady() const { diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index 92ecedaa88..b5de308d75 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -32,7 +32,21 @@ TEST(TestRate, rate_basics) { auto start = std::chrono::system_clock::now(); rclcpp::Rate r(period); EXPECT_EQ(rclcpp::Duration(period), r.period()); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif ASSERT_FALSE(r.is_steady()); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif ASSERT_EQ(RCL_SYSTEM_TIME, r.get_type()); ASSERT_TRUE(r.sleep()); auto one = std::chrono::system_clock::now(); @@ -77,7 +91,23 @@ TEST(TestRate, wall_rate_basics) { auto start = std::chrono::steady_clock::now(); rclcpp::WallRate r(period); EXPECT_EQ(rclcpp::Duration(period), r.period()); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif ASSERT_TRUE(r.is_steady()); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif ASSERT_EQ(RCL_STEADY_TIME, r.get_type()); ASSERT_TRUE(r.sleep()); auto one = std::chrono::steady_clock::now(); @@ -124,7 +154,23 @@ TEST(TestRate, ros_rate_basics) { auto start = clock.now(); rclcpp::ROSRate r(period); EXPECT_EQ(rclcpp::Duration(period), r.period()); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif ASSERT_FALSE(r.is_steady()); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif ASSERT_EQ(RCL_ROS_TIME, r.get_type()); ASSERT_TRUE(r.sleep()); auto one = clock.now(); @@ -170,7 +216,7 @@ TEST(TestRate, generic_rate) { { auto start = std::chrono::system_clock::now(); -// suppress deprecated function warning +// suppress deprecated warnings #if !defined(_WIN32) # pragma GCC diagnostic push # pragma GCC diagnostic ignored "-Wdeprecated-declarations" @@ -180,6 +226,7 @@ TEST(TestRate, generic_rate) { #endif rclcpp::GenericRate gr(period); + ASSERT_FALSE(gr.is_steady()); // remove warning suppression #if !defined(_WIN32) @@ -189,7 +236,6 @@ TEST(TestRate, generic_rate) { #endif EXPECT_EQ(period, gr.period()); - ASSERT_FALSE(gr.is_steady()); ASSERT_TRUE(gr.sleep()); auto one = std::chrono::system_clock::now(); auto delta = one - start; @@ -223,7 +269,7 @@ TEST(TestRate, generic_rate) { { auto start = std::chrono::steady_clock::now(); -// suppress deprecated function warning +// suppress deprecated warnings #if !defined(_WIN32) # pragma GCC diagnostic push # pragma GCC diagnostic ignored "-Wdeprecated-declarations" @@ -233,6 +279,7 @@ TEST(TestRate, generic_rate) { #endif rclcpp::GenericRate gr(period); + ASSERT_TRUE(gr.is_steady()); // remove warning suppression #if !defined(_WIN32) @@ -242,7 +289,6 @@ TEST(TestRate, generic_rate) { #endif EXPECT_EQ(period, gr.period()); - ASSERT_TRUE(gr.is_steady()); ASSERT_TRUE(gr.sleep()); auto one = std::chrono::steady_clock::now(); auto delta = one - start; @@ -296,17 +342,59 @@ TEST(TestRate, from_double) { TEST(TestRate, clock_types) { { rclcpp::Rate rate(1.0, std::make_shared(RCL_SYSTEM_TIME)); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif EXPECT_FALSE(rate.is_steady()); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif EXPECT_EQ(RCL_SYSTEM_TIME, rate.get_type()); } { rclcpp::Rate rate(1.0, std::make_shared(RCL_STEADY_TIME)); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif EXPECT_TRUE(rate.is_steady()); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif EXPECT_EQ(RCL_STEADY_TIME, rate.get_type()); } { rclcpp::Rate rate(1.0, std::make_shared(RCL_ROS_TIME)); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif EXPECT_FALSE(rate.is_steady()); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif EXPECT_EQ(RCL_ROS_TIME, rate.get_type()); } } From 112b7b7b0f8a88a496e2381a67fdcc81a9d3f700 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Tue, 15 Aug 2023 10:56:58 +0300 Subject: [PATCH 07/18] Move Rate implementations to rate.cpp Signed-off-by: Alexey Merzlyakov --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/rate.hpp | 51 +++-------------------- rclcpp/src/rclcpp/rate.cpp | 75 ++++++++++++++++++++++++++++++++++ 3 files changed, 82 insertions(+), 45 deletions(-) create mode 100644 rclcpp/src/rclcpp/rate.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 7eefe80ea1..c30ad4d818 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -107,6 +107,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/qos.cpp src/rclcpp/event_handler.cpp src/rclcpp/qos_overriding_options.cpp + src/rclcpp/rate.cpp src/rclcpp/serialization.cpp src/rclcpp/serialized_message.cpp src/rclcpp/service.cpp diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index cc7e5df678..78aef22c93 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -138,59 +138,20 @@ class Rate : public RateBase {} virtual bool - sleep() - { - // Time coming into sleep - auto now = clock_->now(); - // Time of next interval - auto next_interval = last_interval_ + period_; - // Detect backwards time flow - if (now < last_interval_) { - // Best thing to do is to set the next_interval to now + period - next_interval = now + period_; - } - // Update the interval - last_interval_ += period_; - // If the time_to_sleep is negative or zero, don't sleep - if (next_interval <= now) { - // If an entire cycle was missed then reset next interval. - // This might happen if the loop took more than a cycle. - // Or if time jumps forward. - if (now > next_interval + period_) { - last_interval_ = now + period_; - } - // Either way do not sleep and return false - return false; - } - // Calculate the time to sleep - auto time_to_sleep = next_interval - now; - // Sleep (will get interrupted by ctrl-c, may not sleep full time) - return clock_->sleep_for(time_to_sleep); - } + sleep(); [[deprecated("use get_type() instead")]] virtual bool - is_steady() const - { - return clock_->get_clock_type() == RCL_STEADY_TIME; - } + is_steady() const; virtual rcl_clock_type_t - get_type() const - { - return clock_->get_clock_type(); - } + get_type() const; virtual void - reset() - { - last_interval_ = clock_->now(); - } + reset(); - Duration period() const - { - return period_; - } + Duration + period() const; private: RCLCPP_DISABLE_COPY(Rate) diff --git a/rclcpp/src/rclcpp/rate.cpp b/rclcpp/src/rclcpp/rate.cpp new file mode 100644 index 0000000000..80d7965b22 --- /dev/null +++ b/rclcpp/src/rclcpp/rate.cpp @@ -0,0 +1,75 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/rate.hpp" + +namespace rclcpp +{ + +bool +Rate::sleep() +{ + // Time coming into sleep + auto now = clock_->now(); + // Time of next interval + auto next_interval = last_interval_ + period_; + // Detect backwards time flow + if (now < last_interval_) { + // Best thing to do is to set the next_interval to now + period + next_interval = now + period_; + } + // Update the interval + last_interval_ += period_; + // If the time_to_sleep is negative or zero, don't sleep + if (next_interval <= now) { + // If an entire cycle was missed then reset next interval. + // This might happen if the loop took more than a cycle. + // Or if time jumps forward. + if (now > next_interval + period_) { + last_interval_ = now + period_; + } + // Either way do not sleep and return false + return false; + } + // Calculate the time to sleep + auto time_to_sleep = next_interval - now; + // Sleep (will get interrupted by ctrl-c, may not sleep full time) + return clock_->sleep_for(time_to_sleep); +} + +bool +Rate::is_steady() const +{ + return clock_->get_clock_type() == RCL_STEADY_TIME; +} + +rcl_clock_type_t +Rate::get_type() const +{ + return clock_->get_clock_type(); +} + +void +Rate::reset() +{ + last_interval_ = clock_->now(); +} + +Duration +Rate::period() const +{ + return period_; +} + +} // namespace rclcpp From 8f19f28dfb72c50ab1d353026ffdf366777f327f Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Fri, 25 Aug 2023 13:39:17 +0300 Subject: [PATCH 08/18] Add checks for incorrect Rate constructor arguments Signed-off-by: Alexey Merzlyakov --- rclcpp/include/rclcpp/rate.hpp | 16 +++++++++--- rclcpp/src/rclcpp/rate.cpp | 2 +- rclcpp/test/rclcpp/test_rate.cpp | 44 ++++++++++++++++++++++++++++++++ 3 files changed, 57 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 78aef22c93..88359317b8 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -128,14 +128,22 @@ class Rate : public RateBase explicit Rate( const double rate, Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME)) - : Rate( - Duration::from_seconds(1.0 / rate), clock) - {} + : clock_(clock), period_(0, 0), last_interval_(clock_->now()) + { + if (rate <= 0.0) { + throw std::invalid_argument{"rate must greater than 0"}; + } + period_ = Duration::from_seconds(1.0 / rate); + } explicit Rate( const Duration & period, Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME)) : clock_(clock), period_(period), last_interval_(clock_->now()) - {} + { + if (period <= Duration(0, 0)) { + throw std::invalid_argument{"period must greater than 0"}; + } + } virtual bool sleep(); diff --git a/rclcpp/src/rclcpp/rate.cpp b/rclcpp/src/rclcpp/rate.cpp index 80d7965b22..bb886c9b85 100644 --- a/rclcpp/src/rclcpp/rate.cpp +++ b/rclcpp/src/rclcpp/rate.cpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// Copyright 2023 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index b5de308d75..50c248c3ff 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -398,3 +398,47 @@ TEST(TestRate, clock_types) { EXPECT_EQ(RCL_ROS_TIME, rate.get_type()); } } + +TEST(TestRate, incorrect_constuctor) { + // Constructor with 0-frequency + try { + rclcpp::Rate rate(0.0); + FAIL() << "Rate with 0-frequency shouldn't be constructed\n"; + } catch(const std::invalid_argument & ex) { + // Do nothing + } catch(const std::exception & ex) { + FAIL() << "Unexpected fail: " << ex.what() << "\n"; + } + + // Constructor with negative frequency + try { + rclcpp::Rate rate(-1.0); + FAIL() << "Rate with negative frequency shouldn't be constructed\n"; + } catch(const std::invalid_argument & ex) { + // Do nothing + } catch(const std::exception & ex) { + FAIL() << "Unexpected fail: " << ex.what() << "\n"; + } + + // Constructor with 0-duration + try { + rclcpp::Duration d0(0, 0); + rclcpp::Rate rate(d0); + FAIL() << "Rate with 0-duration shouldn't be constructed\n"; + } catch(const std::invalid_argument & ex) { + // Do nothing + } catch(const std::exception & ex) { + FAIL() << "Unexpected fail: " << ex.what() << "\n"; + } + + // Constructor with negative duration + try { + rclcpp::Duration dN(-1, 0); + rclcpp::Rate rate(dN); + FAIL() << "Rate with negative duraiton shouldn't be constructed\n"; + } catch(const std::invalid_argument & ex) { + // Do nothing + } catch(const std::exception & ex) { + FAIL() << "Unexpected fail: " << ex.what() << "\n"; + } +} From 188b8ddab1c9cbf2c272857157e7d6d73622e81c Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Fri, 25 Aug 2023 14:14:21 +0300 Subject: [PATCH 09/18] Cover GenericRate::get_type() functionality Signed-off-by: Alexey Merzlyakov --- rclcpp/test/rclcpp/test_rate.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index 50c248c3ff..37afca2eb7 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -235,6 +235,7 @@ TEST(TestRate, generic_rate) { # pragma warning(pop) #endif + ASSERT_EQ(gr.get_type(), RCL_SYSTEM_TIME); EXPECT_EQ(period, gr.period()); ASSERT_TRUE(gr.sleep()); auto one = std::chrono::system_clock::now(); @@ -288,6 +289,7 @@ TEST(TestRate, generic_rate) { # pragma warning(pop) #endif + ASSERT_EQ(gr.get_type(), RCL_STEADY_TIME); EXPECT_EQ(period, gr.period()); ASSERT_TRUE(gr.sleep()); auto one = std::chrono::steady_clock::now(); From 7c1a331c63f3cc8b4f6c789e00c4c76be437f874 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Fri, 25 Aug 2023 15:12:35 +0300 Subject: [PATCH 10/18] Fix uncrustify errors Signed-off-by: Alexey Merzlyakov --- rclcpp/test/rclcpp/test_rate.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index 37afca2eb7..810439c3fb 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -406,9 +406,9 @@ TEST(TestRate, incorrect_constuctor) { try { rclcpp::Rate rate(0.0); FAIL() << "Rate with 0-frequency shouldn't be constructed\n"; - } catch(const std::invalid_argument & ex) { + } catch (const std::invalid_argument & ex) { // Do nothing - } catch(const std::exception & ex) { + } catch (const std::exception & ex) { FAIL() << "Unexpected fail: " << ex.what() << "\n"; } @@ -416,9 +416,9 @@ TEST(TestRate, incorrect_constuctor) { try { rclcpp::Rate rate(-1.0); FAIL() << "Rate with negative frequency shouldn't be constructed\n"; - } catch(const std::invalid_argument & ex) { + } catch (const std::invalid_argument & ex) { // Do nothing - } catch(const std::exception & ex) { + } catch (const std::exception & ex) { FAIL() << "Unexpected fail: " << ex.what() << "\n"; } @@ -427,9 +427,9 @@ TEST(TestRate, incorrect_constuctor) { rclcpp::Duration d0(0, 0); rclcpp::Rate rate(d0); FAIL() << "Rate with 0-duration shouldn't be constructed\n"; - } catch(const std::invalid_argument & ex) { + } catch (const std::invalid_argument & ex) { // Do nothing - } catch(const std::exception & ex) { + } catch (const std::exception & ex) { FAIL() << "Unexpected fail: " << ex.what() << "\n"; } @@ -438,9 +438,9 @@ TEST(TestRate, incorrect_constuctor) { rclcpp::Duration dN(-1, 0); rclcpp::Rate rate(dN); FAIL() << "Rate with negative duraiton shouldn't be constructed\n"; - } catch(const std::invalid_argument & ex) { + } catch (const std::invalid_argument & ex) { // Do nothing - } catch(const std::exception & ex) { + } catch (const std::exception & ex) { FAIL() << "Unexpected fail: " << ex.what() << "\n"; } } From 5c3cec8f6bd1ee86166894d0e27ffd188b2ecd6c Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Fri, 25 Aug 2023 16:36:03 +0300 Subject: [PATCH 11/18] Misprint fix Signed-off-by: Alexey Merzlyakov --- rclcpp/include/rclcpp/rate.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 88359317b8..0c968d8bca 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -131,7 +131,7 @@ class Rate : public RateBase : clock_(clock), period_(0, 0), last_interval_(clock_->now()) { if (rate <= 0.0) { - throw std::invalid_argument{"rate must greater than 0"}; + throw std::invalid_argument{"rate must be greater than 0"}; } period_ = Duration::from_seconds(1.0 / rate); } @@ -141,7 +141,7 @@ class Rate : public RateBase : clock_(clock), period_(period), last_interval_(clock_->now()) { if (period <= Duration(0, 0)) { - throw std::invalid_argument{"period must greater than 0"}; + throw std::invalid_argument{"period must be greater than 0"}; } } From a947972f50007dd3e0249f71f92017fcf22ca9e0 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Mon, 28 Aug 2023 12:40:57 +0300 Subject: [PATCH 12/18] Separate Rate constructors implementations Signed-off-by: Alexey Merzlyakov --- rclcpp/include/rclcpp/rate.hpp | 36 +++++++-------------------------- rclcpp/src/rclcpp/rate.cpp | 37 ++++++++++++++++++++++++++++++++++ 2 files changed, 44 insertions(+), 29 deletions(-) diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 0c968d8bca..65aa80b17c 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -127,23 +127,11 @@ class Rate : public RateBase explicit Rate( const double rate, - Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME)) - : clock_(clock), period_(0, 0), last_interval_(clock_->now()) - { - if (rate <= 0.0) { - throw std::invalid_argument{"rate must be greater than 0"}; - } - period_ = Duration::from_seconds(1.0 / rate); - } + Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME)); + explicit Rate( const Duration & period, - Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME)) - : clock_(clock), period_(period), last_interval_(clock_->now()) - { - if (period <= Duration(0, 0)) { - throw std::invalid_argument{"period must be greater than 0"}; - } - } + Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME)); virtual bool sleep(); @@ -172,25 +160,15 @@ class Rate : public RateBase class WallRate : public Rate { public: - explicit WallRate(const double rate) - : Rate(rate, std::make_shared(RCL_STEADY_TIME)) - {} - - explicit WallRate(const Duration & period) - : Rate(period, std::make_shared(RCL_STEADY_TIME)) - {} + explicit WallRate(const double rate); + explicit WallRate(const Duration & period); }; class ROSRate : public Rate { public: - explicit ROSRate(const double rate) - : Rate(rate, std::make_shared(RCL_ROS_TIME)) - {} - - explicit ROSRate(const Duration & period) - : Rate(period, std::make_shared(RCL_ROS_TIME)) - {} + explicit ROSRate(const double rate); + explicit ROSRate(const Duration & period); }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/rate.cpp b/rclcpp/src/rclcpp/rate.cpp index bb886c9b85..04a1f57185 100644 --- a/rclcpp/src/rclcpp/rate.cpp +++ b/rclcpp/src/rclcpp/rate.cpp @@ -14,9 +14,30 @@ #include "rclcpp/rate.hpp" +#include + namespace rclcpp { +Rate::Rate( + const double rate, Clock::SharedPtr clock) +: clock_(clock), period_(0, 0), last_interval_(clock_->now()) +{ + if (rate <= 0.0) { + throw std::invalid_argument{"rate must be greater than 0"}; + } + period_ = Duration::from_seconds(1.0 / rate); +} + +Rate::Rate( + const Duration & period, Clock::SharedPtr clock) +: clock_(clock), period_(period), last_interval_(clock_->now()) +{ + if (period <= Duration(0, 0)) { + throw std::invalid_argument{"period must be greater than 0"}; + } +} + bool Rate::sleep() { @@ -72,4 +93,20 @@ Rate::period() const return period_; } +WallRate::WallRate(const double rate) +: Rate(rate, std::make_shared(RCL_STEADY_TIME)) +{} + +WallRate::WallRate(const Duration & period) +: Rate(period, std::make_shared(RCL_STEADY_TIME)) +{} + +ROSRate::ROSRate(const double rate) +: Rate(rate, std::make_shared(RCL_ROS_TIME)) +{} + +ROSRate::ROSRate(const Duration & period) +: Rate(period, std::make_shared(RCL_ROS_TIME)) +{} + } // namespace rclcpp From 480681acdb09ad6ad179e60090e7665f2fd78d7f Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Tue, 29 Aug 2023 07:29:08 +0300 Subject: [PATCH 13/18] Update rclcpp/include/rclcpp/rate.hpp Co-authored-by: Chris Lalancette Signed-off-by: Alexey Merzlyakov --- rclcpp/include/rclcpp/rate.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 65aa80b17c..39f204c75b 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -120,7 +120,7 @@ class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRat std::chrono::time_point last_interval_; }; -class Rate : public RateBase +class RCLCPP_PUBLIC Rate : public RateBase { public: RCLCPP_SMART_PTR_DEFINITIONS(Rate) From 99b6d07e123c3dea34d036e5de7299c5173bc9cf Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Tue, 29 Aug 2023 07:29:21 +0300 Subject: [PATCH 14/18] Update rclcpp/include/rclcpp/rate.hpp Co-authored-by: Chris Lalancette Signed-off-by: Alexey Merzlyakov --- rclcpp/include/rclcpp/rate.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 39f204c75b..76ebb71b05 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -157,7 +157,7 @@ class RCLCPP_PUBLIC Rate : public RateBase Time last_interval_; }; -class WallRate : public Rate +class RCLCPP_PUBLIC WallRate : public Rate { public: explicit WallRate(const double rate); From 42c14ddbdfad022093011acf2786e4f76b7d09e6 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Tue, 29 Aug 2023 07:29:36 +0300 Subject: [PATCH 15/18] Update rclcpp/include/rclcpp/rate.hpp Co-authored-by: Chris Lalancette Signed-off-by: Alexey Merzlyakov --- rclcpp/include/rclcpp/rate.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 76ebb71b05..c06d8aec01 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -164,7 +164,7 @@ class RCLCPP_PUBLIC WallRate : public Rate explicit WallRate(const Duration & period); }; -class ROSRate : public Rate +class RCLCPP_PUBLIC ROSRate : public Rate { public: explicit ROSRate(const double rate); From 6354c6810ac6b411c4af1b745e7b5229226f27f1 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Tue, 29 Aug 2023 09:45:05 +0300 Subject: [PATCH 16/18] Switch to RCLCPP_EXPECT_THROW_EQ in test_rate Signed-off-by: Alexey Merzlyakov --- rclcpp/test/rclcpp/test_rate.cpp | 49 ++++++++++---------------------- 1 file changed, 15 insertions(+), 34 deletions(-) diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index 810439c3fb..5ab64ee608 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -14,10 +14,13 @@ #include +#include #include #include "rclcpp/rate.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + /* Basic tests for the Rate, WallRate and ROSRate classes. */ @@ -403,44 +406,22 @@ TEST(TestRate, clock_types) { TEST(TestRate, incorrect_constuctor) { // Constructor with 0-frequency - try { - rclcpp::Rate rate(0.0); - FAIL() << "Rate with 0-frequency shouldn't be constructed\n"; - } catch (const std::invalid_argument & ex) { - // Do nothing - } catch (const std::exception & ex) { - FAIL() << "Unexpected fail: " << ex.what() << "\n"; - } + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(0.0), + std::invalid_argument("rate must be greater than 0")); // Constructor with negative frequency - try { - rclcpp::Rate rate(-1.0); - FAIL() << "Rate with negative frequency shouldn't be constructed\n"; - } catch (const std::invalid_argument & ex) { - // Do nothing - } catch (const std::exception & ex) { - FAIL() << "Unexpected fail: " << ex.what() << "\n"; - } + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(-1.0), + std::invalid_argument("rate must be greater than 0")); // Constructor with 0-duration - try { - rclcpp::Duration d0(0, 0); - rclcpp::Rate rate(d0); - FAIL() << "Rate with 0-duration shouldn't be constructed\n"; - } catch (const std::invalid_argument & ex) { - // Do nothing - } catch (const std::exception & ex) { - FAIL() << "Unexpected fail: " << ex.what() << "\n"; - } + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(rclcpp::Duration(0, 0)), + std::invalid_argument("period must be greater than 0")); // Constructor with negative duration - try { - rclcpp::Duration dN(-1, 0); - rclcpp::Rate rate(dN); - FAIL() << "Rate with negative duraiton shouldn't be constructed\n"; - } catch (const std::invalid_argument & ex) { - // Do nothing - } catch (const std::exception & ex) { - FAIL() << "Unexpected fail: " << ex.what() << "\n"; - } + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(rclcpp::Duration(-1, 0)), + std::invalid_argument("period must be greater than 0")); } From bff4fd07fe01d171461a05c69f6baeb31ef8a161 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 30 Aug 2023 23:17:52 +0000 Subject: [PATCH 17/18] Another try at Windows annotations. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/rate.hpp | 28 +++++++++++++++++++++++++--- 1 file changed, 25 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index c06d8aec01..a001378d9f 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -33,10 +33,19 @@ class RateBase public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(RateBase) + RCLCPP_PUBLIC virtual ~RateBase() {} + + RCLCPP_PUBLIC virtual bool sleep() = 0; + + RCLCPP_PUBLIC [[deprecated("use get_type() instead")]] virtual bool is_steady() const = 0; + + RCLCPP_PUBLIC virtual rcl_clock_type_t get_type() const = 0; + + RCLCPP_PUBLIC virtual void reset() = 0; }; @@ -120,32 +129,39 @@ class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRat std::chrono::time_point last_interval_; }; -class RCLCPP_PUBLIC Rate : public RateBase +class Rate : public RateBase { public: RCLCPP_SMART_PTR_DEFINITIONS(Rate) + RCLCPP_PUBLIC explicit Rate( const double rate, Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME)); + RCLCPP_PUBLIC explicit Rate( const Duration & period, Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME)); + RCLCPP_PUBLIC virtual bool sleep(); + RCLCPP_PUBLIC [[deprecated("use get_type() instead")]] virtual bool is_steady() const; + RCLCPP_PUBLIC virtual rcl_clock_type_t get_type() const; + RCLCPP_PUBLIC virtual void reset(); + RCLCPP_PUBLIC Duration period() const; @@ -157,17 +173,23 @@ class RCLCPP_PUBLIC Rate : public RateBase Time last_interval_; }; -class RCLCPP_PUBLIC WallRate : public Rate +class WallRate : public Rate { public: + RCLCPP_PUBLIC explicit WallRate(const double rate); + + RCLCPP_PUBLIC explicit WallRate(const Duration & period); }; -class RCLCPP_PUBLIC ROSRate : public Rate +class ROSRate : public Rate { public: + RCLCPP_PUBLIC explicit ROSRate(const double rate); + + RCLCPP_PUBLIC explicit ROSRate(const Duration & period); }; From f54ca7db992b4e64eaa38218c8fb6a3ab14dde67 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 31 Aug 2023 12:23:37 +0000 Subject: [PATCH 18/18] Fix compilation with clang. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/rate.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index a001378d9f..884e462a76 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -39,8 +39,9 @@ class RateBase RCLCPP_PUBLIC virtual bool sleep() = 0; + [[deprecated("use get_type() instead")]] RCLCPP_PUBLIC - [[deprecated("use get_type() instead")]] virtual bool is_steady() const = 0; + virtual bool is_steady() const = 0; RCLCPP_PUBLIC virtual rcl_clock_type_t get_type() const = 0; @@ -148,8 +149,8 @@ class Rate : public RateBase virtual bool sleep(); - RCLCPP_PUBLIC [[deprecated("use get_type() instead")]] + RCLCPP_PUBLIC virtual bool is_steady() const;