rclcpp  master
C++ ROS Client Library API
rate.hpp
Go to the documentation of this file.
1 // Copyright 2014 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 #ifndef RCLCPP__RATE_HPP_
16 #define RCLCPP__RATE_HPP_
17 
18 #include <chrono>
19 #include <memory>
20 #include <thread>
21 
22 #include "rclcpp/macros.hpp"
23 #include "rclcpp/utilities.hpp"
25 
26 namespace rclcpp
27 {
28 namespace rate
29 {
30 
31 class RateBase
32 {
33 public:
35 
36  virtual bool sleep() = 0;
37  virtual bool is_steady() const = 0;
38  virtual void reset() = 0;
39 };
40 
41 using std::chrono::duration;
42 using std::chrono::duration_cast;
43 using std::chrono::nanoseconds;
44 
45 template<class Clock = std::chrono::high_resolution_clock>
46 class GenericRate : public RateBase
47 {
48 public:
50 
51  explicit GenericRate(double rate)
52  : GenericRate<Clock>(
53  duration_cast<nanoseconds>(duration<double>(1.0 / rate)))
54  {}
55  explicit GenericRate(std::chrono::nanoseconds period)
56  : period_(period), last_interval_(Clock::now())
57  {}
58 
59  virtual bool
61  {
62  // Time coming into sleep
63  auto now = Clock::now();
64  // Time of next interval
65  auto next_interval = last_interval_ + period_;
66  // Detect backwards time flow
67  if (now < last_interval_) {
68  // Best thing to do is to set the next_interval to now + period
69  next_interval = now + period_;
70  }
71  // Calculate the time to sleep
72  auto time_to_sleep = next_interval - now;
73  // Update the interval
74  last_interval_ += period_;
75  // If the time_to_sleep is negative or zero, don't sleep
76  if (time_to_sleep <= std::chrono::seconds(0)) {
77  // If an entire cycle was missed then reset next interval.
78  // This might happen if the loop took more than a cycle.
79  // Or if time jumps forward.
80  if (now > next_interval + period_) {
81  last_interval_ = now + period_;
82  }
83  // Either way do not sleep and return false
84  return false;
85  }
86  // Sleep (will get interrupted by ctrl-c, may not sleep full time)
87  rclcpp::utilities::sleep_for(time_to_sleep);
88  return true;
89  }
90 
91  virtual bool
92  is_steady() const
93  {
94  return Clock::is_steady;
95  }
96 
97  virtual void
99  {
100  last_interval_ = Clock::now();
101  }
102 
103  std::chrono::nanoseconds period() const
104  {
105  return period_;
106  }
107 
108 private:
110 
111  std::chrono::nanoseconds period_;
112  using ClockDurationNano = std::chrono::duration<typename Clock::rep, std::nano>;
113  std::chrono::time_point<Clock, ClockDurationNano> last_interval_;
114 };
115 
118 
119 } // namespace rate
120 } // namespace rclcpp
121 
122 #endif // RCLCPP__RATE_HPP_
GenericRate(std::chrono::nanoseconds period)
Definition: rate.hpp:55
#define RCLCPP_DISABLE_COPY(...)
Definition: macros.hpp:26
virtual bool is_steady() const =0
Definition: allocator_common.hpp:24
virtual bool sleep()
Definition: rate.hpp:60
Definition: rate.hpp:46
#define RCLCPP_SMART_PTR_DEFINITIONS(...)
Definition: macros.hpp:36
virtual void reset()=0
#define RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(...)
Definition: macros.hpp:51
bool sleep_for(const std::chrono::nanoseconds &nanoseconds)
Use the global condition variable to block for the specified amount of time.
virtual bool sleep()=0
virtual bool is_steady() const
Definition: rate.hpp:92
Definition: rate.hpp:31
std::chrono::nanoseconds period() const
Definition: rate.hpp:103
virtual void reset()
Definition: rate.hpp:98