From 7d80613030644d7381e79e0fa4fc7cb658f20c0d Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 28 Mar 2018 11:05:15 -0700 Subject: [PATCH 1/7] Baseline test and force threads to yield. --- rclcpp/CMakeLists.txt | 10 ++- .../executors/multi_threaded_executor.hpp | 15 +++- .../executors/multi_threaded_executor.cpp | 8 +- .../test_multi_threaded_executor.cpp | 85 +++++++++++++++++++ 4 files changed, 114 insertions(+), 4 deletions(-) create mode 100644 rclcpp/test/executors/test_multi_threaded_executor.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 1bd484fcaf..7c444a5a6f 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -387,7 +387,15 @@ if(BUILD_TESTING) "rcl") target_link_libraries(test_utilities ${PROJECT_NAME}) endif() -endif() + + ament_add_gtest(test_multi_threaded_executor test/executors/test_multi_threaded_executor.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET test_multi_threaded_executor) + ament_target_dependencies(test_multi_threaded_executor + "rcl") + target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) + endif() +endif(BUILD_TESTING) ament_package( CONFIG_EXTRAS rclcpp-extras.cmake diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index aa350cc822..d253074510 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -34,10 +34,22 @@ class MultiThreadedExecutor : public executor::Executor public: RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor) + /// Constructor for MultiThreadedExecutor. + /** + * For the yield_before_execute option, when true std::this_thread::yield() + * will be called after acquiring work (as an AnyExecutable) and after + * releasing the spinning lock, but before executing the work. + * This is useful for reproducing some bugs related to taking work more than + * once. + * + * \param args common arguments for all executors + * \param yield_before_execute if true std::this_thread::yield() is called + */ RCLCPP_PUBLIC MultiThreadedExecutor( const executor::ExecutorArgs & args = rclcpp::executor::create_default_executor_arguments(), - size_t number_of_threads = 0); + size_t number_of_threads = 0, + bool yield_before_execute = false); RCLCPP_PUBLIC virtual ~MultiThreadedExecutor(); @@ -60,6 +72,7 @@ class MultiThreadedExecutor : public executor::Executor std::mutex wait_mutex_; size_t number_of_threads_; + bool yield_before_execute_; }; } // namespace executors diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 4752bb8bb8..fc73852fbe 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -25,8 +25,9 @@ using rclcpp::executors::MultiThreadedExecutor; MultiThreadedExecutor::MultiThreadedExecutor( const rclcpp::executor::ExecutorArgs & args, - size_t number_of_threads) -: executor::Executor(args) + size_t number_of_threads, + bool yield_before_execute) +: executor::Executor(args), yield_before_execute_(yield_before_execute) { number_of_threads_ = number_of_threads ? number_of_threads : std::thread::hardware_concurrency(); if (number_of_threads_ == 0) { @@ -79,6 +80,9 @@ MultiThreadedExecutor::run(size_t) continue; } } + if (yield_before_execute_) { + std::this_thread::yield(); + } execute_any_executable(any_exec); } } diff --git a/rclcpp/test/executors/test_multi_threaded_executor.cpp b/rclcpp/test/executors/test_multi_threaded_executor.cpp new file mode 100644 index 0000000000..65a6aac49d --- /dev/null +++ b/rclcpp/test/executors/test_multi_threaded_executor.cpp @@ -0,0 +1,85 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors.hpp" + +#include "rcl_interfaces/msg/intra_process_message.hpp" + +using namespace std::chrono_literals; + +using rcl_interfaces::msg::IntraProcessMessage; + +class TestMultiThreadedExecutor : public::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +/* + Test that timers are not taken multiple times when using reentrant callback groups. + */ +TEST_F(TestMultiThreadedExecutor, timer_over_take) { + + struct sched_param param; + param.sched_priority = 0; + if (sched_setscheduler(0, SCHED_BATCH, ¶m) != 0) { + perror("sched_setscheduler"); + } + + rclcpp::executors::MultiThreadedExecutor executor( + rclcpp::executor::create_default_executor_arguments(), 0, true); + + ASSERT_GT(executor.get_number_of_threads(), 1u); + + std::shared_ptr node = + std::make_shared("test_multi_threaded_executor_timer_over_take"); + + auto cbg = node->create_callback_group(rclcpp::callback_group::CallbackGroupType::Reentrant); + + rclcpp::Clock system_clock(RCL_STEADY_TIME); + std::mutex last_mutex; + auto last = system_clock.now(); + + auto timer_callback = [&executor, &system_clock, &last_mutex, &last]() { + rclcpp::Time now = system_clock.now(); + + { + std::lock_guard lock(last_mutex); + double diff = labs((now - last).nanoseconds()) / 1.0e9; + last = now; + + if (diff < 0.09 || diff > 0.11) { + executor.cancel(); + ASSERT_TRUE(diff > 0.09); + ASSERT_TRUE(diff < 0.11); + } + } + }; + + auto timer = node->create_wall_timer(100ms, timer_callback, cbg); + executor.add_node(node); + executor.spin(); +} From 6853019dc73b2a4bd6b5c03b94add7e451122955 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 28 Mar 2018 11:07:37 -0700 Subject: [PATCH 2/7] Add timer tracking for executor. --- rclcpp/include/rclcpp/executor.hpp | 2 ++ rclcpp/src/rclcpp/executor.cpp | 8 +++++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 657529f404..eea3a8eeb6 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -353,6 +354,7 @@ class Executor RCLCPP_DISABLE_COPY(Executor) std::vector weak_nodes_; + std::set taken_timers_; }; } // namespace executor diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index fc46f74cdf..9a40550565 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -255,6 +255,10 @@ Executor::execute_any_executable(AnyExecutable & any_exec) } if (any_exec.timer) { execute_timer(any_exec.timer); + auto it = taken_timers_.find(any_exec.timer); + if (it != taken_timers_.end()) { + taken_timers_.erase(it); + } } if (any_exec.subscription) { execute_subscription(any_exec.subscription); @@ -545,10 +549,12 @@ Executor::get_next_timer(AnyExecutable & any_exec) } for (auto & timer_ref : group->get_timer_ptrs()) { auto timer = timer_ref.lock(); - if (timer && timer->is_ready()) { + + if (timer && timer->is_ready() && taken_timers_.count(timer) == 0) { any_exec.timer = timer; any_exec.callback_group = group; node = get_node_by_group(group); + taken_timers_.insert(timer); return; } } From a2c7e9e2d7b084f672101b684b80cc152d983731 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 28 Mar 2018 13:42:45 -0700 Subject: [PATCH 3/7] Add locking and test happy-path exit conditions. --- rclcpp/include/rclcpp/executor.hpp | 2 ++ rclcpp/src/rclcpp/executor.cpp | 5 ++- .../test_multi_threaded_executor.cpp | 31 ++++++++++++------- 3 files changed, 25 insertions(+), 13 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index eea3a8eeb6..a44f02d114 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -354,6 +354,8 @@ class Executor RCLCPP_DISABLE_COPY(Executor) std::vector weak_nodes_; + + std::mutex taken_timers_mutex_; std::set taken_timers_; }; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 9a40550565..3ad51043e4 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -255,6 +255,7 @@ Executor::execute_any_executable(AnyExecutable & any_exec) } if (any_exec.timer) { execute_timer(any_exec.timer); + std::lock_guard lock(taken_timers_mutex_); auto it = taken_timers_.find(any_exec.timer); if (it != taken_timers_.end()) { taken_timers_.erase(it); @@ -548,9 +549,11 @@ Executor::get_next_timer(AnyExecutable & any_exec) continue; } for (auto & timer_ref : group->get_timer_ptrs()) { + std::lock_guard lock(taken_timers_mutex_); auto timer = timer_ref.lock(); - if (timer && timer->is_ready() && taken_timers_.count(timer) == 0) { + bool is_not_scheduled = (taken_timers_.count(timer) == 0); + if (timer && timer->is_ready() && is_not_scheduled) { any_exec.timer = timer; any_exec.callback_group = group; node = get_node_by_group(group); diff --git a/rclcpp/test/executors/test_multi_threaded_executor.cpp b/rclcpp/test/executors/test_multi_threaded_executor.cpp index 65a6aac49d..d6be7c2aef 100644 --- a/rclcpp/test/executors/test_multi_threaded_executor.cpp +++ b/rclcpp/test/executors/test_multi_threaded_executor.cpp @@ -29,7 +29,7 @@ using namespace std::chrono_literals; using rcl_interfaces::msg::IntraProcessMessage; -class TestMultiThreadedExecutor : public::testing::Test +class TestMultiThreadedExecutor : public ::testing::Test { protected: static void SetUpTestCase() @@ -63,21 +63,28 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) { std::mutex last_mutex; auto last = system_clock.now(); - auto timer_callback = [&executor, &system_clock, &last_mutex, &last]() { - rclcpp::Time now = system_clock.now(); + std::atomic_int timer_count {0}; - { - std::lock_guard lock(last_mutex); - double diff = labs((now - last).nanoseconds()) / 1.0e9; - last = now; + auto timer_callback = [&timer_count, &executor, &system_clock, &last_mutex, &last]() { + rclcpp::Time now = system_clock.now(); + timer_count++; - if (diff < 0.09 || diff > 0.11) { + if (timer_count > 20) { executor.cancel(); - ASSERT_TRUE(diff > 0.09); - ASSERT_TRUE(diff < 0.11); } - } - }; + + { + std::lock_guard lock(last_mutex); + double diff = labs((now - last).nanoseconds()) / 1.0e9; + last = now; + + if (diff < 0.09 || diff > 0.11) { + executor.cancel(); + ASSERT_TRUE(diff > 0.09); + ASSERT_TRUE(diff < 0.11); + } + } + }; auto timer = node->create_wall_timer(100ms, timer_callback, cbg); executor.add_node(node); From 38c48d17d80b6259b93cf1df81ec4646f937b7b7 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 18 Jun 2018 19:48:23 -0500 Subject: [PATCH 4/7] Move logic to multi_threaded_executor --- rclcpp/CMakeLists.txt | 2 +- rclcpp/include/rclcpp/executor.hpp | 4 ---- .../executors/multi_threaded_executor.hpp | 5 +++++ rclcpp/src/rclcpp/executor.cpp | 11 +--------- .../executors/multi_threaded_executor.cpp | 21 ++++++++++++++++--- .../test_multi_threaded_executor.cpp | 9 ++++---- 6 files changed, 29 insertions(+), 23 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 7c444a5a6f..2ad3674450 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -395,7 +395,7 @@ if(BUILD_TESTING) "rcl") target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) endif() -endif(BUILD_TESTING) +endif() ament_package( CONFIG_EXTRAS rclcpp-extras.cmake diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index a44f02d114..657529f404 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -354,9 +353,6 @@ class Executor RCLCPP_DISABLE_COPY(Executor) std::vector weak_nodes_; - - std::mutex taken_timers_mutex_; - std::set taken_timers_; }; } // namespace executor diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index d253074510..2175bcadb1 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -15,7 +15,9 @@ #ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_ #define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_ +#include #include +#include #include #include @@ -73,6 +75,9 @@ class MultiThreadedExecutor : public executor::Executor std::mutex wait_mutex_; size_t number_of_threads_; bool yield_before_execute_; + + std::mutex scheduled_mutex_; + std::set> scheduled_; }; } // namespace executors diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 3ad51043e4..fc46f74cdf 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -255,11 +255,6 @@ Executor::execute_any_executable(AnyExecutable & any_exec) } if (any_exec.timer) { execute_timer(any_exec.timer); - std::lock_guard lock(taken_timers_mutex_); - auto it = taken_timers_.find(any_exec.timer); - if (it != taken_timers_.end()) { - taken_timers_.erase(it); - } } if (any_exec.subscription) { execute_subscription(any_exec.subscription); @@ -549,15 +544,11 @@ Executor::get_next_timer(AnyExecutable & any_exec) continue; } for (auto & timer_ref : group->get_timer_ptrs()) { - std::lock_guard lock(taken_timers_mutex_); auto timer = timer_ref.lock(); - - bool is_not_scheduled = (taken_timers_.count(timer) == 0); - if (timer && timer->is_ready() && is_not_scheduled) { + if (timer && timer->is_ready()) { any_exec.timer = timer; any_exec.callback_group = group; node = get_node_by_group(group); - taken_timers_.insert(timer); return; } } diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index fc73852fbe..8cdacb338d 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include "rclcpp/utilities.hpp" @@ -70,19 +71,33 @@ void MultiThreadedExecutor::run(size_t) { while (rclcpp::ok() && spinning.load()) { - executor::AnyExecutable any_exec; + auto any_exec = std::make_shared(); { std::lock_guard wait_lock(wait_mutex_); if (!rclcpp::ok() || !spinning.load()) { return; } - if (!get_next_executable(any_exec)) { + if (!get_next_executable(*any_exec)) { continue; } + { + std::lock_guard lock(scheduled_mutex_); + if (scheduled_.count(any_exec) != 0) { + continue; + } + scheduled_.insert(any_exec); + } } if (yield_before_execute_) { std::this_thread::yield(); } - execute_any_executable(any_exec); + execute_any_executable(*any_exec); + { + std::lock_guard lock(scheduled_mutex_); + auto it = scheduled_.find(any_exec); + if (it != scheduled_.end()) { + scheduled_.erase(it); + } + } } } diff --git a/rclcpp/test/executors/test_multi_threaded_executor.cpp b/rclcpp/test/executors/test_multi_threaded_executor.cpp index d6be7c2aef..028d5a2eae 100644 --- a/rclcpp/test/executors/test_multi_threaded_executor.cpp +++ b/rclcpp/test/executors/test_multi_threaded_executor.cpp @@ -42,7 +42,6 @@ class TestMultiThreadedExecutor : public ::testing::Test Test that timers are not taken multiple times when using reentrant callback groups. */ TEST_F(TestMultiThreadedExecutor, timer_over_take) { - struct sched_param param; param.sched_priority = 0; if (sched_setscheduler(0, SCHED_BATCH, ¶m) != 0) { @@ -78,15 +77,15 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) { double diff = labs((now - last).nanoseconds()) / 1.0e9; last = now; - if (diff < 0.09 || diff > 0.11) { + if (diff < 0.009 || diff > 0.011) { executor.cancel(); - ASSERT_TRUE(diff > 0.09); - ASSERT_TRUE(diff < 0.11); + ASSERT_TRUE(diff > 0.009); + ASSERT_TRUE(diff < 0.011); } } }; - auto timer = node->create_wall_timer(100ms, timer_callback, cbg); + auto timer = node->create_wall_timer(10ms, timer_callback, cbg); executor.add_node(node); executor.spin(); } From d51bf8065202702644e84a162f8f63a420681117 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 18 Jun 2018 20:46:10 -0500 Subject: [PATCH 5/7] Address reviewer feedback by reducing scope of set. --- .../executors/multi_threaded_executor.hpp | 4 +-- .../executors/multi_threaded_executor.cpp | 27 ++++++++++--------- .../test_multi_threaded_executor.cpp | 15 ++++++++--- 3 files changed, 28 insertions(+), 18 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index 2175bcadb1..f0f579dd13 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -76,8 +76,8 @@ class MultiThreadedExecutor : public executor::Executor size_t number_of_threads_; bool yield_before_execute_; - std::mutex scheduled_mutex_; - std::set> scheduled_; + std::mutex scheduled_timers_mutex_; + std::set scheduled_timers_; }; } // namespace executors diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 8cdacb338d..f7a6b81c99 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -71,32 +71,35 @@ void MultiThreadedExecutor::run(size_t) { while (rclcpp::ok() && spinning.load()) { - auto any_exec = std::make_shared(); + executor::AnyExecutable any_exec; { std::lock_guard wait_lock(wait_mutex_); if (!rclcpp::ok() || !spinning.load()) { return; } - if (!get_next_executable(*any_exec)) { + if (!get_next_executable(any_exec)) { continue; } - { - std::lock_guard lock(scheduled_mutex_); - if (scheduled_.count(any_exec) != 0) { + if (any_exec.timer) { + // Guard against multiple threads getting the same timer. + std::lock_guard lock(scheduled_timers_mutex_); + if (scheduled_timers_.count(any_exec.timer) != 0) { continue; } - scheduled_.insert(any_exec); + scheduled_timers_.insert(any_exec.timer); } } if (yield_before_execute_) { std::this_thread::yield(); } - execute_any_executable(*any_exec); - { - std::lock_guard lock(scheduled_mutex_); - auto it = scheduled_.find(any_exec); - if (it != scheduled_.end()) { - scheduled_.erase(it); + + execute_any_executable(any_exec); + + if (any_exec.timer) { + std::lock_guard lock(scheduled_timers_mutex_); + auto it = scheduled_timers_.find(any_exec.timer); + if (it != scheduled_timers_.end()) { + scheduled_timers_.erase(it); } } } diff --git a/rclcpp/test/executors/test_multi_threaded_executor.cpp b/rclcpp/test/executors/test_multi_threaded_executor.cpp index 028d5a2eae..d4c9af0054 100644 --- a/rclcpp/test/executors/test_multi_threaded_executor.cpp +++ b/rclcpp/test/executors/test_multi_threaded_executor.cpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2018 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -42,11 +42,15 @@ class TestMultiThreadedExecutor : public ::testing::Test Test that timers are not taken multiple times when using reentrant callback groups. */ TEST_F(TestMultiThreadedExecutor, timer_over_take) { +#ifdef __linux__ + // This seems to be the most effective way to force the bug to happen on Linux. + // This is unnecessary on MacOS, since the default scheduler causes it. struct sched_param param; param.sched_priority = 0; if (sched_setscheduler(0, SCHED_BATCH, ¶m) != 0) { perror("sched_setscheduler"); } +#endif rclcpp::executors::MultiThreadedExecutor executor( rclcpp::executor::create_default_executor_arguments(), 0, true); @@ -65,6 +69,9 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) { std::atomic_int timer_count {0}; auto timer_callback = [&timer_count, &executor, &system_clock, &last_mutex, &last]() { + const double PERIOD = 0.01f; + const double TOLERANCE = 0.001f; + rclcpp::Time now = system_clock.now(); timer_count++; @@ -77,10 +84,10 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) { double diff = labs((now - last).nanoseconds()) / 1.0e9; last = now; - if (diff < 0.009 || diff > 0.011) { + if (diff < PERIOD - TOLERANCE || diff > PERIOD + TOLERANCE) { executor.cancel(); - ASSERT_TRUE(diff > 0.009); - ASSERT_TRUE(diff < 0.011); + ASSERT_TRUE(diff > PERIOD - TOLERANCE); + ASSERT_TRUE(diff < PERIOD + TOLERANCE); } } }; From fdcfd01e620221b7311c3530e10bdc2b911facf7 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 18 Jun 2018 21:40:31 -0500 Subject: [PATCH 6/7] Expand tolerance on testing. --- .../executors/test_multi_threaded_executor.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/rclcpp/test/executors/test_multi_threaded_executor.cpp b/rclcpp/test/executors/test_multi_threaded_executor.cpp index d4c9af0054..6465b28c8a 100644 --- a/rclcpp/test/executors/test_multi_threaded_executor.cpp +++ b/rclcpp/test/executors/test_multi_threaded_executor.cpp @@ -52,8 +52,10 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) { } #endif + bool yield_before_execute = true; + rclcpp::executors::MultiThreadedExecutor executor( - rclcpp::executor::create_default_executor_arguments(), 0, true); + rclcpp::executor::create_default_executor_arguments(), 2u, yield_before_execute); ASSERT_GT(executor.get_number_of_threads(), 1u); @@ -69,19 +71,19 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) { std::atomic_int timer_count {0}; auto timer_callback = [&timer_count, &executor, &system_clock, &last_mutex, &last]() { - const double PERIOD = 0.01f; - const double TOLERANCE = 0.001f; + const double PERIOD = 0.1f; + const double TOLERANCE = 0.01f; rclcpp::Time now = system_clock.now(); timer_count++; - if (timer_count > 20) { + if (timer_count > 5) { executor.cancel(); } { std::lock_guard lock(last_mutex); - double diff = labs((now - last).nanoseconds()) / 1.0e9; + double diff = std::abs((now - last).nanoseconds()) / 1.0e9; last = now; if (diff < PERIOD - TOLERANCE || diff > PERIOD + TOLERANCE) { @@ -92,7 +94,7 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) { } }; - auto timer = node->create_wall_timer(10ms, timer_callback, cbg); + auto timer = node->create_wall_timer(100ms, timer_callback, cbg); executor.add_node(node); executor.spin(); } From 1b5d3fd11ea6256a8f8e656eb537e0660cb23cf1 Mon Sep 17 00:00:00 2001 From: dhood Date: Mon, 18 Jun 2018 20:42:33 -0700 Subject: [PATCH 7/7] comment fixup Otherwise it seemed to me like it would yield twice. --- rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index f0f579dd13..b91059447b 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -39,7 +39,7 @@ class MultiThreadedExecutor : public executor::Executor /// Constructor for MultiThreadedExecutor. /** * For the yield_before_execute option, when true std::this_thread::yield() - * will be called after acquiring work (as an AnyExecutable) and after + * will be called after acquiring work (as an AnyExecutable) and * releasing the spinning lock, but before executing the work. * This is useful for reproducing some bugs related to taking work more than * once.