Skip to content

Commit bc43577

Browse files
AlexeyMerzlyakovclalancette
AlexeyMerzlyakov
and
clalancette
authored
Make Rate to select the clock to work with (ros2#2123)
* Make Rate to select the clock to work with Add ROSRate respective with ROS time * Make GenericRate class to be deprecated * Adjust test cases for new rates is_steady() to be deprecated Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com> Co-authored-by: Chris Lalancette <clalancette@gmail.com>
1 parent 43cf0be commit bc43577

4 files changed

Lines changed: 512 additions & 14 deletions

File tree

‎rclcpp/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -107,6 +107,7 @@ set(${PROJECT_NAME}_SRCS
107107
src/rclcpp/qos.cpp
108108
src/rclcpp/event_handler.cpp
109109
src/rclcpp/qos_overriding_options.cpp
110+
src/rclcpp/rate.cpp
110111
src/rclcpp/serialization.cpp
111112
src/rclcpp/serialized_message.cpp
112113
src/rclcpp/service.cpp

‎rclcpp/include/rclcpp/rate.hpp‎

Lines changed: 84 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,8 @@
1919
#include <memory>
2020
#include <thread>
2121

22+
#include "rclcpp/clock.hpp"
23+
#include "rclcpp/duration.hpp"
2224
#include "rclcpp/macros.hpp"
2325
#include "rclcpp/utilities.hpp"
2426
#include "rclcpp/visibility_control.hpp"
@@ -31,9 +33,20 @@ class RateBase
3133
public:
3234
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(RateBase)
3335

36+
RCLCPP_PUBLIC
3437
virtual ~RateBase() {}
38+
39+
RCLCPP_PUBLIC
3540
virtual bool sleep() = 0;
41+
42+
[[deprecated("use get_type() instead")]]
43+
RCLCPP_PUBLIC
3644
virtual bool is_steady() const = 0;
45+
46+
RCLCPP_PUBLIC
47+
virtual rcl_clock_type_t get_type() const = 0;
48+
49+
RCLCPP_PUBLIC
3750
virtual void reset() = 0;
3851
};
3952

@@ -42,14 +55,13 @@ using std::chrono::duration_cast;
4255
using std::chrono::nanoseconds;
4356

4457
template<class Clock = std::chrono::high_resolution_clock>
45-
class GenericRate : public RateBase
58+
class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRate : public RateBase
4659
{
4760
public:
4861
RCLCPP_SMART_PTR_DEFINITIONS(GenericRate)
4962

5063
explicit GenericRate(double rate)
51-
: GenericRate<Clock>(
52-
duration_cast<nanoseconds>(duration<double>(1.0 / rate)))
64+
: period_(duration_cast<nanoseconds>(duration<double>(1.0 / rate))), last_interval_(Clock::now())
5365
{}
5466
explicit GenericRate(std::chrono::nanoseconds period)
5567
: period_(period), last_interval_(Clock::now())
@@ -87,12 +99,18 @@ class GenericRate : public RateBase
8799
return true;
88100
}
89101

102+
[[deprecated("use get_type() instead")]]
90103
virtual bool
91104
is_steady() const
92105
{
93106
return Clock::is_steady;
94107
}
95108

109+
virtual rcl_clock_type_t get_type() const
110+
{
111+
return Clock::is_steady ? RCL_STEADY_TIME : RCL_SYSTEM_TIME;
112+
}
113+
96114
virtual void
97115
reset()
98116
{
@@ -112,8 +130,69 @@ class GenericRate : public RateBase
112130
std::chrono::time_point<Clock, ClockDurationNano> last_interval_;
113131
};
114132

115-
using Rate = GenericRate<std::chrono::system_clock>;
116-
using WallRate = GenericRate<std::chrono::steady_clock>;
133+
class Rate : public RateBase
134+
{
135+
public:
136+
RCLCPP_SMART_PTR_DEFINITIONS(Rate)
137+
138+
RCLCPP_PUBLIC
139+
explicit Rate(
140+
const double rate,
141+
Clock::SharedPtr clock = std::make_shared<Clock>(RCL_SYSTEM_TIME));
142+
143+
RCLCPP_PUBLIC
144+
explicit Rate(
145+
const Duration & period,
146+
Clock::SharedPtr clock = std::make_shared<Clock>(RCL_SYSTEM_TIME));
147+
148+
RCLCPP_PUBLIC
149+
virtual bool
150+
sleep();
151+
152+
[[deprecated("use get_type() instead")]]
153+
RCLCPP_PUBLIC
154+
virtual bool
155+
is_steady() const;
156+
157+
RCLCPP_PUBLIC
158+
virtual rcl_clock_type_t
159+
get_type() const;
160+
161+
RCLCPP_PUBLIC
162+
virtual void
163+
reset();
164+
165+
RCLCPP_PUBLIC
166+
Duration
167+
period() const;
168+
169+
private:
170+
RCLCPP_DISABLE_COPY(Rate)
171+
172+
Clock::SharedPtr clock_;
173+
Duration period_;
174+
Time last_interval_;
175+
};
176+
177+
class WallRate : public Rate
178+
{
179+
public:
180+
RCLCPP_PUBLIC
181+
explicit WallRate(const double rate);
182+
183+
RCLCPP_PUBLIC
184+
explicit WallRate(const Duration & period);
185+
};
186+
187+
class ROSRate : public Rate
188+
{
189+
public:
190+
RCLCPP_PUBLIC
191+
explicit ROSRate(const double rate);
192+
193+
RCLCPP_PUBLIC
194+
explicit ROSRate(const Duration & period);
195+
};
117196

118197
} // namespace rclcpp
119198

