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 55d3bbcb85..884e462a76 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -19,6 +19,8 @@ #include #include +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" @@ -31,9 +33,20 @@ class RateBase public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(RateBase) + RCLCPP_PUBLIC virtual ~RateBase() {} + + RCLCPP_PUBLIC virtual bool sleep() = 0; + + [[deprecated("use get_type() instead")]] + RCLCPP_PUBLIC virtual bool is_steady() const = 0; + + RCLCPP_PUBLIC + virtual rcl_clock_type_t get_type() const = 0; + + RCLCPP_PUBLIC virtual void reset() = 0; }; @@ -42,14 +55,13 @@ using std::chrono::duration_cast; using std::chrono::nanoseconds; template -class GenericRate : public RateBase +class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRate : public RateBase { public: RCLCPP_SMART_PTR_DEFINITIONS(GenericRate) explicit GenericRate(double rate) - : GenericRate( - duration_cast(duration(1.0 / rate))) + : period_(duration_cast(duration(1.0 / rate))), last_interval_(Clock::now()) {} explicit GenericRate(std::chrono::nanoseconds period) : period_(period), last_interval_(Clock::now()) @@ -87,12 +99,18 @@ class GenericRate : public RateBase 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() { @@ -112,8 +130,69 @@ class GenericRate : public RateBase std::chrono::time_point last_interval_; }; -using Rate = GenericRate; -using WallRate = GenericRate; +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(); + + [[deprecated("use get_type() instead")]] + RCLCPP_PUBLIC + 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; + +private: + RCLCPP_DISABLE_COPY(Rate) + + Clock::SharedPtr clock_; + Duration period_; + Time last_interval_; +}; + +class WallRate : public Rate +{ +public: + RCLCPP_PUBLIC + explicit WallRate(const double rate); + + RCLCPP_PUBLIC + explicit WallRate(const Duration & period); +}; + +class ROSRate : public Rate +{ +public: + RCLCPP_PUBLIC + explicit ROSRate(const double rate); + + RCLCPP_PUBLIC + explicit ROSRate(const Duration & period); +}; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/rate.cpp b/rclcpp/src/rclcpp/rate.cpp new file mode 100644 index 0000000000..04a1f57185 --- /dev/null +++ b/rclcpp/src/rclcpp/rate.cpp @@ -0,0 +1,112 @@ +// 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. +// 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" + +#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() +{ + // 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_; +} + +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 diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index d6608d59f6..5ab64ee608 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -14,14 +14,19 @@ #include +#include #include #include "rclcpp/rate.hpp" +#include "../utils/rclcpp_gtest_macros.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); + auto period = std::chrono::milliseconds(1000); auto offset = std::chrono::milliseconds(500); auto epsilon = std::chrono::milliseconds(100); @@ -29,8 +34,23 @@ TEST(TestRate, rate_basics) { auto start = std::chrono::system_clock::now(); rclcpp::Rate r(period); - EXPECT_EQ(period, 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(); auto delta = one - start; @@ -59,9 +79,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 +93,25 @@ TEST(TestRate, wall_rate_basics) { auto start = std::chrono::steady_clock::now(); rclcpp::WallRate r(period); - EXPECT_EQ(period, 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(); auto delta = one - start; @@ -99,23 +140,288 @@ 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()); +// 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(); + 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(); +} + +/* + 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; + + { + auto start = std::chrono::system_clock::now(); + +// 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 + + rclcpp::GenericRate gr(period); + ASSERT_FALSE(gr.is_steady()); + +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# 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(); + 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 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 + + rclcpp::GenericRate gr(period); + ASSERT_TRUE(gr.is_steady()); + +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# 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(); + 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()); + auto five = std::chrono::steady_clock::now(); + delta = five - four; + EXPECT_GT(epsilon, delta); + } } 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::seconds(1)), 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::seconds(2)), rate.period()); } { - rclcpp::WallRate rate(4.0); - EXPECT_EQ(std::chrono::milliseconds(250), 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)); +// 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()); + } +} + +TEST(TestRate, incorrect_constuctor) { + // Constructor with 0-frequency + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(0.0), + std::invalid_argument("rate must be greater than 0")); + + // Constructor with negative frequency + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(-1.0), + std::invalid_argument("rate must be greater than 0")); + + // Constructor with 0-duration + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(rclcpp::Duration(0, 0)), + std::invalid_argument("period must be greater than 0")); + + // Constructor with negative duration + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(rclcpp::Duration(-1, 0)), + std::invalid_argument("period must be greater than 0")); +}