diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index b9410ca089..33bd0bf0b9 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -497,7 +497,7 @@ Context::sleep_for(const std::chrono::nanoseconds & nanoseconds) std::unique_lock lock(interrupt_mutex_); auto start = std::chrono::steady_clock::now(); // this will release the lock while waiting - interrupt_condition_variable_.wait_for(lock, nanoseconds); + interrupt_condition_variable_.wait_for(lock, time_left); time_left -= std::chrono::steady_clock::now() - start; } } while (time_left > std::chrono::nanoseconds::zero() && this->is_valid()); diff --git a/rclcpp/src/rclcpp/rate.cpp b/rclcpp/src/rclcpp/rate.cpp new file mode 100644 index 0000000000..7c98d6d4fe --- /dev/null +++ b/rclcpp/src/rclcpp/rate.cpp @@ -0,0 +1,106 @@ +// 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 +#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) + clock_->sleep_for(time_to_sleep); + return true; +} + +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(); +} + +std::chrono::nanoseconds +Rate::period() const +{ + return std::chrono::nanoseconds(period_.nanoseconds()); +} + +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)) +{} + +} // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index d6608d59f6..a5a22518ce 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -18,10 +18,33 @@ #include "rclcpp/rate.hpp" +<<<<<<< HEAD /* Basic tests for the Rate and WallRate classes. */ TEST(TestRate, rate_basics) { +======= +#include "../utils/rclcpp_gtest_macros.hpp" + +class TestRate : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +/* + Basic tests for the Rate and WallRate classes. + */ +TEST_F(TestRate, rate_basics) { +>>>>>>> fcbe64cf (address rate related flaky tests. (#2329)) auto period = std::chrono::milliseconds(1000); auto offset = std::chrono::milliseconds(500); auto epsilon = std::chrono::milliseconds(100); @@ -61,7 +84,11 @@ TEST(TestRate, rate_basics) { ASSERT_TRUE(epsilon > delta); } +<<<<<<< HEAD TEST(TestRate, wall_rate_basics) { +======= +TEST_F(TestRate, wall_rate_basics) { +>>>>>>> fcbe64cf (address rate related flaky tests. (#2329)) auto period = std::chrono::milliseconds(100); auto offset = std::chrono::milliseconds(50); auto epsilon = std::chrono::milliseconds(1); @@ -99,9 +126,130 @@ TEST(TestRate, wall_rate_basics) { auto five = std::chrono::steady_clock::now(); delta = five - four; EXPECT_GT(epsilon, delta); +<<<<<<< HEAD +======= } -TEST(TestRate, from_double) { +/* + Basic test for the deprecated GenericRate class. + */ +TEST_F(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); + } +>>>>>>> fcbe64cf (address rate related flaky tests. (#2329)) +} + +TEST_F(TestRate, from_double) { { rclcpp::WallRate rate(1.0); EXPECT_EQ(std::chrono::seconds(1), rate.period()); @@ -119,3 +267,88 @@ TEST(TestRate, from_double) { EXPECT_EQ(std::chrono::milliseconds(250), rate.period()); } } +<<<<<<< HEAD +======= + +TEST_F(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_F(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")); +} +>>>>>>> fcbe64cf (address rate related flaky tests. (#2329))