condition_variable.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2020, Willow Garage, Inc.
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 // Distributed under the Boost Software License, Version 1.0. (See
29 // accompanying file LICENSE_1_0.txt or copy at
30 // http://www.boost.org/LICENSE_1_0.txt)
31 // (C) Copyright 2007-10 Anthony Williams
32 // (C) Copyright 2011-2012 Vicente J. Botet Escriba
33 
34 #ifndef ROSCPP_INTERNAL_CONDITION_VARIABLE_H
35 #define ROSCPP_INTERNAL_CONDITION_VARIABLE_H
36 
37 #include <boost/thread/condition_variable.hpp>
38 
39 namespace ros {
40 namespace internal {
41 
42 #if !defined(BOOST_THREAD_PLATFORM_PTHREAD) || \
43  defined(BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC) || \
44  defined(BOOST_THREAD_INTERNAL_CLOCK_IS_MONO)
45 using condition_variable_monotonic = boost::condition_variable;
46 
47 #else
48 
50 private:
51 #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
52  pthread_mutex_t internal_mutex;
53 #endif
54  pthread_cond_t cond;
55 
56 public:
58  int res;
59 #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
60  res = pthread_mutex_init(&internal_mutex, NULL);
61  if (res)
62  {
63  boost::throw_exception(boost::thread_resource_error(res, "ros::internal::condition_variable_monotonic::condition_variable_monotonic() constructor failed in pthread_mutex_init"));
64  }
65 #endif
66 
67  // res = boost::detail::monotonic_pthread_cond_init(cond);
68  pthread_condattr_t attr;
69  res = pthread_condattr_init(&attr);
70  if (res == 0) {
71  pthread_condattr_setclock(&attr, CLOCK_MONOTONIC);
72  res = pthread_cond_init(&cond, &attr);
73  pthread_condattr_destroy(&attr);
74  }
75 
76  if (res)
77  {
78 #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
79  BOOST_VERIFY(!pthread_mutex_destroy(&internal_mutex));
80 #endif
81  boost::throw_exception(boost::thread_resource_error(res, "ros::internal::condition_variable_monotonic::condition_variable() constructor failed in detail::monotonic_pthread_cond_init"));
82  }
83  }
84 
85  void notify_one() BOOST_NOEXCEPT
86  {
87 #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
88  boost::pthread::pthread_mutex_scoped_lock internal_lock(&internal_mutex);
89 #endif
90  BOOST_VERIFY(!pthread_cond_signal(&cond));
91  }
92 
93  void notify_all() BOOST_NOEXCEPT
94  {
95 #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
96  boost::pthread::pthread_mutex_scoped_lock internal_lock(&internal_mutex);
97 #endif
98  BOOST_VERIFY(!pthread_cond_broadcast(&cond));
99  }
100 
101  template <class Duration>
102  boost::cv_status wait_until(
103  boost::unique_lock<boost::mutex> &lock,
104  const boost::chrono::time_point<boost::chrono::steady_clock, Duration> &t)
105  {
106  typedef boost::chrono::time_point<boost::chrono::steady_clock, boost::chrono::nanoseconds>
107  nano_sys_tmpt;
108  wait_until(lock,
109  nano_sys_tmpt(boost::chrono::ceil<boost::chrono::nanoseconds>(t.time_since_epoch())));
110  return boost::chrono::steady_clock::now() < t ?
111  boost::cv_status::no_timeout :
112  boost::cv_status::timeout;
113  }
114 
115  template <class Clock, class Duration>
116  boost::cv_status wait_until(
117  boost::unique_lock<boost::mutex> &lock,
118  const boost::chrono::time_point<Clock, Duration> &t)
119  {
120  boost::chrono::steady_clock::time_point s_now = boost::chrono::steady_clock::now();
121  typename Clock::time_point c_now = Clock::now();
122  wait_until(lock, s_now + boost::chrono::ceil<boost::chrono::nanoseconds>(t - c_now));
123  return Clock::now() < t ? boost::cv_status::no_timeout : boost::cv_status::timeout;
124  }
125 
126  template <class Rep, class Period>
127  boost::cv_status wait_for(
128  boost::unique_lock<boost::mutex> &lock,
129  const boost::chrono::duration<Rep, Period> &d)
130  {
131  boost::chrono::steady_clock::time_point c_now = boost::chrono::steady_clock::now();
132  wait_until(lock, c_now + boost::chrono::ceil<boost::chrono::nanoseconds>(d));
133  return boost::chrono::steady_clock::now() - c_now < d ?
134  boost::cv_status::no_timeout :
135  boost::cv_status::timeout;
136  }
137 
138  boost::cv_status wait_until(
139  boost::unique_lock<boost::mutex> &lk,
140  boost::chrono::time_point<boost::chrono::steady_clock, boost::chrono::nanoseconds> tp)
141  {
142  boost::chrono::nanoseconds d = tp.time_since_epoch();
143  timespec ts = boost::detail::to_timespec(d);
144  if (do_wait_until(lk, ts))
145  return boost::cv_status::no_timeout;
146  else
147  return boost::cv_status::timeout;
148  }
149 
150  void wait(boost::unique_lock<boost::mutex> &m)
151  {
152  int res = 0;
153  {
154 #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
155  boost::thread_cv_detail::lock_on_exit<boost::unique_lock<boost::mutex> > guard;
156  boost::detail::interruption_checker check_for_interruption(&internal_mutex, &cond);
157  pthread_mutex_t *the_mutex = &internal_mutex;
158  guard.activate(m);
159  res = pthread_cond_wait(&cond, the_mutex);
160 #if BOOST_VERSION >= 106500
161  check_for_interruption.check();
162  guard.deactivate();
163 #endif
164 #else
165  pthread_mutex_t *the_mutex = m.mutex()->native_handle();
166  res = pthread_cond_wait(&cond, the_mutex);
167 #endif
168  }
169 #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
170  boost::this_thread::interruption_point();
171 #endif
172  if (res && res != EINTR)
173  {
174  boost::throw_exception(boost::condition_error(res, "ros::internal::condition_variable_monotonic::wait failed in pthread_cond_wait"));
175  }
176  }
177 
178  bool do_wait_until(
179  boost::unique_lock<boost::mutex> &m,
180  struct timespec const &timeout)
181  {
182  int cond_res;
183  {
184 #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
185  boost::thread_cv_detail::lock_on_exit<boost::unique_lock<boost::mutex> > guard;
186  boost::detail::interruption_checker check_for_interruption(&internal_mutex, &cond);
187  pthread_mutex_t *the_mutex = &internal_mutex;
188  guard.activate(m);
189  cond_res = pthread_cond_timedwait(&cond, the_mutex, &timeout);
190 #if BOOST_VERSION >= 106500
191  check_for_interruption.check();
192  guard.deactivate();
193 #endif
194 #else
195  pthread_mutex_t *the_mutex = m.mutex()->native_handle();
196  cond_res = pthread_cond_timedwait(&cond, the_mutex, &timeout);
197 #endif
198  }
199 #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
200  boost::this_thread::interruption_point();
201 #endif
202  if (cond_res == ETIMEDOUT)
203  {
204  return false;
205  }
206  if (cond_res)
207  {
208  boost::throw_exception(boost::condition_error(cond_res, "ros::internal::condition_variable_monotonic::do_wait_until failed in pthread_cond_timedwait"));
209  }
210  return true;
211  }
212 };
213 #endif
214 
215 } // namespace internal
216 } // namespaec ros
217 
218 #endif // ROSCPP_INTERNAL_CONDITION_VARIABLE_H
d
boost::condition_variable condition_variable_monotonic


roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim, Dirk Thomas
autogenerated on Mon Nov 2 2020 03:52:26