‎rclcpp/src/rclcpp/rate.cpp‎

Lines changed: 112 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
// Copyright 2023 Open Source Robotics Foundation, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "rclcpp/rate.hpp"
16+
17+
#include <stdexcept>
18+
19+
namespace rclcpp
20+
{
21+
22+
Rate::Rate(
23+
const double rate, Clock::SharedPtr clock)
24+
: clock_(clock), period_(0, 0), last_interval_(clock_->now())
25+
{
26+
if (rate <= 0.0) {
27+
throw std::invalid_argument{"rate must be greater than 0"};
28+
}
29+
period_ = Duration::from_seconds(1.0 / rate);
30+
}
31+
32+
Rate::Rate(
33+
const Duration & period, Clock::SharedPtr clock)
34+
: clock_(clock), period_(period), last_interval_(clock_->now())
35+
{
36+
if (period <= Duration(0, 0)) {
37+
throw std::invalid_argument{"period must be greater than 0"};
38+
}
39+
}
40+
41+
bool
42+
Rate::sleep()
43+
{
44+
// Time coming into sleep
45+
auto now = clock_->now();
46+
// Time of next interval
47+
auto next_interval = last_interval_ + period_;
48+
// Detect backwards time flow
49+
if (now < last_interval_) {
50+
// Best thing to do is to set the next_interval to now + period
51+
next_interval = now + period_;
52+
}
53+
// Update the interval
54+
last_interval_ += period_;
55+
// If the time_to_sleep is negative or zero, don't sleep
56+
if (next_interval <= now) {
57+
// If an entire cycle was missed then reset next interval.
58+
// This might happen if the loop took more than a cycle.
59+
// Or if time jumps forward.
60+
if (now > next_interval + period_) {
61+
last_interval_ = now + period_;
62+
}
63+
// Either way do not sleep and return false
64+
return false;
65+
}
66+
// Calculate the time to sleep
67+
auto time_to_sleep = next_interval - now;
68+
// Sleep (will get interrupted by ctrl-c, may not sleep full time)
69+
return clock_->sleep_for(time_to_sleep);
70+
}
71+
72+
bool
73+
Rate::is_steady() const
74+
{
75+
return clock_->get_clock_type() == RCL_STEADY_TIME;
76+
}
77+
78+
rcl_clock_type_t
79+
Rate::get_type() const
80+
{
81+
return clock_->get_clock_type();
82+
}
83+
84+
void
85+
Rate::reset()
86+
{
87+
last_interval_ = clock_->now();
88+
}
89+
90+
Duration
91+
Rate::period() const
92+
{
93+
return period_;
94+
}
95+
96+
WallRate::WallRate(const double rate)
97+
: Rate(rate, std::make_shared<Clock>(RCL_STEADY_TIME))
98+
{}
99+
100+
WallRate::WallRate(const Duration & period)
101+
: Rate(period, std::make_shared<Clock>(RCL_STEADY_TIME))
102+
{}
103+
104+
ROSRate::ROSRate(const double rate)
105+
: Rate(rate, std::make_shared<Clock>(RCL_ROS_TIME))
106+
{}
107+
108+
ROSRate::ROSRate(const Duration & period)
109+
: Rate(period, std::make_shared<Clock>(RCL_ROS_TIME))
110+
{}
111+
112+
} // namespace rclcpp

0 commit comments

Comments
 (0)