rclcpp  master
C++ ROS Client Library API
timer.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__TIMER_HPP_
16 #define RCLCPP__TIMER_HPP_
17 
18 #include <chrono>
19 #include <functional>
20 #include <memory>
21 #include <sstream>
22 #include <thread>
23 #include <type_traits>
24 #include <utility>
25 
27 #include "rclcpp/macros.hpp"
28 #include "rclcpp/rate.hpp"
29 #include "rclcpp/utilities.hpp"
31 
32 #include "rcl/error_handling.h"
33 #include "rcl/timer.h"
34 
35 #include "rmw/error_handling.h"
36 #include "rmw/rmw.h"
37 
38 namespace rclcpp
39 {
40 
41 class TimerBase
42 {
43 public:
45 
47  explicit TimerBase(std::chrono::nanoseconds period);
48 
50  ~TimerBase();
51 
53  void
54  cancel();
55 
57  void
58  reset();
59 
61  virtual void
62  execute_callback() = 0;
63 
67 
69 
73 
75 
76  virtual bool is_steady() = 0;
77 
79 
85  bool is_ready();
86 
87 protected:
89 };
90 
91 
94 
96 template<
97  typename FunctorT,
98  class Clock,
99  typename std::enable_if<
102  Clock::is_steady
103  >::type * = nullptr
104 >
105 class GenericTimer : public TimerBase
106 {
107 public:
109 
110 
111 
115  GenericTimer(std::chrono::nanoseconds period, FunctorT && callback)
116  : TimerBase(period), callback_(std::forward<FunctorT>(callback))
117  {
118  }
119 
121  virtual ~GenericTimer()
122  {
123  // Stop the timer from running.
124  cancel();
125  }
126 
127  void
129  {
130  rcl_ret_t ret = rcl_timer_call(timer_handle_.get());
131  if (ret == RCL_RET_TIMER_CANCELED) {
132  return;
133  }
134  if (ret != RCL_RET_OK) {
135  throw std::runtime_error("Failed to notify timer that callback occurred");
136  }
137  execute_callback_delegate<>();
138  }
139 
140  // void specialization
141  template<
142  typename CallbackT = FunctorT,
143  typename std::enable_if<
145  >::type * = nullptr
146  >
147  void
149  {
150  callback_();
151  }
152 
153  template<
154  typename CallbackT = FunctorT,
155  typename std::enable_if<
157  >::type * = nullptr
158  >
159  void
161  {
162  callback_(*this);
163  }
164 
165  virtual bool
167  {
168  return Clock::is_steady;
169  }
170 
171 protected:
173 
174  FunctorT callback_;
175 };
176 
177 template<typename CallbackType>
178 using WallTimer = GenericTimer<CallbackType, std::chrono::steady_clock>;
179 
180 } // namespace rclcpp
181 
182 #endif // RCLCPP__TIMER_HPP_
Generic timer templated on the clock type. Periodically executes a user-specified callback...
Definition: timer.hpp:105
#define RCLCPP_DISABLE_COPY(...)
Definition: macros.hpp:26
virtual bool is_steady()
Is the clock steady (i.e. is the time between ticks constant?)
Definition: timer.hpp:166
rmw_ret_t rcl_ret_t
Definition: clock.hpp:83
#define RCL_RET_TIMER_CANCELED
Definition: timer.hpp:41
Definition: allocator_common.hpp:24
#define RCL_RET_OK
rcl_ret_t rcl_timer_call(rcl_timer_t *timer)
std::chrono::nanoseconds time_until_trigger()
Check how long the timer has until its next scheduled callback.
std::shared_ptr< const rcl_timer_t > get_timer_handle()
void execute_callback()
Definition: timer.hpp:128
virtual bool is_steady()=0
Is the clock steady (i.e. is the time between ticks constant?)
std::shared_ptr< rcl_timer_t > timer_handle_
Definition: timer.hpp:88
#define RCLCPP_SMART_PTR_DEFINITIONS(...)
Definition: macros.hpp:36
void execute_callback_delegate()
Definition: timer.hpp:148
Definition: function_traits.hpp:143
TimerBase(std::chrono::nanoseconds period)
bool is_ready()
Check if the timer is ready to trigger the callback.
#define RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(...)
Definition: macros.hpp:51
virtual ~GenericTimer()
Default destructor.
Definition: timer.hpp:121
#define RCLCPP_PUBLIC
Definition: visibility_control.hpp:50
virtual void execute_callback()=0