Skip to content

Commit

Permalink
Return back GenericRate logic
Browse files Browse the repository at this point in the history 
Correct Rate::sleep() return value

Signed-off-by: Alexey Merzlyakov <[email protected]>
  • Loading branch information
@AlexeyMerzlyakov
AlexeyMerzlyakov committed Apr 13, 2023
1 parent 7323e97 commit 30b95de
Show file tree
Hide file tree
Showing 2 changed files with 187 additions and 48 deletions.
83 changes: 75 additions & 8 deletions rclcpp/include/rclcpp/rate.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include <chrono>
#include <memory>
#include <thread>

#include "rclcpp/clock.hpp"
#include "rclcpp/duration.hpp"
Expand All @@ -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 Clock = std::chrono::high_resolution_clock>
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<nanoseconds>(duration<double>(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<typename Clock::rep, std::nano>;
std::chrono::time_point<Clock, ClockDurationNano> last_interval_;
};

class Rate : public RateBase
Expand Down Expand Up @@ -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
Expand Down
152 changes: 112 additions & 40 deletions rclcpp/test/rclcpp/test_rate.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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();
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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<rclcpp::Clock>(RCL_SYSTEM_TIME));
EXPECT_EQ(RCL_SYSTEM_TIME, rate.get_type());
}
{
rclcpp::Rate rate(1.0, std::make_shared<rclcpp::Clock>(RCL_STEADY_TIME));
EXPECT_EQ(RCL_STEADY_TIME, rate.get_type());
}
{
rclcpp::Rate rate(1.0, std::make_shared<rclcpp::Clock>(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
Expand All @@ -200,7 +179,7 @@ TEST(TestRate, generic_rate_api) {
# pragma warning(disable: 4996)
#endif

rclcpp::GenericRate<std::chrono::system_clock> gr(1.0);
rclcpp::GenericRate<std::chrono::system_clock> gr(period);

// remove warning suppression
#if !defined(_WIN32)
Expand All @@ -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
Expand All @@ -224,7 +232,7 @@ TEST(TestRate, generic_rate_api) {
# pragma warning(disable: 4996)
#endif

rclcpp::GenericRate<std::chrono::steady_clock> gr(std::chrono::seconds(1));
rclcpp::GenericRate<std::chrono::steady_clock> gr(period);

// remove warning suppression
#if !defined(_WIN32)
Expand All @@ -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<rclcpp::Clock>(RCL_SYSTEM_TIME));
EXPECT_FALSE(rate.is_steady());
EXPECT_EQ(RCL_SYSTEM_TIME, rate.get_type());
}
{
rclcpp::Rate rate(1.0, std::make_shared<rclcpp::Clock>(RCL_STEADY_TIME));
EXPECT_TRUE(rate.is_steady());
EXPECT_EQ(RCL_STEADY_TIME, rate.get_type());
}
{
rclcpp::Rate rate(1.0, std::make_shared<rclcpp::Clock>(RCL_ROS_TIME));
EXPECT_FALSE(rate.is_steady());
EXPECT_EQ(RCL_ROS_TIME, rate.get_type());
}
}

0 comments on commit 30b95de

Please sign in to comment.