From 2577130415ff80667268589f7223b230fb6c6c1d Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Fri, 2 Nov 2018 21:58:32 +0100 Subject: [PATCH 1/9] roscpp: fix potential busy-wait loop caused by backported Boost condition_variable (fix ros/ros_comm#1343) https://github.com/ros/ros_comm/pull/1014 and https://github.com/ros/ros_comm/pull/1250 introduced a backported version of boost::condition_variable, where support for steady (monotonic) clocks has been added in version 1.61. But the namespace of the backported version was not changed and the symbol names might clash with the original version. Because the underlying clock used for the condition_variable is set in the constructor and must be consistent with the the expectations within member variables. The compiler might choose to inline one or the other or both, and is more likely to do so for optimized Release builds. But if it does not, the symbol ends up in the symbol table of roscpp and depending on which other libraries will be linked into the process it is unpredictable which of the two versions will be actually called at the end. In case the constructor defined in `/usr/include/boost/thread/pthread/condition_variable.hpp` was called and did not configure the internal pthread condition variable for monotonic clock, each call to the backported do_wait_until() method with a monotonic timestamp will return immediately and hence causes `CallbackQueue::callOne(timeout)` or `CallbackQueue::callAvailable(timeout)` to return immediately. This patch changes the namespace of the backported condition_variable implementation to boost_161. This removes the ambiguity with the original definition if both are used in the same process. --- clients/roscpp/CMakeLists.txt | 12 +++++ .../include/boost_161_condition_variable.h | 7 ++- .../boost_161_pthread_condition_variable.h | 54 +++---------------- ...boost_161_pthread_condition_variable_fwd.h | 36 ++++--------- clients/roscpp/include/ros/callback_queue.h | 17 ++---- clients/roscpp/include/ros/common.h.in | 13 +++++ clients/roscpp/include/ros/rosout_appender.h | 8 ++- .../roscpp/include/ros/service_server_link.h | 5 +- clients/roscpp/include/ros/timer_manager.h | 24 ++++----- clients/roscpp/src/libros/callback_queue.cpp | 13 ----- .../src/libros/internal_timer_manager.cpp | 13 ----- clients/roscpp/src/libros/steady_timer.cpp | 13 ----- 12 files changed, 69 insertions(+), 146 deletions(-) diff --git a/clients/roscpp/CMakeLists.txt b/clients/roscpp/CMakeLists.txt index 37a8587a37..6b666fc629 100644 --- a/clients/roscpp/CMakeLists.txt +++ b/clients/roscpp/CMakeLists.txt @@ -20,6 +20,18 @@ list(GET roscpp_VERSION_LIST 0 roscpp_VERSION_MAJOR) list(GET roscpp_VERSION_LIST 1 roscpp_VERSION_MINOR) list(GET roscpp_VERSION_LIST 2 roscpp_VERSION_PATCH) +# Make sure we use CLOCK_MONOTONIC for the condition variable wait_for if not Apple. +if(NOT APPLE) + if(Boost_VERSION LESS 106100) + set(ROSCPP_USE_BACKPORTED_BOOST_CONDITION_VARIABLE_IMPLEMENTATION ON) + endif() + if(Boost_VERSION LESS 106700) + # CLOCK_MONOTONIC became the default in Boost 1.67: + # https://github.com/boostorg/thread/commit/1e84b978b2bb0aae830cc14533dea3b7ddda5cde + add_definitions(-DBOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC) + endif() +endif() + configure_file(${CMAKE_CURRENT_SOURCE_DIR}/include/ros/common.h.in ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/ros/common.h) find_package(Boost REQUIRED COMPONENTS chrono filesystem system) diff --git a/clients/roscpp/include/boost_161_condition_variable.h b/clients/roscpp/include/boost_161_condition_variable.h index ee2ff46188..7277ce1dc4 100644 --- a/clients/roscpp/include/boost_161_condition_variable.h +++ b/clients/roscpp/include/boost_161_condition_variable.h @@ -1,5 +1,5 @@ -#ifndef BOOST_THREAD_CONDITION_VARIABLE_HPP -#define BOOST_THREAD_CONDITION_VARIABLE_HPP +#ifndef BOOST_161_THREAD_CONDITION_VARIABLE_HPP +#define BOOST_161_THREAD_CONDITION_VARIABLE_HPP // condition_variable.hpp // @@ -12,6 +12,9 @@ #include #if defined(BOOST_THREAD_PLATFORM_WIN32) #include +namespace boost_161 { + using condition_variable = boost::condition_variable; +} #elif defined(BOOST_THREAD_PLATFORM_PTHREAD) //#include #include "boost_161_pthread_condition_variable.h" diff --git a/clients/roscpp/include/boost_161_pthread_condition_variable.h b/clients/roscpp/include/boost_161_pthread_condition_variable.h index 8100447bae..2aca774a48 100644 --- a/clients/roscpp/include/boost_161_pthread_condition_variable.h +++ b/clients/roscpp/include/boost_161_pthread_condition_variable.h @@ -1,5 +1,5 @@ -#ifndef BOOST_THREAD_CONDITION_VARIABLE_PTHREAD_HPP -#define BOOST_THREAD_CONDITION_VARIABLE_PTHREAD_HPP +#ifndef BOOST_161_THREAD_CONDITION_VARIABLE_PTHREAD_HPP +#define BOOST_161_THREAD_CONDITION_VARIABLE_PTHREAD_HPP // Distributed under the Boost Software License, Version 1.0. (See // accompanying file LICENSE_1_0.txt or copy at // http://www.boost.org/LICENSE_1_0.txt) @@ -7,57 +7,15 @@ // (C) Copyright 2011-2012 Vicente J. Botet Escriba // make sure we include our backported version first!! -// (before the system version might be included via some of the other header files) #include "boost_161_pthread_condition_variable_fwd.h" -#include -#include -#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS -#include -#endif -//#include -#ifdef BOOST_THREAD_USES_CHRONO -#include -#include -#endif -#include +// include upstream +#include #include -namespace boost +namespace boost_161 { -#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS - namespace this_thread - { - void BOOST_THREAD_DECL interruption_point(); - } -#endif - - namespace thread_cv_detail - { - template - struct lock_on_exit - { - MutexType* m; - - lock_on_exit(): - m(0) - {} - - void activate(MutexType& m_) - { - m_.unlock(); - m=&m_; - } - ~lock_on_exit() - { - if(m) - { - m->lock(); - } - } - }; - } inline void condition_variable::wait(unique_lock& m) { @@ -154,7 +112,7 @@ namespace boost { boost::throw_exception(thread_resource_error(res, "boost::condition_variable_any::condition_variable_any() failed in pthread_mutex_init")); } - int const res2 = detail::monotonic_pthread_cond_init(cond); + int const res2 = detail_161::monotonic_pthread_cond_init(cond); if(res2) { BOOST_VERIFY(!pthread_mutex_destroy(&internal_mutex)); diff --git a/clients/roscpp/include/boost_161_pthread_condition_variable_fwd.h b/clients/roscpp/include/boost_161_pthread_condition_variable_fwd.h index 2e4e4c6761..0a58b16bf6 100644 --- a/clients/roscpp/include/boost_161_pthread_condition_variable_fwd.h +++ b/clients/roscpp/include/boost_161_pthread_condition_variable_fwd.h @@ -1,38 +1,22 @@ -#ifndef BOOST_THREAD_PTHREAD_CONDITION_VARIABLE_FWD_HPP -#define BOOST_THREAD_PTHREAD_CONDITION_VARIABLE_FWD_HPP +#ifndef BOOST_161_THREAD_PTHREAD_CONDITION_VARIABLE_FWD_HPP +#define BOOST_161_THREAD_PTHREAD_CONDITION_VARIABLE_FWD_HPP // Distributed under the Boost Software License, Version 1.0. (See // accompanying file LICENSE_1_0.txt or copy at // http://www.boost.org/LICENSE_1_0.txt) // (C) Copyright 2007-8 Anthony Williams // (C) Copyright 2011-2012 Vicente J. Botet Escriba -// define to check if this backported version was included -#define USING_BACKPORTED_BOOST_CONDITION_VARIABLE 1 - -#include -#include -#include -#include -#include -#include -#include -#include -#if defined BOOST_THREAD_USES_DATETIME -#include -#endif - -#ifdef BOOST_THREAD_USES_CHRONO -#include -#include -#endif -#include -#include +// include upstream +#include #include -namespace boost +namespace boost_161 { - namespace detail { + using namespace boost; + namespace detail = boost::detail; + + namespace detail_161 { inline int monotonic_pthread_cond_init(pthread_cond_t& cond) { #ifdef BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC @@ -100,7 +84,7 @@ namespace boost boost::throw_exception(thread_resource_error(res, "boost::condition_variable::condition_variable() constructor failed in pthread_mutex_init")); } #endif - res = detail::monotonic_pthread_cond_init(cond); + res = detail_161::monotonic_pthread_cond_init(cond); if (res) { #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS diff --git a/clients/roscpp/include/ros/callback_queue.h b/clients/roscpp/include/ros/callback_queue.h index 4fe811d745..44a021f31e 100644 --- a/clients/roscpp/include/ros/callback_queue.h +++ b/clients/roscpp/include/ros/callback_queue.h @@ -37,22 +37,11 @@ // check if we might need to include our own backported version boost::condition_variable // in order to use CLOCK_MONOTONIC for the condition variable -// the include order here is important! -#ifdef BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC -#include -#if BOOST_VERSION < 106100 -// use backported version of boost condition variable, see https://svn.boost.org/trac/boost/ticket/6377 -#include "boost_161_condition_variable.h" -#else // Boost version is 1.61 or greater and has the steady clock fixes -#include -#endif -#else // !BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC -#include -#endif // BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC +#include "ros/common.h" +#include ROSCPP_BOOST_CONDITION_VARIABLE_HEADER #include "ros/callback_queue_interface.h" #include "ros/time.h" -#include "common.h" #include #include @@ -177,7 +166,7 @@ class ROSCPP_DECL CallbackQueue : public CallbackQueueInterface D_CallbackInfo callbacks_; size_t calling_; boost::mutex mutex_; - boost::condition_variable condition_; + ROSCPP_BOOST_CONDITION_VARIABLE condition_; boost::mutex id_info_mutex_; M_IDInfo id_info_; diff --git a/clients/roscpp/include/ros/common.h.in b/clients/roscpp/include/ros/common.h.in index 5351aeb937..f5655d793f 100644 --- a/clients/roscpp/include/ros/common.h.in +++ b/clients/roscpp/include/ros/common.h.in @@ -38,6 +38,7 @@ #include "ros/serialized_message.h" #include +#include #define ROS_VERSION_MAJOR @roscpp_VERSION_MAJOR@ #define ROS_VERSION_MINOR @roscpp_VERSION_MINOR@ @@ -48,6 +49,18 @@ #define ROS_VERSION_GE(major1, minor1, patch1, major2, minor2, patch2) (ROS_VERSION_COMBINED(major1, minor1, patch1) >= ROS_VERSION_COMBINED(major2, minor2, patch2)) #define ROS_VERSION_MINIMUM(major, minor, patch) ROS_VERSION_GE(ROS_VERSION_MAJOR, ROS_VERSION_MINOR, ROS_VERSION_PATCH, major, minor, patch) +// check if we might need to include our own backported version boost::condition_variable +// in order to use CLOCK_MONOTONIC for the condition variable and for the SteadyTimer +#cmakedefine ROSCPP_USE_BACKPORTED_BOOST_CONDITION_VARIABLE_IMPLEMENTATION +#ifdef ROSCPP_USE_BACKPORTED_BOOST_CONDITION_VARIABLE_IMPLEMENTATION + // use backported version of boost condition variable, see https://svn.boost.org/trac/boost/ticket/6377 + #define ROSCPP_BOOST_CONDITION_VARIABLE_HEADER "boost_161_condition_variable.h" + #define ROSCPP_BOOST_CONDITION_VARIABLE boost_161::condition_variable +#else + #define ROSCPP_BOOST_CONDITION_VARIABLE_HEADER + #define ROSCPP_BOOST_CONDITION_VARIABLE boost::condition_variable +#endif + #include // Import/export for windows dll's and visibility for gcc shared libraries. diff --git a/clients/roscpp/include/ros/rosout_appender.h b/clients/roscpp/include/ros/rosout_appender.h index 5a4b91fc4a..31c1944a7b 100644 --- a/clients/roscpp/include/ros/rosout_appender.h +++ b/clients/roscpp/include/ros/rosout_appender.h @@ -35,8 +35,12 @@ #ifndef ROSCPP_ROSOUT_APPENDER_H #define ROSCPP_ROSOUT_APPENDER_H +// check if we might need to include our own backported version boost::condition_variable +// in order to use CLOCK_MONOTONIC for the condition variable +#include "ros/common.h" +#include ROSCPP_BOOST_CONDITION_VARIABLE_HEADER + #include -#include "common.h" #include #include @@ -73,7 +77,7 @@ class ROSCPP_DECL ROSOutAppender : public ros::console::LogAppender typedef std::vector V_Log; V_Log log_queue_; boost::mutex queue_mutex_; - boost::condition_variable queue_condition_; + ROSCPP_BOOST_CONDITION_VARIABLE queue_condition_; bool shutting_down_; bool disable_topics_; diff --git a/clients/roscpp/include/ros/service_server_link.h b/clients/roscpp/include/ros/service_server_link.h index 55b6109b96..8cf8789fdb 100644 --- a/clients/roscpp/include/ros/service_server_link.h +++ b/clients/roscpp/include/ros/service_server_link.h @@ -35,7 +35,10 @@ #ifndef ROSCPP_SERVICE_SERVER_LINK_H #define ROSCPP_SERVICE_SERVER_LINK_H +// check if we might need to include our own backported version boost::condition_variable +// in order to use CLOCK_MONOTONIC for the condition variable #include "ros/common.h" +#include ROSCPP_BOOST_CONDITION_VARIABLE_HEADER #include #include @@ -64,7 +67,7 @@ class ROSCPP_DECL ServiceServerLink : public boost::enable_shared_from_this -#if BOOST_VERSION < 106100 -// use backported version of boost condition variable, see https://svn.boost.org/trac/boost/ticket/6377 -#include "boost_161_condition_variable.h" -#else // Boost version is 1.61 or greater and has the steady clock fixes -#include -#endif -#else // !BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC -#include -#endif // BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC +#include "ros/common.h" +#include ROSCPP_BOOST_CONDITION_VARIABLE_HEADER #include "ros/forwards.h" #include "ros/time.h" @@ -126,7 +116,7 @@ class TimerManager V_TimerInfo timers_; boost::mutex timers_mutex_; - boost::condition_variable timers_cond_; + ROSCPP_BOOST_CONDITION_VARIABLE timers_cond_; volatile bool new_timer_; boost::mutex waiting_mutex_; @@ -230,7 +220,13 @@ template TimerManager::TimerManager() : new_timer_(false), id_counter_(0), thread_started_(false), quit_(false) { - +#if (BOOST_VERSION < 106700) && !defined(BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC) + ROS_ASSERT_MSG(false, + "ros::TimerManager was instantiated by package " ROS_PACKAGE_NAME + "without that BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC is defined! " + "Be aware that timers might misbehave when system time jumps, " + "e.g. due to network time corrections."); +#endif } template diff --git a/clients/roscpp/src/libros/callback_queue.cpp b/clients/roscpp/src/libros/callback_queue.cpp index b2bab574b3..c38fcbde95 100644 --- a/clients/roscpp/src/libros/callback_queue.cpp +++ b/clients/roscpp/src/libros/callback_queue.cpp @@ -32,23 +32,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ -// Make sure we use CLOCK_MONOTONIC for the condition variable wait_for if not Apple. -#ifndef __APPLE__ -#define BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC -#endif - #include "ros/callback_queue.h" #include "ros/assert.h" #include -// check if we have really included the backported boost condition variable -// just in case someone messes with the include order... -#if BOOST_VERSION < 106100 -#ifndef USING_BACKPORTED_BOOST_CONDITION_VARIABLE -#error "needs boost version >= 1.61 or the backported headers!" -#endif -#endif - namespace ros { diff --git a/clients/roscpp/src/libros/internal_timer_manager.cpp b/clients/roscpp/src/libros/internal_timer_manager.cpp index 4f5f751412..67bb5d214c 100644 --- a/clients/roscpp/src/libros/internal_timer_manager.cpp +++ b/clients/roscpp/src/libros/internal_timer_manager.cpp @@ -25,22 +25,9 @@ * POSSIBILITY OF SUCH DAMAGE. */ -// Make sure we use CLOCK_MONOTONIC for the condition variable if not Apple. -#if !defined(__APPLE__) && !defined(WIN32) -#define BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC -#endif - #include "ros/timer_manager.h" #include "ros/internal_timer_manager.h" -// check if we have really included the backported boost condition variable -// just in case someone messes with the include order... -#if BOOST_VERSION < 106100 -#ifndef USING_BACKPORTED_BOOST_CONDITION_VARIABLE -#error "steady timer needs boost version >= 1.61 or the backported headers!" -#endif -#endif - namespace ros { diff --git a/clients/roscpp/src/libros/steady_timer.cpp b/clients/roscpp/src/libros/steady_timer.cpp index b4340ecbb1..8c0517498c 100644 --- a/clients/roscpp/src/libros/steady_timer.cpp +++ b/clients/roscpp/src/libros/steady_timer.cpp @@ -25,22 +25,9 @@ * POSSIBILITY OF SUCH DAMAGE. */ -// Make sure we use CLOCK_MONOTONIC for the condition variable if not Apple. -#if !defined(__APPLE__) && !defined(WIN32) -#define BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC -#endif - #include "ros/steady_timer.h" #include "ros/timer_manager.h" -// check if we have really included the backported boost condition variable -// just in case someone messes with the include order... -#if BOOST_VERSION < 106100 -#ifndef USING_BACKPORTED_BOOST_CONDITION_VARIABLE -#error "steady timer needs boost version >= 1.61 or the backported headers!" -#endif -#endif - namespace ros { From b4e7cbc31dd745a2fa505c157e9c9fc2e544cac4 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Thu, 6 Dec 2018 00:48:35 +0100 Subject: [PATCH 2/9] roscpp: use boost::condition_variable::wait_for() instead of deprecated timed_wait() This fixes ROS timers in combination with 2c18b9f29269ee713c182409666be66af05e06a7. The timer callbacks were not called because the TimerManager's thread function blocked indefinitely on boost::condition_variable::timed_wait(). Relative timed_wait() uses the system clock (boost::get_system_time()) unconditionally to calculate the absolute timestamp for do_wait_until(). If the condition variable has been initialized with BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC, it compares this timestamp with the monotonic clock and therefore blocks. This issue has been reported in https://svn.boost.org/trac10/ticket/12728 and will not be fixed. The timed_wait interface is apparently deprecated. --- clients/roscpp/include/boost_161_condition_variable.h | 1 - clients/roscpp/include/ros/timer_manager.h | 4 ++-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/clients/roscpp/include/boost_161_condition_variable.h b/clients/roscpp/include/boost_161_condition_variable.h index 7277ce1dc4..6e13915493 100644 --- a/clients/roscpp/include/boost_161_condition_variable.h +++ b/clients/roscpp/include/boost_161_condition_variable.h @@ -16,7 +16,6 @@ namespace boost_161 { using condition_variable = boost::condition_variable; } #elif defined(BOOST_THREAD_PLATFORM_PTHREAD) -//#include #include "boost_161_pthread_condition_variable.h" #else #error "Boost threads unavailable on this platform" diff --git a/clients/roscpp/include/ros/timer_manager.h b/clients/roscpp/include/ros/timer_manager.h index a96f83af79..f8c1daddf1 100644 --- a/clients/roscpp/include/ros/timer_manager.h +++ b/clients/roscpp/include/ros/timer_manager.h @@ -574,14 +574,14 @@ void TimerManager::threadFunc() // since simulation time may be running faster than real time. if (!T::isSystemTime()) { - timers_cond_.timed_wait(lock, boost::posix_time::milliseconds(1)); + timers_cond_.wait_for(lock, boost::chrono::milliseconds(1)); } else { // On system time we can simply sleep for the rest of the wait time, since anything else requiring processing will // signal the condition variable int64_t remaining_time = std::max((sleep_end - current).toSec() * 1000.0f, 1); - timers_cond_.timed_wait(lock, boost::posix_time::milliseconds(remaining_time)); + timers_cond_.wait_for(lock, boost::chrono::milliseconds(remaining_time)); } } From be288e2176b4c283390f083ec49a5ce3cecf79f9 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Tue, 11 Dec 2018 16:37:10 +0100 Subject: [PATCH 3/9] roscpp: do not use boost_161_condition_variable.h on Windows (untested) --- clients/roscpp/CMakeLists.txt | 2 +- clients/roscpp/include/boost_161_condition_variable.h | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/clients/roscpp/CMakeLists.txt b/clients/roscpp/CMakeLists.txt index 6b666fc629..42a5ff419b 100644 --- a/clients/roscpp/CMakeLists.txt +++ b/clients/roscpp/CMakeLists.txt @@ -21,7 +21,7 @@ list(GET roscpp_VERSION_LIST 1 roscpp_VERSION_MINOR) list(GET roscpp_VERSION_LIST 2 roscpp_VERSION_PATCH) # Make sure we use CLOCK_MONOTONIC for the condition variable wait_for if not Apple. -if(NOT APPLE) +if(NOT APPLE AND NOT WIN32) if(Boost_VERSION LESS 106100) set(ROSCPP_USE_BACKPORTED_BOOST_CONDITION_VARIABLE_IMPLEMENTATION ON) endif() diff --git a/clients/roscpp/include/boost_161_condition_variable.h b/clients/roscpp/include/boost_161_condition_variable.h index 6e13915493..6d299f16f1 100644 --- a/clients/roscpp/include/boost_161_condition_variable.h +++ b/clients/roscpp/include/boost_161_condition_variable.h @@ -12,9 +12,6 @@ #include #if defined(BOOST_THREAD_PLATFORM_WIN32) #include -namespace boost_161 { - using condition_variable = boost::condition_variable; -} #elif defined(BOOST_THREAD_PLATFORM_PTHREAD) #include "boost_161_pthread_condition_variable.h" #else From 9d2715d7d1f41f4eead865e3856ceffdcb20684b Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Fri, 14 Dec 2018 14:47:35 +0100 Subject: [PATCH 4/9] roscpp: remove specialized implementation of TimerManager::threadFunc() in steady_timer.cpp The updated generic definition in timer_manager.h should do the same with a minor update. In all cases we can call boost::condition_variable::wait_until() with an absolute time_point of the respective clock. The conversion from system_clock to steady_clock for Time and WallTime is done internally in boost::condition_variable::wait_until(lock_type& lock, const chrono::time_point& t). --- clients/roscpp/include/ros/timer_manager.h | 20 ++++++- clients/roscpp/src/libros/steady_timer.cpp | 70 ---------------------- 2 files changed, 18 insertions(+), 72 deletions(-) diff --git a/clients/roscpp/include/ros/timer_manager.h b/clients/roscpp/include/ros/timer_manager.h index f8c1daddf1..abfed77973 100644 --- a/clients/roscpp/include/ros/timer_manager.h +++ b/clients/roscpp/include/ros/timer_manager.h @@ -50,6 +50,22 @@ namespace ros { +namespace { + template + class TimerManagerTraits + { + public: + typedef boost::chrono::system_clock::time_point time_point; + }; + + template<> + class TimerManagerTraits + { + public: + typedef boost::chrono::steady_clock::time_point time_point; + }; +} + template class TimerManager { @@ -580,8 +596,8 @@ void TimerManager::threadFunc() { // On system time we can simply sleep for the rest of the wait time, since anything else requiring processing will // signal the condition variable - int64_t remaining_time = std::max((sleep_end - current).toSec() * 1000.0f, 1); - timers_cond_.wait_for(lock, boost::chrono::milliseconds(remaining_time)); + typename TimerManagerTraits::time_point end_tp(boost::chrono::nanoseconds(sleep_end.toNSec())); + timers_cond_.wait_until(lock, end_tp); } } diff --git a/clients/roscpp/src/libros/steady_timer.cpp b/clients/roscpp/src/libros/steady_timer.cpp index 8c0517498c..12ee1cc6d0 100644 --- a/clients/roscpp/src/libros/steady_timer.cpp +++ b/clients/roscpp/src/libros/steady_timer.cpp @@ -31,76 +31,6 @@ namespace ros { -#if !defined(WIN32) -// specialization for SteadyTimer, to make sure we use a version with wait_until that uses the monotonic clock -template<> -void TimerManager::threadFunc() -{ - SteadyTime current; - while (!quit_) - { - SteadyTime sleep_end; - - boost::mutex::scoped_lock lock(timers_mutex_); - - current = SteadyTime::now(); - - { - boost::mutex::scoped_lock waitlock(waiting_mutex_); - - if (waiting_.empty()) - { - sleep_end = current + WallDuration(0.1); - } - else - { - TimerInfoPtr info = findTimer(waiting_.front()); - - while (!waiting_.empty() && info && info->next_expected <= current) - { - current = SteadyTime::now(); - - //ROS_DEBUG("Scheduling timer callback for timer [%d] of period [%f], [%f] off expected", info->handle, info->period.toSec(), (current - info->next_expected).toSec()); - CallbackInterfacePtr cb(boost::make_shared(this, info, info->last_expected, info->last_real, info->next_expected, info->last_expired, current)); - info->callback_queue->addCallback(cb, (uint64_t)info.get()); - - waiting_.pop_front(); - - if (waiting_.empty()) - { - break; - } - - info = findTimer(waiting_.front()); - } - - if (info) - { - sleep_end = info->next_expected; - } - } - } - - while (!new_timer_ && SteadyTime::now() < sleep_end && !quit_) - { - current = SteadyTime::now(); - - if (current >= sleep_end) - { - break; - } - - // requires boost 1.61 for wait_until to actually use the steady clock - // see: https://svn.boost.org/trac/boost/ticket/6377 - boost::chrono::steady_clock::time_point end_tp(boost::chrono::nanoseconds(sleep_end.toNSec())); - timers_cond_.wait_until(lock, end_tp); - } - - new_timer_ = false; - } -} -#endif - SteadyTimer::Impl::Impl() : started_(false) , timer_handle_(-1) From f6a05e323612a82ce9974546450c2e72a3cb63e4 Mon Sep 17 00:00:00 2001 From: Antoine Hoarau Date: Mon, 25 Feb 2019 18:50:05 +0100 Subject: [PATCH 5/9] fix namespaces --- .../boost_161_pthread_condition_variable.h | 156 ++++++++-------- ...boost_161_pthread_condition_variable_fwd.h | 167 +++++++++--------- 2 files changed, 153 insertions(+), 170 deletions(-) diff --git a/clients/roscpp/include/boost_161_pthread_condition_variable.h b/clients/roscpp/include/boost_161_pthread_condition_variable.h index 2aca774a48..719a1f3cfa 100644 --- a/clients/roscpp/include/boost_161_pthread_condition_variable.h +++ b/clients/roscpp/include/boost_161_pthread_condition_variable.h @@ -17,18 +17,18 @@ namespace boost_161 { - inline void condition_variable::wait(unique_lock& m) + inline void condition_variable::wait(boost::unique_lock& m) { #if defined BOOST_THREAD_THROW_IF_PRECONDITION_NOT_SATISFIED if(! m.owns_lock()) { - boost::throw_exception(condition_error(-1, "boost::condition_variable::wait() failed precondition mutex not owned")); + boost::throw_exception(boost::condition_error(-1, "boost::condition_variable::wait() failed precondition mutex not owned")); } #endif int res=0; { #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS - thread_cv_detail::lock_on_exit > guard; + boost::thread_cv_detail::lock_on_exit > guard; detail::interruption_checker check_for_interruption(&internal_mutex,&cond); pthread_mutex_t* the_mutex = &internal_mutex; guard.activate(m); @@ -38,28 +38,28 @@ namespace boost_161 res = pthread_cond_wait(&cond,the_mutex); } #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS - this_thread::interruption_point(); + boost::this_thread::interruption_point(); #endif if(res && res != EINTR) { - boost::throw_exception(condition_error(res, "boost::condition_variable::wait failed in pthread_cond_wait")); + boost::throw_exception(boost::condition_error(res, "boost::condition_variable::wait failed in pthread_cond_wait")); } } inline bool condition_variable::do_wait_until( - unique_lock& m, + boost::unique_lock& m, struct timespec const &timeout) { #if defined BOOST_THREAD_THROW_IF_PRECONDITION_NOT_SATISFIED if (!m.owns_lock()) { - boost::throw_exception(condition_error(EPERM, "boost::condition_variable::do_wait_until() failed precondition mutex not owned")); + boost::throw_exception(boost::condition_error(EPERM, "boost::condition_variable::do_wait_until() failed precondition mutex not owned")); } #endif int cond_res; { #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS - thread_cv_detail::lock_on_exit > guard; + boost::thread_cv_detail::lock_on_exit > guard; detail::interruption_checker check_for_interruption(&internal_mutex,&cond); pthread_mutex_t* the_mutex = &internal_mutex; guard.activate(m); @@ -69,7 +69,7 @@ namespace boost_161 cond_res=pthread_cond_timedwait(&cond,the_mutex,&timeout); } #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS - this_thread::interruption_point(); + boost::this_thread::interruption_point(); #endif if(cond_res==ETIMEDOUT) { @@ -77,7 +77,7 @@ namespace boost_161 } if(cond_res) { - boost::throw_exception(condition_error(cond_res, "boost::condition_variable::do_wait_until failed in pthread_cond_timedwait")); + boost::throw_exception(boost::condition_error(cond_res, "boost::condition_variable::do_wait_until failed in pthread_cond_timedwait")); } return true; } @@ -110,13 +110,13 @@ namespace boost_161 int const res=pthread_mutex_init(&internal_mutex,NULL); if(res) { - boost::throw_exception(thread_resource_error(res, "boost::condition_variable_any::condition_variable_any() failed in pthread_mutex_init")); + boost::throw_exception(boost::thread_resource_error(res, "boost::condition_variable_any::condition_variable_any() failed in pthread_mutex_init")); } int const res2 = detail_161::monotonic_pthread_cond_init(cond); if(res2) { BOOST_VERIFY(!pthread_mutex_destroy(&internal_mutex)); - boost::throw_exception(thread_resource_error(res2, "boost::condition_variable_any::condition_variable_any() failed in detail::monotonic_pthread_cond_init")); + boost::throw_exception(boost::thread_resource_error(res2, "boost::condition_variable_any::condition_variable_any() failed in detail::monotonic_pthread_cond_init")); } } ~condition_variable_any() @@ -130,7 +130,7 @@ namespace boost_161 { int res=0; { - thread_cv_detail::lock_on_exit guard; + boost::thread_cv_detail::lock_on_exit guard; #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS detail::interruption_checker check_for_interruption(&internal_mutex,&cond); #else @@ -140,11 +140,11 @@ namespace boost_161 res=pthread_cond_wait(&cond,&internal_mutex); } #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS - this_thread::interruption_point(); + boost::this_thread::interruption_point(); #endif if(res) { - boost::throw_exception(condition_error(res, "boost::condition_variable_any::wait() failed in pthread_cond_wait")); + boost::throw_exception(boost::condition_error(res, "boost::condition_variable_any::wait() failed in pthread_cond_wait")); } } @@ -158,19 +158,19 @@ namespace boost_161 template bool timed_wait(lock_type& m,boost::system_time const& abs_time) { - struct timespec const timeout=detail::to_timespec(abs_time); + struct timespec const timeout=boost::detail::to_timespec(abs_time); return do_wait_until(m, timeout); } template - bool timed_wait(lock_type& m,xtime const& abs_time) + bool timed_wait(lock_type& m,boost::xtime const& abs_time) { - return timed_wait(m,system_time(abs_time)); + return timed_wait(m,boost::system_time(abs_time)); } template bool timed_wait(lock_type& m,duration_type const& wait_duration) { - return timed_wait(m,get_system_time()+wait_duration); + return timed_wait(m,boost::get_system_time()+wait_duration); } template @@ -185,127 +185,119 @@ namespace boost_161 } template - bool timed_wait(lock_type& m,xtime const& abs_time, predicate_type pred) + bool timed_wait(lock_type& m,boost::xtime const& abs_time, predicate_type pred) { - return timed_wait(m,system_time(abs_time),pred); + return timed_wait(m,boost::system_time(abs_time),pred); } template bool timed_wait(lock_type& m,duration_type const& wait_duration,predicate_type pred) { - return timed_wait(m,get_system_time()+wait_duration,pred); + return timed_wait(m,boost::get_system_time()+wait_duration,pred); } #endif #ifndef BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC #ifdef BOOST_THREAD_USES_CHRONO template - cv_status + boost::cv_status wait_until( lock_type& lock, - const chrono::time_point& t) + const boost::chrono::time_point& t) { - using namespace chrono; - typedef time_point nano_sys_tmpt; + typedef boost::chrono::time_point nano_sys_tmpt; wait_until(lock, - nano_sys_tmpt(ceil(t.time_since_epoch()))); - return system_clock::now() < t ? cv_status::no_timeout : - cv_status::timeout; + nano_sys_tmpt(ceil(t.time_since_epoch()))); + return boost::chrono::system_clock::now() < t ? boost::cv_status::no_timeout : + boost::cv_status::timeout; } template - cv_status + boost::cv_status wait_until( lock_type& lock, - const chrono::time_point& t) + const boost::chrono::time_point& t) { - using namespace chrono; - system_clock::time_point s_now = system_clock::now(); + boost::chrono::system_clock::time_point s_now = boost::chrono::system_clock::now(); typename Clock::time_point c_now = Clock::now(); - wait_until(lock, s_now + ceil(t - c_now)); - return Clock::now() < t ? cv_status::no_timeout : cv_status::timeout; + wait_until(lock, s_now + ceil(t - c_now)); + return Clock::now() < t ? boost::cv_status::no_timeout : boost::cv_status::timeout; } template - cv_status + boost::cv_status wait_for( lock_type& lock, - const chrono::duration& d) + const boost::chrono::duration& d) { - using namespace chrono; - system_clock::time_point s_now = system_clock::now(); - steady_clock::time_point c_now = steady_clock::now(); - wait_until(lock, s_now + ceil(d)); - return steady_clock::now() - c_now < d ? cv_status::no_timeout : - cv_status::timeout; + boost::chrono::system_clock::time_point s_now = boost::chrono::system_clock::now(); + boost::chrono::steady_clock::time_point c_now = boost::chrono::steady_clock::now(); + wait_until(lock, s_now + ceil(d)); + return boost::chrono::steady_clock::now() - c_now < d ? boost::cv_status::no_timeout : + boost::cv_status::timeout; } template - cv_status wait_until( + boost::cv_status wait_until( lock_type& lk, - chrono::time_point tp) + boost::chrono::time_point tp) { - using namespace chrono; - nanoseconds d = tp.time_since_epoch(); + boost::chrono::nanoseconds d = tp.time_since_epoch(); timespec ts = boost::detail::to_timespec(d); - if (do_wait_until(lk, ts)) return cv_status::no_timeout; - else return cv_status::timeout; + if (do_wait_until(lk, ts)) return boost::cv_status::no_timeout; + else return boost::cv_status::timeout; } #endif #else // defined BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC #ifdef BOOST_THREAD_USES_CHRONO template - cv_status + boost::cv_status wait_until( lock_type& lock, - const chrono::time_point& t) + const boost::chrono::time_point& t) { - using namespace chrono; - typedef time_point nano_sys_tmpt; + typedef boost::chrono::time_point nano_sys_tmpt; wait_until(lock, - nano_sys_tmpt(ceil(t.time_since_epoch()))); - return steady_clock::now() < t ? cv_status::no_timeout : - cv_status::timeout; + nano_sys_tmpt(ceil(t.time_since_epoch()))); + return boost::chrono::steady_clock::now() < t ? boost::cv_status::no_timeout : + boost::cv_status::timeout; } template - cv_status + boost::cv_status wait_until( lock_type& lock, - const chrono::time_point& t) + const boost::chrono::time_point& t) { - using namespace chrono; - steady_clock::time_point s_now = steady_clock::now(); + boost::chrono::steady_clock::time_point s_now = boost::chrono::steady_clock::now(); typename Clock::time_point c_now = Clock::now(); - wait_until(lock, s_now + ceil(t - c_now)); - return Clock::now() < t ? cv_status::no_timeout : cv_status::timeout; + wait_until(lock, s_now + ceil(t - c_now)); + return Clock::now() < t ? boost::cv_status::no_timeout : boost::cv_status::timeout; } template - cv_status + boost::cv_status wait_for( lock_type& lock, - const chrono::duration& d) + const boost::chrono::duration& d) { - using namespace chrono; - steady_clock::time_point c_now = steady_clock::now(); - wait_until(lock, c_now + ceil(d)); - return steady_clock::now() - c_now < d ? cv_status::no_timeout : - cv_status::timeout; + boost::chrono::steady_clock::time_point c_now = boost::chrono::steady_clock::now(); + wait_until(lock, c_now + ceil(d)); + return boost::chrono::steady_clock::now() - c_now < d ? boost::cv_status::no_timeout : + boost::cv_status::timeout; } template - inline cv_status wait_until( + inline boost::cv_status wait_until( lock_type& lock, - chrono::time_point tp) + boost::chrono::time_point tp) { - using namespace chrono; - nanoseconds d = tp.time_since_epoch(); + boost::chrono::nanoseconds d = tp.time_since_epoch(); timespec ts = boost::detail::to_timespec(d); - if (do_wait_until(lock, ts)) return cv_status::no_timeout; - else return cv_status::timeout; + if (do_wait_until(lock, ts)) return boost::cv_status::no_timeout; + else return boost::cv_status::timeout; } #endif @@ -316,12 +308,12 @@ namespace boost_161 bool wait_until( lock_type& lock, - const chrono::time_point& t, + const boost::chrono::time_point& t, Predicate pred) { while (!pred()) { - if (wait_until(lock, t) == cv_status::timeout) + if (wait_until(lock, t) == boost::cv_status::timeout) return pred(); } return true; @@ -331,10 +323,10 @@ namespace boost_161 bool wait_for( lock_type& lock, - const chrono::duration& d, + const boost::chrono::duration& d, Predicate pred) { - return wait_until(lock, chrono::steady_clock::now() + d, boost::move(pred)); + return wait_until(lock, boost::chrono::steady_clock::now() + d, boost::move(pred)); } #endif @@ -358,7 +350,7 @@ namespace boost_161 { int res=0; { - thread_cv_detail::lock_on_exit guard; + boost::thread_cv_detail::lock_on_exit guard; #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS detail::interruption_checker check_for_interruption(&internal_mutex,&cond); #else @@ -368,7 +360,7 @@ namespace boost_161 res=pthread_cond_timedwait(&cond,&internal_mutex,&timeout); } #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS - this_thread::interruption_point(); + boost::this_thread::interruption_point(); #endif if(res==ETIMEDOUT) { @@ -376,7 +368,7 @@ namespace boost_161 } if(res) { - boost::throw_exception(condition_error(res, "boost::condition_variable_any::do_wait_until() failed in pthread_cond_timedwait")); + boost::throw_exception(boost::condition_error(res, "boost::condition_variable_any::do_wait_until() failed in pthread_cond_timedwait")); } return true; } diff --git a/clients/roscpp/include/boost_161_pthread_condition_variable_fwd.h b/clients/roscpp/include/boost_161_pthread_condition_variable_fwd.h index 0a58b16bf6..66e34bcc97 100644 --- a/clients/roscpp/include/boost_161_pthread_condition_variable_fwd.h +++ b/clients/roscpp/include/boost_161_pthread_condition_variable_fwd.h @@ -13,7 +13,6 @@ namespace boost_161 { - using namespace boost; namespace detail = boost::detail; namespace detail_161 { @@ -49,11 +48,11 @@ namespace boost_161 //private: // used by boost::thread::try_join_until inline bool do_wait_until( - unique_lock& lock, + boost::unique_lock& lock, struct timespec const &timeout); bool do_wait_for( - unique_lock& lock, + boost::unique_lock& lock, struct timespec const &timeout) { #if ! defined BOOST_THREAD_USEFIXES_TIMESPEC @@ -81,7 +80,7 @@ namespace boost_161 res=pthread_mutex_init(&internal_mutex,NULL); if(res) { - boost::throw_exception(thread_resource_error(res, "boost::condition_variable::condition_variable() constructor failed in pthread_mutex_init")); + boost::throw_exception(boost::thread_resource_error(res, "boost::condition_variable::condition_variable() constructor failed in pthread_mutex_init")); } #endif res = detail_161::monotonic_pthread_cond_init(cond); @@ -90,7 +89,7 @@ namespace boost_161 #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS BOOST_VERIFY(!pthread_mutex_destroy(&internal_mutex)); #endif - boost::throw_exception(thread_resource_error(res, "boost::condition_variable::condition_variable() constructor failed in detail::monotonic_pthread_cond_init")); + boost::throw_exception(boost::thread_resource_error(res, "boost::condition_variable::condition_variable() constructor failed in detail::monotonic_pthread_cond_init")); } } ~condition_variable() @@ -108,17 +107,17 @@ namespace boost_161 BOOST_ASSERT(!ret); } - void wait(unique_lock& m); + void wait(boost::unique_lock& m); template - void wait(unique_lock& m,predicate_type pred) + void wait(boost::unique_lock& m,predicate_type pred) { while(!pred()) wait(m); } #if defined BOOST_THREAD_USES_DATETIME inline bool timed_wait( - unique_lock& m, + boost::unique_lock& m, boost::system_time const& abs_time) { #if defined BOOST_THREAD_WAIT_BUG @@ -130,15 +129,15 @@ namespace boost_161 #endif } bool timed_wait( - unique_lock& m, - xtime const& abs_time) + boost::unique_lock& m, + boost::xtime const& abs_time) { - return timed_wait(m,system_time(abs_time)); + return timed_wait(m,boost::system_time(abs_time)); } template bool timed_wait( - unique_lock& m, + boost::unique_lock& m, duration_type const& wait_duration) { if (wait_duration.is_pos_infinity()) @@ -150,12 +149,12 @@ namespace boost_161 { return true; } - return timed_wait(m,get_system_time()+wait_duration); + return timed_wait(m,boost::get_system_time()+wait_duration); } template bool timed_wait( - unique_lock& m, + boost::unique_lock& m, boost::system_time const& abs_time,predicate_type pred) { while (!pred()) @@ -168,15 +167,15 @@ namespace boost_161 template bool timed_wait( - unique_lock& m, - xtime const& abs_time,predicate_type pred) + boost::unique_lock& m, + boost::xtime const& abs_time,predicate_type pred) { - return timed_wait(m,system_time(abs_time),pred); + return timed_wait(m,boost::system_time(abs_time),pred); } template bool timed_wait( - unique_lock& m, + boost::unique_lock& m, duration_type const& wait_duration,predicate_type pred) { if (wait_duration.is_pos_infinity()) @@ -191,7 +190,7 @@ namespace boost_161 { return pred(); } - return timed_wait(m,get_system_time()+wait_duration,pred); + return timed_wait(m,boost::get_system_time()+wait_duration,pred); } #endif @@ -200,58 +199,54 @@ namespace boost_161 #ifdef BOOST_THREAD_USES_CHRONO template - cv_status + boost::cv_status wait_until( - unique_lock& lock, - const chrono::time_point& t) + boost::unique_lock& lock, + const boost::chrono::time_point& t) { - using namespace chrono; - typedef time_point nano_sys_tmpt; + typedef boost::chrono::time_point nano_sys_tmpt; wait_until(lock, - nano_sys_tmpt(ceil(t.time_since_epoch()))); - return system_clock::now() < t ? cv_status::no_timeout : - cv_status::timeout; + nano_sys_tmpt(ceil(t.time_since_epoch()))); + return boost::chrono::system_clock::now() < t ? boost::cv_status::no_timeout : + boost::cv_status::timeout; } template - cv_status + boost::cv_status wait_until( - unique_lock& lock, - const chrono::time_point& t) + boost::unique_lock& lock, + const boost::chrono::time_point& t) { - using namespace chrono; - system_clock::time_point s_now = system_clock::now(); + boost::chrono::system_clock::time_point s_now = boost::chrono::system_clock::now(); typename Clock::time_point c_now = Clock::now(); - wait_until(lock, s_now + ceil(t - c_now)); - return Clock::now() < t ? cv_status::no_timeout : cv_status::timeout; + wait_until(lock, s_now + ceil(t - c_now)); + return Clock::now() < t ? boost::cv_status::no_timeout : boost::cv_status::timeout; } template - cv_status + boost::cv_status wait_for( - unique_lock& lock, - const chrono::duration& d) + boost::unique_lock& lock, + const boost::chrono::duration& d) { - using namespace chrono; - system_clock::time_point s_now = system_clock::now(); - steady_clock::time_point c_now = steady_clock::now(); - wait_until(lock, s_now + ceil(d)); - return steady_clock::now() - c_now < d ? cv_status::no_timeout : - cv_status::timeout; + boost::chrono::system_clock::time_point s_now = boost::chrono::system_clock::now(); + boost::chrono::steady_clock::time_point c_now = boost::chrono::steady_clock::now(); + wait_until(lock, s_now + ceil(d)); + return boost::chrono::steady_clock::now() - c_now < d ? boost::cv_status::no_timeout : + boost::cv_status::timeout; } - inline cv_status wait_until( - unique_lock& lk, - chrono::time_point tp) + inline boost::cv_status wait_until( + boost::unique_lock& lk, + boost::chrono::time_point tp) { - using namespace chrono; - nanoseconds d = tp.time_since_epoch(); - timespec ts = boost::detail::to_timespec(d); - if (do_wait_until(lk, ts)) return cv_status::no_timeout; - else return cv_status::timeout; + boost::chrono::nanoseconds d = tp.time_since_epoch(); + timespec ts = detail::to_timespec(d); + if (do_wait_until(lk, ts)) return boost::cv_status::no_timeout; + else return boost::cv_status::timeout; } #endif @@ -259,54 +254,50 @@ namespace boost_161 #ifdef BOOST_THREAD_USES_CHRONO template - cv_status + boost::cv_status wait_until( - unique_lock& lock, - const chrono::time_point& t) + boost::unique_lock& lock, + const boost::chrono::time_point& t) { - using namespace chrono; - typedef time_point nano_sys_tmpt; + typedef time_point nano_sys_tmpt; wait_until(lock, - nano_sys_tmpt(ceil(t.time_since_epoch()))); - return steady_clock::now() < t ? cv_status::no_timeout : - cv_status::timeout; + nano_sys_tmpt(ceil(t.time_since_epoch()))); + return boost::chrono::steady_clock::now() < t ? boost::cv_status::no_timeout : + boost::cv_status::timeout; } template - cv_status + boost::cv_status wait_until( - unique_lock& lock, - const chrono::time_point& t) + boost::unique_lock& lock, + const boost::chrono::time_point& t) { - using namespace chrono; - steady_clock::time_point s_now = steady_clock::now(); + boost::chrono::steady_clock::time_point s_now = boost::chrono::steady_clock::now(); typename Clock::time_point c_now = Clock::now(); - wait_until(lock, s_now + ceil(t - c_now)); - return Clock::now() < t ? cv_status::no_timeout : cv_status::timeout; + wait_until(lock, s_now + ceil(t - c_now)); + return Clock::now() < t ? boost::cv_status::no_timeout : boost::cv_status::timeout; } template - cv_status + boost::cv_status wait_for( - unique_lock& lock, - const chrono::duration& d) + boost::unique_lock& lock, + const boost::chrono::duration& d) { - using namespace chrono; - steady_clock::time_point c_now = steady_clock::now(); - wait_until(lock, c_now + ceil(d)); - return steady_clock::now() - c_now < d ? cv_status::no_timeout : - cv_status::timeout; + boost::chrono::steady_clock::time_point c_now = boost::chrono::steady_clock::now(); + wait_until(lock, c_now + ceil(d)); + return boost::chrono::steady_clock::now() - c_now < d ? boost::cv_status::no_timeout : + boost::cv_status::timeout; } - inline cv_status wait_until( - unique_lock& lk, - chrono::time_point tp) + inline boost::cv_status wait_until( + boost::unique_lock& lk, + boost::chrono::time_point tp) { - using namespace chrono; nanoseconds d = tp.time_since_epoch(); - timespec ts = boost::detail::to_timespec(d); - if (do_wait_until(lk, ts)) return cv_status::no_timeout; - else return cv_status::timeout; + timespec ts = detail::to_timespec(d); + if (do_wait_until(lk, ts)) return boost::cv_status::no_timeout; + else return boost::cv_status::timeout; } #endif @@ -316,13 +307,13 @@ namespace boost_161 template bool wait_until( - unique_lock& lock, - const chrono::time_point& t, + boost::unique_lock& lock, + const boost::chrono::time_point& t, Predicate pred) { while (!pred()) { - if (wait_until(lock, t) == cv_status::timeout) + if (wait_until(lock, t) == boost::cv_status::timeout) return pred(); } return true; @@ -331,11 +322,11 @@ namespace boost_161 template bool wait_for( - unique_lock& lock, - const chrono::duration& d, + boost::unique_lock& lock, + const boost::chrono::duration& d, Predicate pred) { - return wait_until(lock, chrono::steady_clock::now() + d, boost::move(pred)); + return wait_until(lock, boost::chrono::steady_clock::now() + d, boost::move(pred)); } #endif @@ -352,7 +343,7 @@ namespace boost_161 }; - BOOST_THREAD_DECL void notify_all_at_thread_exit(condition_variable& cond, unique_lock lk); + BOOST_THREAD_DECL void notify_all_at_thread_exit(condition_variable& cond, boost::unique_lock lk); } From dd503e2a8c5b2ffc1a35bb956fe9d6868d62e456 Mon Sep 17 00:00:00 2001 From: Antoine Hoarau Date: Tue, 26 Feb 2019 11:21:32 +0100 Subject: [PATCH 6/9] add more explicit namespaces --- .../boost_161_pthread_condition_variable.h | 20 +++---- ...boost_161_pthread_condition_variable_fwd.h | 58 +++++++++---------- 2 files changed, 39 insertions(+), 39 deletions(-) diff --git a/clients/roscpp/include/boost_161_pthread_condition_variable.h b/clients/roscpp/include/boost_161_pthread_condition_variable.h index 719a1f3cfa..83d1d369b8 100644 --- a/clients/roscpp/include/boost_161_pthread_condition_variable.h +++ b/clients/roscpp/include/boost_161_pthread_condition_variable.h @@ -17,7 +17,7 @@ namespace boost_161 { - inline void condition_variable::wait(boost::unique_lock& m) + inline void condition_variable::wait(boost::unique_lock& m) { #if defined BOOST_THREAD_THROW_IF_PRECONDITION_NOT_SATISFIED if(! m.owns_lock()) @@ -28,7 +28,7 @@ namespace boost_161 int res=0; { #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS - boost::thread_cv_detail::lock_on_exit > guard; + boost::thread_cv_detail::lock_on_exit > guard; detail::interruption_checker check_for_interruption(&internal_mutex,&cond); pthread_mutex_t* the_mutex = &internal_mutex; guard.activate(m); @@ -47,7 +47,7 @@ namespace boost_161 } inline bool condition_variable::do_wait_until( - boost::unique_lock& m, + boost::unique_lock& m, struct timespec const &timeout) { #if defined BOOST_THREAD_THROW_IF_PRECONDITION_NOT_SATISFIED @@ -59,7 +59,7 @@ namespace boost_161 int cond_res; { #if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS - boost::thread_cv_detail::lock_on_exit > guard; + boost::thread_cv_detail::lock_on_exit > guard; detail::interruption_checker check_for_interruption(&internal_mutex,&cond); pthread_mutex_t* the_mutex = &internal_mutex; guard.activate(m); @@ -207,7 +207,7 @@ namespace boost_161 { typedef boost::chrono::time_point nano_sys_tmpt; wait_until(lock, - nano_sys_tmpt(ceil(t.time_since_epoch()))); + nano_sys_tmpt(boost::chrono::ceil(t.time_since_epoch()))); return boost::chrono::system_clock::now() < t ? boost::cv_status::no_timeout : boost::cv_status::timeout; } @@ -220,7 +220,7 @@ namespace boost_161 { boost::chrono::system_clock::time_point s_now = boost::chrono::system_clock::now(); typename Clock::time_point c_now = Clock::now(); - wait_until(lock, s_now + ceil(t - c_now)); + wait_until(lock, s_now + boost::chrono::ceil(t - c_now)); return Clock::now() < t ? boost::cv_status::no_timeout : boost::cv_status::timeout; } @@ -232,7 +232,7 @@ namespace boost_161 { boost::chrono::system_clock::time_point s_now = boost::chrono::system_clock::now(); boost::chrono::steady_clock::time_point c_now = boost::chrono::steady_clock::now(); - wait_until(lock, s_now + ceil(d)); + wait_until(lock, s_now + boost::chrono::ceil(d)); return boost::chrono::steady_clock::now() - c_now < d ? boost::cv_status::no_timeout : boost::cv_status::timeout; @@ -260,7 +260,7 @@ namespace boost_161 { typedef boost::chrono::time_point nano_sys_tmpt; wait_until(lock, - nano_sys_tmpt(ceil(t.time_since_epoch()))); + nano_sys_tmpt(boost::chrono::ceil(t.time_since_epoch()))); return boost::chrono::steady_clock::now() < t ? boost::cv_status::no_timeout : boost::cv_status::timeout; } @@ -273,7 +273,7 @@ namespace boost_161 { boost::chrono::steady_clock::time_point s_now = boost::chrono::steady_clock::now(); typename Clock::time_point c_now = Clock::now(); - wait_until(lock, s_now + ceil(t - c_now)); + wait_until(lock, s_now + boost::chrono::ceil(t - c_now)); return Clock::now() < t ? boost::cv_status::no_timeout : boost::cv_status::timeout; } @@ -284,7 +284,7 @@ namespace boost_161 const boost::chrono::duration& d) { boost::chrono::steady_clock::time_point c_now = boost::chrono::steady_clock::now(); - wait_until(lock, c_now + ceil(d)); + wait_until(lock, c_now + boost::chrono::ceil(d)); return boost::chrono::steady_clock::now() - c_now < d ? boost::cv_status::no_timeout : boost::cv_status::timeout; } diff --git a/clients/roscpp/include/boost_161_pthread_condition_variable_fwd.h b/clients/roscpp/include/boost_161_pthread_condition_variable_fwd.h index 66e34bcc97..f301ea50f1 100644 --- a/clients/roscpp/include/boost_161_pthread_condition_variable_fwd.h +++ b/clients/roscpp/include/boost_161_pthread_condition_variable_fwd.h @@ -48,11 +48,11 @@ namespace boost_161 //private: // used by boost::thread::try_join_until inline bool do_wait_until( - boost::unique_lock& lock, + boost::unique_lock& lock, struct timespec const &timeout); bool do_wait_for( - boost::unique_lock& lock, + boost::unique_lock& lock, struct timespec const &timeout) { #if ! defined BOOST_THREAD_USEFIXES_TIMESPEC @@ -107,17 +107,17 @@ namespace boost_161 BOOST_ASSERT(!ret); } - void wait(boost::unique_lock& m); + void wait(boost::unique_lock& m); template - void wait(boost::unique_lock& m,predicate_type pred) + void wait(boost::unique_lock& m,predicate_type pred) { while(!pred()) wait(m); } #if defined BOOST_THREAD_USES_DATETIME inline bool timed_wait( - boost::unique_lock& m, + boost::unique_lock& m, boost::system_time const& abs_time) { #if defined BOOST_THREAD_WAIT_BUG @@ -129,7 +129,7 @@ namespace boost_161 #endif } bool timed_wait( - boost::unique_lock& m, + boost::unique_lock& m, boost::xtime const& abs_time) { return timed_wait(m,boost::system_time(abs_time)); @@ -137,7 +137,7 @@ namespace boost_161 template bool timed_wait( - boost::unique_lock& m, + boost::unique_lock& m, duration_type const& wait_duration) { if (wait_duration.is_pos_infinity()) @@ -154,7 +154,7 @@ namespace boost_161 template bool timed_wait( - boost::unique_lock& m, + boost::unique_lock& m, boost::system_time const& abs_time,predicate_type pred) { while (!pred()) @@ -167,7 +167,7 @@ namespace boost_161 template bool timed_wait( - boost::unique_lock& m, + boost::unique_lock& m, boost::xtime const& abs_time,predicate_type pred) { return timed_wait(m,boost::system_time(abs_time),pred); @@ -175,7 +175,7 @@ namespace boost_161 template bool timed_wait( - boost::unique_lock& m, + boost::unique_lock& m, duration_type const& wait_duration,predicate_type pred) { if (wait_duration.is_pos_infinity()) @@ -201,12 +201,12 @@ namespace boost_161 template boost::cv_status wait_until( - boost::unique_lock& lock, + boost::unique_lock& lock, const boost::chrono::time_point& t) { typedef boost::chrono::time_point nano_sys_tmpt; wait_until(lock, - nano_sys_tmpt(ceil(t.time_since_epoch()))); + nano_sys_tmpt(boost::chrono::ceil(t.time_since_epoch()))); return boost::chrono::system_clock::now() < t ? boost::cv_status::no_timeout : boost::cv_status::timeout; } @@ -214,12 +214,12 @@ namespace boost_161 template boost::cv_status wait_until( - boost::unique_lock& lock, + boost::unique_lock& lock, const boost::chrono::time_point& t) { boost::chrono::system_clock::time_point s_now = boost::chrono::system_clock::now(); typename Clock::time_point c_now = Clock::now(); - wait_until(lock, s_now + ceil(t - c_now)); + wait_until(lock, s_now + boost::chrono::ceil(t - c_now)); return Clock::now() < t ? boost::cv_status::no_timeout : boost::cv_status::timeout; } @@ -228,19 +228,19 @@ namespace boost_161 template boost::cv_status wait_for( - boost::unique_lock& lock, + boost::unique_lock& lock, const boost::chrono::duration& d) { boost::chrono::system_clock::time_point s_now = boost::chrono::system_clock::now(); boost::chrono::steady_clock::time_point c_now = boost::chrono::steady_clock::now(); - wait_until(lock, s_now + ceil(d)); + wait_until(lock, s_now + boost::chrono::ceil(d)); return boost::chrono::steady_clock::now() - c_now < d ? boost::cv_status::no_timeout : boost::cv_status::timeout; } inline boost::cv_status wait_until( - boost::unique_lock& lk, + boost::unique_lock& lk, boost::chrono::time_point tp) { boost::chrono::nanoseconds d = tp.time_since_epoch(); @@ -256,12 +256,12 @@ namespace boost_161 template boost::cv_status wait_until( - boost::unique_lock& lock, + boost::unique_lock& lock, const boost::chrono::time_point& t) { - typedef time_point nano_sys_tmpt; + typedef boost::chrono::time_point nano_sys_tmpt; wait_until(lock, - nano_sys_tmpt(ceil(t.time_since_epoch()))); + nano_sys_tmpt(boost::chrono::ceil(t.time_since_epoch()))); return boost::chrono::steady_clock::now() < t ? boost::cv_status::no_timeout : boost::cv_status::timeout; } @@ -269,32 +269,32 @@ namespace boost_161 template boost::cv_status wait_until( - boost::unique_lock& lock, + boost::unique_lock& lock, const boost::chrono::time_point& t) { boost::chrono::steady_clock::time_point s_now = boost::chrono::steady_clock::now(); typename Clock::time_point c_now = Clock::now(); - wait_until(lock, s_now + ceil(t - c_now)); + wait_until(lock, s_now + boost::chrono::ceil(t - c_now)); return Clock::now() < t ? boost::cv_status::no_timeout : boost::cv_status::timeout; } template boost::cv_status wait_for( - boost::unique_lock& lock, + boost::unique_lock& lock, const boost::chrono::duration& d) { boost::chrono::steady_clock::time_point c_now = boost::chrono::steady_clock::now(); - wait_until(lock, c_now + ceil(d)); + wait_until(lock, c_now + boost::chrono::ceil(d)); return boost::chrono::steady_clock::now() - c_now < d ? boost::cv_status::no_timeout : boost::cv_status::timeout; } inline boost::cv_status wait_until( - boost::unique_lock& lk, + boost::unique_lock& lk, boost::chrono::time_point tp) { - nanoseconds d = tp.time_since_epoch(); + boost::chrono::nanoseconds d = tp.time_since_epoch(); timespec ts = detail::to_timespec(d); if (do_wait_until(lk, ts)) return boost::cv_status::no_timeout; else return boost::cv_status::timeout; @@ -307,7 +307,7 @@ namespace boost_161 template bool wait_until( - boost::unique_lock& lock, + boost::unique_lock& lock, const boost::chrono::time_point& t, Predicate pred) { @@ -322,7 +322,7 @@ namespace boost_161 template bool wait_for( - boost::unique_lock& lock, + boost::unique_lock& lock, const boost::chrono::duration& d, Predicate pred) { @@ -343,7 +343,7 @@ namespace boost_161 }; - BOOST_THREAD_DECL void notify_all_at_thread_exit(condition_variable& cond, boost::unique_lock lk); + BOOST_THREAD_DECL void notify_all_at_thread_exit(condition_variable& cond, boost::unique_lock lk); } From 48b3f6b6aa45bef6adcb71029351f9fdc7226fa8 Mon Sep 17 00:00:00 2001 From: Antoine Hoarau Date: Tue, 26 Feb 2019 13:51:27 +0100 Subject: [PATCH 7/9] add missing ns --- clients/roscpp/include/boost_161_pthread_condition_variable.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clients/roscpp/include/boost_161_pthread_condition_variable.h b/clients/roscpp/include/boost_161_pthread_condition_variable.h index 83d1d369b8..4a4e2bf307 100644 --- a/clients/roscpp/include/boost_161_pthread_condition_variable.h +++ b/clients/roscpp/include/boost_161_pthread_condition_variable.h @@ -203,7 +203,7 @@ namespace boost_161 boost::cv_status wait_until( lock_type& lock, - const boost::chrono::time_point& t) + const boost::chrono::time_point& t) { typedef boost::chrono::time_point nano_sys_tmpt; wait_until(lock, From dfbf7266fbafbde4b7a206054661f150d873505e Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Fri, 5 Apr 2019 20:39:26 +0200 Subject: [PATCH 8/9] roscpp: fixed Boost version check in CMakeLists.txt find_package(Boost) has to come before checking the Boost version. Otherwise BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC was not defined which triggered the assertion in timer_manager.h:240. Since Boost 1.67 BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC became the default if the platform supports it and the macro is not defined anymore. Instead, check for BOOST_THREAD_INTERNAL_CLOCK_IS_MONO. --- clients/roscpp/CMakeLists.txt | 4 ++-- clients/roscpp/include/ros/timer_manager.h | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/clients/roscpp/CMakeLists.txt b/clients/roscpp/CMakeLists.txt index 42a5ff419b..ce24f8af04 100644 --- a/clients/roscpp/CMakeLists.txt +++ b/clients/roscpp/CMakeLists.txt @@ -20,6 +20,8 @@ list(GET roscpp_VERSION_LIST 0 roscpp_VERSION_MAJOR) list(GET roscpp_VERSION_LIST 1 roscpp_VERSION_MINOR) list(GET roscpp_VERSION_LIST 2 roscpp_VERSION_PATCH) +find_package(Boost REQUIRED COMPONENTS chrono filesystem system) + # Make sure we use CLOCK_MONOTONIC for the condition variable wait_for if not Apple. if(NOT APPLE AND NOT WIN32) if(Boost_VERSION LESS 106100) @@ -34,8 +36,6 @@ endif() configure_file(${CMAKE_CURRENT_SOURCE_DIR}/include/ros/common.h.in ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/ros/common.h) -find_package(Boost REQUIRED COMPONENTS chrono filesystem system) - include_directories(include ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/ros ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) # this is needed for use within a bazel workspace. See #1548 for details. include_directories(${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/clients/roscpp/include/ros/timer_manager.h b/clients/roscpp/include/ros/timer_manager.h index abfed77973..3c459672f1 100644 --- a/clients/roscpp/include/ros/timer_manager.h +++ b/clients/roscpp/include/ros/timer_manager.h @@ -236,10 +236,10 @@ template TimerManager::TimerManager() : new_timer_(false), id_counter_(0), thread_started_(false), quit_(false) { -#if (BOOST_VERSION < 106700) && !defined(BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC) +#if !defined(BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC) && !defined(BOOST_THREAD_INTERNAL_CLOCK_IS_MONO) ROS_ASSERT_MSG(false, - "ros::TimerManager was instantiated by package " ROS_PACKAGE_NAME - "without that BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC is defined! " + "ros::TimerManager was instantiated by package " ROS_PACKAGE_NAME ", but " + "neither BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC nor BOOST_THREAD_INTERNAL_CLOCK_IS_MONO is defined! " "Be aware that timers might misbehave when system time jumps, " "e.g. due to network time corrections."); #endif From b9e8f3a176e95b309e6a421889cf450f2886c9cc Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Fri, 12 Apr 2019 16:14:29 +0200 Subject: [PATCH 9/9] roscpp: replace ROSCPP_BOOST_CONDITION_VARIABLE and ROSCPP_BOOST_CONDITION_VARIABLE_HEADER macros by a typedef in internal_condition_variable.h --- clients/roscpp/CMakeLists.txt | 3 - clients/roscpp/include/ros/callback_queue.h | 8 +-- clients/roscpp/include/ros/common.h.in | 13 ----- .../include/ros/internal_condition_variable.h | 56 +++++++++++++++++++ clients/roscpp/include/ros/rosout_appender.h | 9 +-- .../roscpp/include/ros/service_server_link.h | 8 +-- clients/roscpp/include/ros/timer_manager.h | 8 +-- 7 files changed, 66 insertions(+), 39 deletions(-) create mode 100644 clients/roscpp/include/ros/internal_condition_variable.h diff --git a/clients/roscpp/CMakeLists.txt b/clients/roscpp/CMakeLists.txt index ce24f8af04..02f16954a6 100644 --- a/clients/roscpp/CMakeLists.txt +++ b/clients/roscpp/CMakeLists.txt @@ -24,9 +24,6 @@ find_package(Boost REQUIRED COMPONENTS chrono filesystem system) # Make sure we use CLOCK_MONOTONIC for the condition variable wait_for if not Apple. if(NOT APPLE AND NOT WIN32) - if(Boost_VERSION LESS 106100) - set(ROSCPP_USE_BACKPORTED_BOOST_CONDITION_VARIABLE_IMPLEMENTATION ON) - endif() if(Boost_VERSION LESS 106700) # CLOCK_MONOTONIC became the default in Boost 1.67: # https://github.com/boostorg/thread/commit/1e84b978b2bb0aae830cc14533dea3b7ddda5cde diff --git a/clients/roscpp/include/ros/callback_queue.h b/clients/roscpp/include/ros/callback_queue.h index 44a021f31e..8b92788f3d 100644 --- a/clients/roscpp/include/ros/callback_queue.h +++ b/clients/roscpp/include/ros/callback_queue.h @@ -35,12 +35,8 @@ #ifndef ROSCPP_CALLBACK_QUEUE_H #define ROSCPP_CALLBACK_QUEUE_H -// check if we might need to include our own backported version boost::condition_variable -// in order to use CLOCK_MONOTONIC for the condition variable -#include "ros/common.h" -#include ROSCPP_BOOST_CONDITION_VARIABLE_HEADER - #include "ros/callback_queue_interface.h" +#include "ros/internal_condition_variable.h" #include "ros/time.h" #include @@ -166,7 +162,7 @@ class ROSCPP_DECL CallbackQueue : public CallbackQueueInterface D_CallbackInfo callbacks_; size_t calling_; boost::mutex mutex_; - ROSCPP_BOOST_CONDITION_VARIABLE condition_; + roscpp::internal::condition_variable condition_; boost::mutex id_info_mutex_; M_IDInfo id_info_; diff --git a/clients/roscpp/include/ros/common.h.in b/clients/roscpp/include/ros/common.h.in index f5655d793f..5351aeb937 100644 --- a/clients/roscpp/include/ros/common.h.in +++ b/clients/roscpp/include/ros/common.h.in @@ -38,7 +38,6 @@ #include "ros/serialized_message.h" #include -#include #define ROS_VERSION_MAJOR @roscpp_VERSION_MAJOR@ #define ROS_VERSION_MINOR @roscpp_VERSION_MINOR@ @@ -49,18 +48,6 @@ #define ROS_VERSION_GE(major1, minor1, patch1, major2, minor2, patch2) (ROS_VERSION_COMBINED(major1, minor1, patch1) >= ROS_VERSION_COMBINED(major2, minor2, patch2)) #define ROS_VERSION_MINIMUM(major, minor, patch) ROS_VERSION_GE(ROS_VERSION_MAJOR, ROS_VERSION_MINOR, ROS_VERSION_PATCH, major, minor, patch) -// check if we might need to include our own backported version boost::condition_variable -// in order to use CLOCK_MONOTONIC for the condition variable and for the SteadyTimer -#cmakedefine ROSCPP_USE_BACKPORTED_BOOST_CONDITION_VARIABLE_IMPLEMENTATION -#ifdef ROSCPP_USE_BACKPORTED_BOOST_CONDITION_VARIABLE_IMPLEMENTATION - // use backported version of boost condition variable, see https://svn.boost.org/trac/boost/ticket/6377 - #define ROSCPP_BOOST_CONDITION_VARIABLE_HEADER "boost_161_condition_variable.h" - #define ROSCPP_BOOST_CONDITION_VARIABLE boost_161::condition_variable -#else - #define ROSCPP_BOOST_CONDITION_VARIABLE_HEADER - #define ROSCPP_BOOST_CONDITION_VARIABLE boost::condition_variable -#endif - #include // Import/export for windows dll's and visibility for gcc shared libraries. diff --git a/clients/roscpp/include/ros/internal_condition_variable.h b/clients/roscpp/include/ros/internal_condition_variable.h new file mode 100644 index 0000000000..12ff9514f8 --- /dev/null +++ b/clients/roscpp/include/ros/internal_condition_variable.h @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROSCPP_INTERNAL_CONDITION_VARIABLE_H +#define ROSCPP_INTERNAL_CONDITION_VARIABLE_H + +#include + +// check if we might need to include our own backported version boost::condition_variable +// in order to use CLOCK_MONOTONIC for the condition variable and for the SteadyTimer +#if !defined(_WIN32) && !defined(__APPLE__) && (BOOST_VERSION < 106100) + #define ROSCPP_USE_BACKPORTED_BOOST_CONDITION_VARIABLE_IMPLEMENTATION +#endif + +#ifdef ROSCPP_USE_BACKPORTED_BOOST_CONDITION_VARIABLE_IMPLEMENTATION + // use backported version of boost condition variable, see https://svn.boost.org/trac/boost/ticket/6377 + #include "boost_161_condition_variable.h" + namespace roscpp { + namespace internal { + typedef boost_161::condition_variable condition_variable; + } + } +#else + #include + namespace roscpp { + namespace internal { + typedef boost::condition_variable condition_variable; + } + } +#endif + +#endif // ROSCPP_INTERNAL_CONDITION_VARIABLE_H diff --git a/clients/roscpp/include/ros/rosout_appender.h b/clients/roscpp/include/ros/rosout_appender.h index 31c1944a7b..45a3fbf60d 100644 --- a/clients/roscpp/include/ros/rosout_appender.h +++ b/clients/roscpp/include/ros/rosout_appender.h @@ -35,11 +35,8 @@ #ifndef ROSCPP_ROSOUT_APPENDER_H #define ROSCPP_ROSOUT_APPENDER_H -// check if we might need to include our own backported version boost::condition_variable -// in order to use CLOCK_MONOTONIC for the condition variable -#include "ros/common.h" -#include ROSCPP_BOOST_CONDITION_VARIABLE_HEADER - +#include +#include #include #include @@ -77,7 +74,7 @@ class ROSCPP_DECL ROSOutAppender : public ros::console::LogAppender typedef std::vector V_Log; V_Log log_queue_; boost::mutex queue_mutex_; - ROSCPP_BOOST_CONDITION_VARIABLE queue_condition_; + roscpp::internal::condition_variable queue_condition_; bool shutting_down_; bool disable_topics_; diff --git a/clients/roscpp/include/ros/service_server_link.h b/clients/roscpp/include/ros/service_server_link.h index 8cf8789fdb..5a4d228b6f 100644 --- a/clients/roscpp/include/ros/service_server_link.h +++ b/clients/roscpp/include/ros/service_server_link.h @@ -35,10 +35,8 @@ #ifndef ROSCPP_SERVICE_SERVER_LINK_H #define ROSCPP_SERVICE_SERVER_LINK_H -// check if we might need to include our own backported version boost::condition_variable -// in order to use CLOCK_MONOTONIC for the condition variable -#include "ros/common.h" -#include ROSCPP_BOOST_CONDITION_VARIABLE_HEADER +#include +#include #include #include @@ -67,7 +65,7 @@ class ROSCPP_DECL ServiceServerLink : public boost::enable_shared_from_this