|
41 | 41 | #include "test_msgs/msg/empty.hpp" |
42 | 42 |
|
43 | 43 | #include "./executor_types.hpp" |
| 44 | +#include "./test_waitable.hpp" |
44 | 45 |
|
45 | 46 | using namespace std::chrono_literals; |
46 | 47 |
|
@@ -331,114 +332,6 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) |
331 | 332 | spinner.join(); |
332 | 333 | } |
333 | 334 |
|
334 | | -class TestWaitable : public rclcpp::Waitable |
335 | | -{ |
336 | | -public: |
337 | | - TestWaitable() = default; |
338 | | - |
339 | | - void |
340 | | - add_to_wait_set(rcl_wait_set_t & wait_set) override |
341 | | - { |
342 | | - if (trigger_count_ > 0) { |
343 | | - // Keep the gc triggered until the trigger count is reduced back to zero. |
344 | | - // This is necessary if trigger() results in the wait set waking, but not |
345 | | - // executing this waitable, in which case it needs to be re-triggered. |
346 | | - gc_.trigger(); |
347 | | - } |
348 | | - rclcpp::detail::add_guard_condition_to_rcl_wait_set(wait_set, gc_); |
349 | | - } |
350 | | - |
351 | | - void trigger() |
352 | | - { |
353 | | - trigger_count_++; |
354 | | - gc_.trigger(); |
355 | | - } |
356 | | - |
357 | | - bool |
358 | | - is_ready(const rcl_wait_set_t & wait_set) override |
359 | | - { |
360 | | - is_ready_count_++; |
361 | | - for (size_t i = 0; i < wait_set.size_of_guard_conditions; ++i) { |
362 | | - auto rcl_guard_condition = wait_set.guard_conditions[i]; |
363 | | - if (&gc_.get_rcl_guard_condition() == rcl_guard_condition) { |
364 | | - return true; |
365 | | - } |
366 | | - } |
367 | | - return false; |
368 | | - } |
369 | | - |
370 | | - std::shared_ptr<void> |
371 | | - take_data() override |
372 | | - { |
373 | | - return nullptr; |
374 | | - } |
375 | | - |
376 | | - std::shared_ptr<void> |
377 | | - take_data_by_entity_id(size_t id) override |
378 | | - { |
379 | | - (void) id; |
380 | | - return nullptr; |
381 | | - } |
382 | | - |
383 | | - void |
384 | | - execute(const std::shared_ptr<void> &) override |
385 | | - { |
386 | | - trigger_count_--; |
387 | | - count_++; |
388 | | - if (nullptr != on_execute_callback_) { |
389 | | - on_execute_callback_(); |
390 | | - } else { |
391 | | - // TODO(wjwwood): I don't know why this was here, but probably it should |
392 | | - // not be there, or test cases where that is important should use the |
393 | | - // on_execute_callback? |
394 | | - std::this_thread::sleep_for(3ms); |
395 | | - } |
396 | | - } |
397 | | - |
398 | | - void |
399 | | - set_on_execute_callback(std::function<void()> on_execute_callback) |
400 | | - { |
401 | | - on_execute_callback_ = on_execute_callback; |
402 | | - } |
403 | | - |
404 | | - void |
405 | | - set_on_ready_callback(std::function<void(size_t, int)> callback) override |
406 | | - { |
407 | | - auto gc_callback = [callback](size_t count) { |
408 | | - callback(count, 0); |
409 | | - }; |
410 | | - gc_.set_on_trigger_callback(gc_callback); |
411 | | - } |
412 | | - |
413 | | - void |
414 | | - clear_on_ready_callback() override |
415 | | - { |
416 | | - gc_.set_on_trigger_callback(nullptr); |
417 | | - } |
418 | | - |
419 | | - size_t |
420 | | - get_number_of_ready_guard_conditions() override {return 1;} |
421 | | - |
422 | | - size_t |
423 | | - get_count() const |
424 | | - { |
425 | | - return count_; |
426 | | - } |
427 | | - |
428 | | - size_t |
429 | | - get_is_ready_call_count() const |
430 | | - { |
431 | | - return is_ready_count_; |
432 | | - } |
433 | | - |
434 | | -private: |
435 | | - std::atomic<size_t> trigger_count_ = 0; |
436 | | - std::atomic<size_t> is_ready_count_ = 0; |
437 | | - std::atomic<size_t> count_ = 0; |
438 | | - rclcpp::GuardCondition gc_; |
439 | | - std::function<void()> on_execute_callback_ = nullptr; |
440 | | -}; |
441 | | - |
442 | 335 | TYPED_TEST(TestExecutors, spinAll) |
443 | 336 | { |
444 | 337 | using ExecutorType = TypeParam; |
@@ -938,155 +831,3 @@ TEST(TestExecutors, testSpinWithNonDefaultContext) |
938 | 831 |
|
939 | 832 | rclcpp::shutdown(non_default_context); |
940 | 833 | } |
941 | | - |
942 | | -template<typename T> |
943 | | -class TestBusyWaiting : public ::testing::Test |
944 | | -{ |
945 | | -public: |
946 | | - void SetUp() override |
947 | | - { |
948 | | - rclcpp::init(0, nullptr); |
949 | | - |
950 | | - const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); |
951 | | - std::stringstream test_name; |
952 | | - test_name << test_info->test_case_name() << "_" << test_info->name(); |
953 | | - node = std::make_shared<rclcpp::Node>("node", test_name.str()); |
954 | | - callback_group = node->create_callback_group( |
955 | | - rclcpp::CallbackGroupType::MutuallyExclusive, |
956 | | - /* automatically_add_to_executor_with_node =*/ false); |
957 | | - |
958 | | - auto waitable_interfaces = node->get_node_waitables_interface(); |
959 | | - waitable = std::make_shared<TestWaitable>(); |
960 | | - waitable_interfaces->add_waitable(waitable, callback_group); |
961 | | - |
962 | | - executor = std::make_shared<T>(); |
963 | | - executor->add_callback_group(callback_group, node->get_node_base_interface()); |
964 | | - } |
965 | | - |
966 | | - void TearDown() override |
967 | | - { |
968 | | - rclcpp::shutdown(); |
969 | | - } |
970 | | - |
971 | | - void |
972 | | - set_up_and_trigger_waitable(std::function<void()> extra_callback = nullptr) |
973 | | - { |
974 | | - this->has_executed = false; |
975 | | - this->waitable->set_on_execute_callback([this, extra_callback]() { |
976 | | - if (!this->has_executed) { |
977 | | - // trigger once to see if the second trigger is handled or not |
978 | | - // this follow up trigger simulates new entities becoming ready while |
979 | | - // the executor is executing something else, e.g. subscription got data |
980 | | - // or a timer expired, etc. |
981 | | - // spin_some would not handle this second trigger, since it collects |
982 | | - // work only once, whereas spin_all should handle it since it |
983 | | - // collects work multiple times |
984 | | - this->waitable->trigger(); |
985 | | - this->has_executed = true; |
986 | | - } |
987 | | - if (nullptr != extra_callback) { |
988 | | - extra_callback(); |
989 | | - } |
990 | | - }); |
991 | | - this->waitable->trigger(); |
992 | | - } |
993 | | - |
994 | | - void |
995 | | - check_for_busy_waits(std::chrono::steady_clock::time_point start_time) |
996 | | - { |
997 | | - // rough time based check, since the work to be done was very small it |
998 | | - // should be safe to check that we didn't use more than half the |
999 | | - // max duration, which itself is much larger than necessary |
1000 | | - // however, it could still produce a false-positive |
1001 | | - EXPECT_LT( |
1002 | | - std::chrono::steady_clock::now() - start_time, |
1003 | | - max_duration / 2) |
1004 | | - << "executor took a long time to execute when it should have done " |
1005 | | - << "nothing and should not have blocked either, but this could be a " |
1006 | | - << "false negative if the computer is really slow"; |
1007 | | - |
1008 | | - // this check is making some assumptions about the implementation of the |
1009 | | - // executors, but it should be safe to say that a busy wait may result in |
1010 | | - // hundreds or thousands of calls to is_ready(), but "normal" executor |
1011 | | - // behavior should be within an order of magnitude of the number of |
1012 | | - // times that the waitable was executed |
1013 | | - ASSERT_LT(waitable->get_is_ready_call_count(), 10u * this->waitable->get_count()); |
1014 | | - } |
1015 | | - |
1016 | | - static constexpr auto max_duration = 10s; |
1017 | | - |
1018 | | - rclcpp::Node::SharedPtr node; |
1019 | | - rclcpp::CallbackGroup::SharedPtr callback_group; |
1020 | | - std::shared_ptr<TestWaitable> waitable; |
1021 | | - std::chrono::steady_clock::time_point start_time; |
1022 | | - std::shared_ptr<T> executor; |
1023 | | - bool has_executed; |
1024 | | -}; |
1025 | | - |
1026 | | -TYPED_TEST_SUITE(TestBusyWaiting, ExecutorTypes, ExecutorTypeNames); |
1027 | | - |
1028 | | -TYPED_TEST(TestBusyWaiting, test_spin_all) |
1029 | | -{ |
1030 | | - this->set_up_and_trigger_waitable(); |
1031 | | - |
1032 | | - auto start_time = std::chrono::steady_clock::now(); |
1033 | | - this->executor->spin_all(this->max_duration); |
1034 | | - this->check_for_busy_waits(start_time); |
1035 | | - // this should get the initial trigger, and the follow up from in the callback |
1036 | | - ASSERT_EQ(this->waitable->get_count(), 2u); |
1037 | | -} |
1038 | | - |
1039 | | -TYPED_TEST(TestBusyWaiting, test_spin_some) |
1040 | | -{ |
1041 | | - this->set_up_and_trigger_waitable(); |
1042 | | - |
1043 | | - auto start_time = std::chrono::steady_clock::now(); |
1044 | | - this->executor->spin_some(this->max_duration); |
1045 | | - this->check_for_busy_waits(start_time); |
1046 | | - // this should get the inital trigger, but not the follow up in the callback |
1047 | | - ASSERT_EQ(this->waitable->get_count(), 1u); |
1048 | | -} |
1049 | | - |
1050 | | -TYPED_TEST(TestBusyWaiting, test_spin) |
1051 | | -{ |
1052 | | - std::condition_variable cv; |
1053 | | - std::mutex cv_m; |
1054 | | - bool first_check_passed = false; |
1055 | | - |
1056 | | - this->set_up_and_trigger_waitable([&cv, &cv_m, &first_check_passed]() { |
1057 | | - cv.notify_one(); |
1058 | | - if (!first_check_passed) { |
1059 | | - std::unique_lock<std::mutex> lk(cv_m); |
1060 | | - cv.wait_for(lk, 1s, [&]() {return first_check_passed;}); |
1061 | | - } |
1062 | | - }); |
1063 | | - |
1064 | | - auto start_time = std::chrono::steady_clock::now(); |
1065 | | - std::thread t([this]() { |
1066 | | - this->executor->spin(); |
1067 | | - }); |
1068 | | - |
1069 | | - // wait until thread has started (first execute of waitable) |
1070 | | - { |
1071 | | - std::unique_lock<std::mutex> lk(cv_m); |
1072 | | - cv.wait_for(lk, 10s); |
1073 | | - } |
1074 | | - EXPECT_GT(this->waitable->get_count(), 0u); |
1075 | | - |
1076 | | - first_check_passed = true; |
1077 | | - cv.notify_one(); |
1078 | | - |
1079 | | - // wait until the executor has finished (second execute of waitable) |
1080 | | - { |
1081 | | - std::unique_lock<std::mutex> lk(cv_m); |
1082 | | - cv.wait_for(lk, 10s); |
1083 | | - } |
1084 | | - EXPECT_EQ(this->waitable->get_count(), 2u); |
1085 | | - |
1086 | | - this->executor->cancel(); |
1087 | | - t.join(); |
1088 | | - |
1089 | | - this->check_for_busy_waits(start_time); |
1090 | | - // this should get the initial trigger, and the follow up from in the callback |
1091 | | - ASSERT_EQ(this->waitable->get_count(), 2u); |
1092 | | -} |
0 commit comments