From dd6aeb258213d8cb125c5cc0c8222defc88ed057 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 3 Nov 2015 13:40:59 -0800 Subject: [PATCH 1/2] add interrupted bool, to account for spurious wake ups from std::condition_variable::wait_for --- rclcpp/include/rclcpp/utilities.hpp | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index fb913bae19..49b830799c 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -45,6 +45,7 @@ rmw_guard_condition_t * g_sigint_guard_cond_handle = \ rmw_create_guard_condition(); /// Condition variable for timed sleep (see sleep_for). std::condition_variable g_interrupt_condition_variable; +std::atomic g_is_interrupted = false; /// Mutex for protecting the global condition variable. std::mutex g_interrupt_mutex; @@ -95,6 +96,7 @@ signal_handler(int signal_value) fprintf(stderr, "[rclcpp::error] failed to trigger guard condition: %s\n", rmw_get_error_string_safe()); } + g_is_interrupted.store(true); g_interrupt_condition_variable.notify_all(); } } // namespace @@ -117,6 +119,7 @@ init(int argc, char * argv[]) { (void)argc; (void)argv; + g_is_interrupted.store(false); rmw_ret_t status = rmw_init(); if (status != RMW_RET_OK) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) @@ -170,6 +173,7 @@ shutdown() fprintf(stderr, "[rclcpp::error] failed to trigger guard condition: %s\n", rmw_get_error_string_safe()); } + g_is_interrupted.store(true); g_interrupt_condition_variable.notify_all(); } @@ -190,10 +194,18 @@ bool sleep_for(const std::chrono::nanoseconds & nanoseconds) { // TODO: determine if posix's nanosleep(2) is more efficient here - std::unique_lock lock(::g_interrupt_mutex); - auto cvs = ::g_interrupt_condition_variable.wait_for(lock, nanoseconds); + std::chrono::nanoseconds time_left = nanoseconds; + { + std::unique_lock lock(::g_interrupt_mutex); + auto start = std::chrono::steady_clock::now(); + ::g_interrupt_condition_variable.wait_for(lock, nanoseconds); + time_left -= std::chrono::steady_clock::now() - start; + } + if (time_left > std::chrono::nanoseconds::zero() && !g_is_interrupted) { + return sleep_for(time_left); + } // Return true if the timeout elapsed successfully, otherwise false. - return cvs != std::cv_status::no_timeout; + return !g_is_interrupted; } } /* namespace utilities */ From 426b77bcd97a4e660fd2a42f172c804537b8945e Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Tue, 3 Nov 2015 14:28:28 -0800 Subject: [PATCH 2/2] fix deleted invocation of copy constructor on Linux --- rclcpp/include/rclcpp/utilities.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 49b830799c..a6062516c3 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -45,7 +45,7 @@ rmw_guard_condition_t * g_sigint_guard_cond_handle = \ rmw_create_guard_condition(); /// Condition variable for timed sleep (see sleep_for). std::condition_variable g_interrupt_condition_variable; -std::atomic g_is_interrupted = false; +std::atomic g_is_interrupted(false); /// Mutex for protecting the global condition variable. std::mutex g_interrupt_mutex;