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 
29 class RateBase
30 {
31 public:
33 
34  virtual bool sleep() = 0;
35  virtual bool is_steady() const = 0;
36  virtual void reset() = 0;
37 };
38 
42 
43 template<class Clock = std::chrono::high_resolution_clock>
44 class GenericRate : public RateBase
45 {
46 public:
48 
49  explicit GenericRate(double rate)
50  : GenericRate<Clock>(
51  duration_cast<nanoseconds>(duration<double>(1.0 / rate)))
52  {}
54  : period_(period), last_interval_(Clock::now())
55  {}
56 
57  virtual bool
59  {
60  // Time coming into sleep
61  auto now = Clock::now();
62  // Time of next interval
63  auto next_interval = last_interval_ + period_;
64  // Detect backwards time flow
65  if (now < last_interval_) {
66  // Best thing to do is to set the next_interval to now + period
67  next_interval = now + period_;
68  }
69  // Calculate the time to sleep
70  auto time_to_sleep = next_interval - now;
71  // Update the interval
72  last_interval_ += period_;
73  // If the time_to_sleep is negative or zero, don't sleep
74  if (time_to_sleep <= std::chrono::seconds(0)) {
75  // If an entire cycle was missed then reset next interval.
76  // This might happen if the loop took more than a cycle.
77  // Or if time jumps forward.
78  if (now > next_interval + period_) {
79  last_interval_ = now + period_;
80  }
81  // Either way do not sleep and return false
82  return false;
83  }
84  // Sleep (will get interrupted by ctrl-c, may not sleep full time)
85  rclcpp::sleep_for(time_to_sleep);
86  return true;
87  }
88 
89  virtual bool
90  is_steady() const
91  {
92  return Clock::is_steady;
93  }
94 
95  virtual void
97  {
98  last_interval_ = Clock::now();
99  }
100 
102  {
103  return period_;
104  }
105 
106 private:
108 
109  std::chrono::nanoseconds period_;
112 };
113 
116 
117 } // namespace rclcpp
118 
119 #endif // RCLCPP__RATE_HPP_
#define RCLCPP_DISABLE_COPY(...)
Definition: macros.hpp:26
virtual bool sleep()=0
virtual bool is_steady() const
Definition: rate.hpp:90
Definition: clock.hpp:83
std::chrono::nanoseconds period() const
Definition: rate.hpp:101
Definition: allocator_common.hpp:24
virtual void reset()
Definition: rate.hpp:96
GenericRate(std::chrono::nanoseconds period)
Definition: rate.hpp:53
T duration_cast(T... args)
Definition: rate.hpp:29
bool sleep_for(const std::chrono::nanoseconds &nanoseconds)
Use the global condition variable to block for the specified amount of time.
virtual bool sleep()
Definition: rate.hpp:58
#define RCLCPP_SMART_PTR_DEFINITIONS(...)
Definition: macros.hpp:36
virtual void reset()=0
#define RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(...)
Definition: macros.hpp:51
virtual bool is_steady() const =0
Definition: rate.hpp:44