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 <[email protected]> * Add regression test Signed-off-by: Your Name <[email protected]> * Uncrustify Signed-off-by: Your Name <[email protected]> * Fix warning unused Signed-off-by: Your Name <[email protected]> Signed-off-by: Pablo Garrido <[email protected]> * return sucess on cancled timer Signed-off-by: Ilya Guterman <[email protected]> * validate cancled timer doesn't impact the execution of other handlers Signed-off-by: Ilya Guterman <[email protected]> * executor_spin_publisher_timer_cancelled cancel timer on beggining of the test Signed-off-by: Ilya Guterman <[email protected]> Co-authored-by: Pablo Garrido <[email protected]>
ros2 · Nov 29, 2021 · 27b4d4f · 27b4d4f
1 parent f8de341
commit 27b4d4f
Show file tree

Hide file tree

Showing 2 changed files with 96 additions and 0 deletions.
diff --git 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
@@ -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;