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