From 27b4d4fc30149339da50376acd08893141748aa5 Mon Sep 17 00:00:00 2001 From: Ilya guterman Date: Mon, 29 Nov 2021 10:07:24 +0200 Subject: [PATCH] executor ignore canceled timers (#220) * executor ignore canceled timers this is similar behavior observed in rclcpp, canceled timers shouldn't be handled Signed-off-by: Ilya Guterman * Add regression test Signed-off-by: Your Name * Uncrustify Signed-off-by: Your Name * Fix warning unused Signed-off-by: Your Name Signed-off-by: Pablo Garrido * return sucess on cancled timer Signed-off-by: Ilya Guterman * validate cancled timer doesn't impact the execution of other handlers Signed-off-by: Ilya Guterman * executor_spin_publisher_timer_cancelled cancel timer on beggining of the test Signed-off-by: Ilya Guterman Co-authored-by: Pablo Garrido --- rclc/src/rclc/executor.c | 7 +++ rclc/test/rclc/test_executor.cpp | 89 ++++++++++++++++++++++++++++++++ 2 files changed, 96 insertions(+) diff --git a/rclc/src/rclc/executor.c b/rclc/src/rclc/executor.c index 4b508b94..885fceda 100644 --- a/rclc/src/rclc/executor.c +++ b/rclc/src/rclc/executor.c @@ -1433,6 +1433,13 @@ _rclc_execute(rclc_executor_handle_t * handle) case TIMER: // case TIMER_WITH_CONTEXT: rc = rcl_timer_call(handle->timer); + + // cancled timer are not handled, return success + if (rc == RCL_RET_TIMER_CANCELED) { + rc = RCL_RET_OK; + break; + } + if (rc != RCL_RET_OK) { PRINT_RCLC_ERROR(rclc_execute, rcl_timer_call); return rc; diff --git a/rclc/test/rclc/test_executor.cpp b/rclc/test/rclc/test_executor.cpp index ed68faa7..7eca5caa 100644 --- a/rclc/test/rclc/test_executor.cpp +++ b/rclc/test/rclc/test_executor.cpp @@ -1021,6 +1021,95 @@ TEST_F(TestDefaultExecutor, executor_spin_timer) { EXPECT_EQ(RCL_RET_OK, rc) << rcl_get_error_string().str; } +TEST_F(TestDefaultExecutor, executor_spin_publisher_timer_cancelled) { + rcl_ret_t rc; + rclc_executor_t executor; + unsigned int expected_msg; + + rc = rclc_executor_init(&executor, &this->context, 10, this->allocator_ptr); + EXPECT_EQ(RCL_RET_OK, rc) << rcl_get_error_string().str; + + _executor_results_init(); + + rc = rclc_executor_add_subscription( + &executor, &this->sub1, &this->sub1_msg, + &CALLBACK_1, ON_NEW_DATA); + + rc = rclc_executor_add_timer(&executor, &this->timer1); + + for (unsigned int i = 0; i < TC_SPIN_SOME_PUBLISHED_MSGS; i++) { + rc = rcl_publish(&this->pub1, &this->pub1_msg, nullptr); + EXPECT_EQ(RCL_RET_OK, rc) << " pub1 not published"; + } + + // wait until messages are received + bool success = false; + unsigned int tries; + unsigned int max_tries = 100; + uint64_t timeout_ns = 100000000; // 100ms + + rc = rcl_timer_cancel(&this->timer1); + // process subscriptions. Assumption: messages for sub1 available + for (unsigned int i = 0; i < 100; i++) { + // Assumption: messages for all sub1, sub2 and sub3 are available + _wait_for_msg( + &this->sub1, &this->context, max_tries, timeout_ns, &tries, + &success); + ASSERT_TRUE(success); + + + EXPECT_EQ(RCL_RET_OK, rc) << "failed to cancel timer"; + + rc = rclc_executor_spin_some(&executor, rclc_test_timeout_ns); + if ((rc == RCL_RET_OK) || (rc == RCL_RET_TIMEOUT)) { + // valid return values + } else { + // any other error + EXPECT_EQ(RCL_RET_OK, rc) << "spin_some error"; + } + if (_cb1_cnt == TC_SPIN_SOME_PUBLISHED_MSGS) { + break; + } + } + + expected_msg = TC_SPIN_SOME_PUBLISHED_MSGS; + EXPECT_EQ(_cb1_cnt, expected_msg) << "cb1 msg does not match"; + + rc = rclc_executor_fini(&executor); + EXPECT_EQ(RCL_RET_OK, rc) << rcl_get_error_string().str; + rcutils_reset_error(); +} + +TEST_F(TestDefaultExecutor, executor_spin_timer_cancelled) { + rcl_ret_t rc; + rclc_executor_t executor; + rc = rclc_executor_init(&executor, &this->context, 10, this->allocator_ptr); + EXPECT_EQ(RCL_RET_OK, rc) << rcl_get_error_string().str; + + // spin_timeout must be < timer1_timeout + const unsigned int spin_timeout = 50; + const unsigned int spin_repeat = 10; + const unsigned int expected_callbacks = (spin_timeout * spin_repeat) / timer1_timeout; + _cbt_cnt = 0; + + rc = rclc_executor_add_timer(&executor, &this->timer1); + EXPECT_EQ(RCL_RET_OK, rc) << rcl_get_error_string().str; + + for (size_t i = 0; i < spin_repeat; i++) { + rclc_executor_spin_some(&executor, RCL_MS_TO_NS(spin_timeout)); + if (i > spin_repeat / 2) { + rc = rcl_timer_cancel(&this->timer1); + EXPECT_EQ(RCL_RET_OK, rc) << rcl_get_error_string().str; + } + } + + EXPECT_LT(_cbt_cnt, expected_callbacks); + + // tear down + rc = rclc_executor_fini(&executor); + EXPECT_EQ(RCL_RET_OK, rc) << rcl_get_error_string().str; +} + TEST_F(TestDefaultExecutor, executor_remove_timer) { rcl_ret_t rc; rclc_executor_t executor;