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()); } }