steady_timer.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017, Felix Ruess, Roboception GmbH
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions are met:
6  * * Redistributions of source code must retain the above copyright notice,
7  * this list of conditions and the following disclaimer.
8  * * Redistributions in binary form must reproduce the above copyright
9  * notice, this list of conditions and the following disclaimer in the
10  * documentation and/or other materials provided with the distribution.
11  * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
12  * contributors may be used to endorse or promote products derived from
13  * this software without specific prior written permission.
14  *
15  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25  * POSSIBILITY OF SUCH DAMAGE.
26  */
27 
28 // Make sure we use CLOCK_MONOTONIC for the condition variable if not Apple.
29 #if !defined(__APPLE__) && !defined(WIN32)
30 #define BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
31 #endif
32 
33 #include "ros/steady_timer.h"
34 #include "ros/timer_manager.h"
35 
36 // check if we have really included the backported boost condition variable
37 // just in case someone messes with the include order...
38 #if BOOST_VERSION < 106100
39 #ifndef USING_BACKPORTED_BOOST_CONDITION_VARIABLE
40 #error "steady timer needs boost version >= 1.61 or the backported headers!"
41 #endif
42 #endif
43 
44 namespace ros
45 {
46 
47 #if !defined(WIN32)
48 // specialization for SteadyTimer, to make sure we use a version with wait_until that uses the monotonic clock
49 template<>
51 {
52  SteadyTime current;
53  while (!quit_)
54  {
55  SteadyTime sleep_end;
56 
57  boost::mutex::scoped_lock lock(timers_mutex_);
58 
59  current = SteadyTime::now();
60 
61  {
62  boost::mutex::scoped_lock waitlock(waiting_mutex_);
63 
64  if (waiting_.empty())
65  {
66  sleep_end = current + WallDuration(0.1);
67  }
68  else
69  {
70  TimerInfoPtr info = findTimer(waiting_.front());
71 
72  while (!waiting_.empty() && info && info->next_expected <= current)
73  {
74  current = SteadyTime::now();
75 
76  //ROS_DEBUG("Scheduling timer callback for timer [%d] of period [%f], [%f] off expected", info->handle, info->period.toSec(), (current - info->next_expected).toSec());
77  CallbackInterfacePtr cb(boost::make_shared<TimerQueueCallback>(this, info, info->last_expected, info->last_real, info->next_expected, info->last_expired, current));
78  info->callback_queue->addCallback(cb, (uint64_t)info.get());
79 
80  waiting_.pop_front();
81 
82  if (waiting_.empty())
83  {
84  break;
85  }
86 
87  info = findTimer(waiting_.front());
88  }
89 
90  if (info)
91  {
92  sleep_end = info->next_expected;
93  }
94  }
95  }
96 
97  while (!new_timer_ && SteadyTime::now() < sleep_end && !quit_)
98  {
99  current = SteadyTime::now();
100 
101  if (current >= sleep_end)
102  {
103  break;
104  }
105 
106  // requires boost 1.61 for wait_until to actually use the steady clock
107  // see: https://svn.boost.org/trac/boost/ticket/6377
108  boost::chrono::steady_clock::time_point end_tp(boost::chrono::nanoseconds(sleep_end.toNSec()));
109  timers_cond_.wait_until(lock, end_tp);
110  }
111 
112  new_timer_ = false;
113  }
114 }
115 #endif
116 
118  : started_(false)
119  , timer_handle_(-1)
120 { }
121 
123 {
124  ROS_DEBUG("SteadyTimer deregistering callbacks.");
125  stop();
126 }
127 
129 {
130  return started_;
131 }
132 
134 {
135  if (!started_)
136  {
137  VoidConstPtr tracked_object;
139  {
140  tracked_object = tracked_object_.lock();
141  }
143  started_ = true;
144  }
145 }
146 
148 {
149  if (started_)
150  {
151  started_ = false;
153  timer_handle_ = -1;
154  }
155 }
156 
158 {
159  return !period_.isZero();
160 }
161 
163 {
164  if (!isValid() || timer_handle_ == -1)
165  {
166  return false;
167  }
168 
170 }
171 
172 void SteadyTimer::Impl::setPeriod(const WallDuration& period, bool reset)
173 {
174  period_ = period;
176 }
177 
178 
180 : impl_(new Impl)
181 {
182  impl_->period_ = ops.period;
183  impl_->callback_ = ops.callback;
184  impl_->callback_queue_ = ops.callback_queue;
185  impl_->tracked_object_ = ops.tracked_object;
186  impl_->has_tracked_object_ = (ops.tracked_object != NULL);
187  impl_->oneshot_ = ops.oneshot;
188 }
189 
191 {
192  impl_ = rhs.impl_;
193 }
194 
196 {
197 }
198 
200 {
201  if (impl_)
202  {
203  impl_->start();
204  }
205 }
206 
208 {
209  if (impl_)
210  {
211  impl_->stop();
212  }
213 }
214 
216 {
217  if (impl_)
218  {
219  return impl_->hasPending();
220  }
221 
222  return false;
223 }
224 
225 void SteadyTimer::setPeriod(const WallDuration& period, bool reset)
226 {
227  if (impl_)
228  {
229  impl_->setPeriod(period, reset);
230  }
231 }
232 
233 }
void start()
Start the timer. Does nothing if the timer is already started.
void remove(int32_t handle)
void stop()
Stop the timer. Once this call returns, no more callbacks will be called. Does nothing if the timer i...
VoidConstWPtr tracked_object_
Definition: steady_timer.h:116
SteadyTimerCallback callback
The callback to call.
WallDuration period
The period to call the callback at.
SteadyTimerCallback callback_
Definition: steady_timer.h:114
bool hasPending(int32_t handle)
void setPeriod(const WallDuration &period, bool reset=true)
Set the period of this timer.
int32_t add(const D &period, const boost::function< void(const E &)> &callback, CallbackQueueInterface *callback_queue, const VoidConstPtr &tracked_object, bool oneshot)
void setPeriod(const WallDuration &period, bool reset=true)
static SteadyTime now()
CallbackQueueInterface * callback_queue_
Definition: steady_timer.h:115
Manages a steady-clock timer callback.
Definition: steady_timer.h:46
bool hasPending()
Returns whether or not the timer has any pending events to call.
static TimerManager & global()
CallbackQueueInterface * callback_queue
Queue to add callbacks to. If NULL, the global callback queue will be used.
Encapsulates all options available for starting a timer.
#define ROS_DEBUG(...)
void setPeriod(int32_t handle, const D &period, bool reset=true)


roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim
autogenerated on Fri Oct 4 2019 03:16:56