Fix bug on the disorder of calling shutdown callback (ros2#2097)

* Fix bug on the disorder of calling shutdown callback Signed-off-by: Barry Xu <barry.xu@sony.com>
irobot-ros · May 3, 2023 · b8ce85b · b8ce85b
1 parent 27a7a15
commit b8ce85b
Show file tree

Hide file tree

Showing 4 changed files with 235 additions and 6 deletions.
diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp
@@ -373,10 +373,10 @@ class Context : public std::enable_shared_from_this<Context>
   // attempt to acquire another sub context.
   std::recursive_mutex sub_contexts_mutex_;
 
-  std::unordered_set<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
+  std::vector<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
   mutable std::mutex on_shutdown_callbacks_mutex_;
 
-  std::unordered_set<std::shared_ptr<PreShutdownCallback>> pre_shutdown_callbacks_;
+  std::vector<std::shared_ptr<PreShutdownCallback>> pre_shutdown_callbacks_;
   mutable std::mutex pre_shutdown_callbacks_mutex_;
 
   /// Condition variable for timed sleep (see sleep_for).

diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp
@@ -398,10 +398,10 @@ Context::add_shutdown_callback(
 
   if constexpr (shutdown_type == ShutdownType::pre_shutdown) {
     std::lock_guard<std::mutex> lock(pre_shutdown_callbacks_mutex_);
-    pre_shutdown_callbacks_.emplace(callback_shared_ptr);
+    pre_shutdown_callbacks_.emplace_back(callback_shared_ptr);
   } else {
     std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
-    on_shutdown_callbacks_.emplace(callback_shared_ptr);
+    on_shutdown_callbacks_.emplace_back(callback_shared_ptr);
   }
 
   ShutdownCallbackHandle callback_handle;
@@ -419,9 +419,19 @@ Context::remove_shutdown_callback(
     return false;
   }
 
-  const auto remove_callback = [this, &callback_shared_ptr](auto & mutex, auto & callback_set) {
+  const auto remove_callback = [&callback_shared_ptr](auto & mutex, auto & callback_vector) {
       const std::lock_guard<std::mutex> lock(mutex);
-      return callback_set.erase(callback_shared_ptr) == 1;
+      auto iter = callback_vector.begin();
+      for (; iter != callback_vector.end(); iter++) {
+        if ((*iter).get() == callback_shared_ptr.get()) {
+          break;
+        }
+      }
+      if (iter == callback_vector.end()) {
+        return false;
+      }
+      callback_vector.erase(iter);
+      return true;
     };
 
   static_assert(

diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt
@@ -572,6 +572,9 @@ target_link_libraries(test_logger ${PROJECT_NAME})
 ament_add_gmock(test_logging test_logging.cpp)
 target_link_libraries(test_logging ${PROJECT_NAME})
 
+ament_add_gmock(test_context test_context.cpp)
+target_link_libraries(test_context ${PROJECT_NAME})
+
 ament_add_gtest(test_time test_time.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
 if(TARGET test_time)

diff --git a/rclcpp/test/rclcpp/test_context.cpp b/rclcpp/test/rclcpp/test_context.cpp
@@ -0,0 +1,216 @@
+// Copyright 2023 Sony Group Corporation.
+//
+// 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 <gtest/gtest.h>
+
+#include "rclcpp/context.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+TEST(TestContext, check_pre_shutdown_callback_order) {
+  auto context = std::make_shared<rclcpp::Context>();
+  context->init(0, nullptr);
+
+  int result[4] = {0, 0, 0, 0};
+  int index = 0;
+
+  auto callback1 = [&result, &index]() {
+      result[index] = 1;
+      index++;
+    };
+  auto callback2 = [&result, &index]() {
+      result[index] = 2;
+      index++;
+    };
+  auto callback3 = [&result, &index]() {
+      result[index] = 3;
+      index++;
+    };
+  auto callback4 = [&result, &index]() {
+      result[index] = 4;
+      index++;
+    };
+
+  context->add_pre_shutdown_callback(callback1);
+  context->add_pre_shutdown_callback(callback2);
+  context->add_pre_shutdown_callback(callback3);
+  context->add_pre_shutdown_callback(callback4);
+
+  context->shutdown("for test");
+
+  EXPECT_TRUE(result[0] == 1 && result[1] == 2 && result[2] == 3 && result[3] == 4);
+}
+
+TEST(TestContext, check_on_shutdown_callback_order) {
+  auto context = std::make_shared<rclcpp::Context>();
+  context->init(0, nullptr);
+
+  int result[4] = {0, 0, 0, 0};
+  int index = 0;
+
+  auto callback1 = [&result, &index]() {
+      result[index] = 1;
+      index++;
+    };
+  auto callback2 = [&result, &index]() {
+      result[index] = 2;
+      index++;
+    };
+  auto callback3 = [&result, &index]() {
+      result[index] = 3;
+      index++;
+    };
+  auto callback4 = [&result, &index]() {
+      result[index] = 4;
+      index++;
+    };
+
+  context->add_on_shutdown_callback(callback1);
+  context->add_on_shutdown_callback(callback2);
+  context->add_on_shutdown_callback(callback3);
+  context->add_on_shutdown_callback(callback4);
+
+  context->shutdown("for test");
+
+  EXPECT_TRUE(result[0] == 1 && result[1] == 2 && result[2] == 3 && result[3] == 4);
+}
+
+TEST(TestContext, check_mixed_register_shutdown_callback_order) {
+  auto context = std::make_shared<rclcpp::Context>();
+  context->init(0, nullptr);
+
+  int result[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+  int index = 0;
+
+  auto callback1 = [&result, &index]() {
+      result[index] = 1;
+      index++;
+    };
+  auto callback2 = [&result, &index]() {
+      result[index] = 2;
+      index++;
+    };
+  auto callback3 = [&result, &index]() {
+      result[index] = 3;
+      index++;
+    };
+  auto callback4 = [&result, &index]() {
+      result[index] = 4;
+      index++;
+    };
+  auto callback5 = [&result, &index]() {
+      result[index] = 5;
+      index++;
+    };
+  auto callback6 = [&result, &index]() {
+      result[index] = 6;
+      index++;
+    };
+  auto callback7 = [&result, &index]() {
+      result[index] = 7;
+      index++;
+    };
+  auto callback8 = [&result, &index]() {
+      result[index] = 8;
+      index++;
+    };
+
+  // Mixed register
+  context->add_pre_shutdown_callback(callback1);
+  context->add_on_shutdown_callback(callback5);
+  context->add_pre_shutdown_callback(callback2);
+  context->add_on_shutdown_callback(callback6);
+  context->add_pre_shutdown_callback(callback3);
+  context->add_on_shutdown_callback(callback7);
+  context->add_pre_shutdown_callback(callback4);
+  context->add_on_shutdown_callback(callback8);
+
+  context->shutdown("for test");
+
+  EXPECT_TRUE(
+    result[0] == 1 && result[1] == 2 && result[2] == 3 && result[3] == 4 &&
+    result[4] == 5 && result[5] == 6 && result[6] == 7 && result[7] == 8);
+}
+
+TEST(TestContext, check_pre_shutdown_callback_order_after_del) {
+  auto context = std::make_shared<rclcpp::Context>();
+  context->init(0, nullptr);
+
+  int result[4] = {0, 0, 0, 0};
+  int index = 0;
+
+  auto callback1 = [&result, &index]() {
+      result[index] = 1;
+      index++;
+    };
+  auto callback2 = [&result, &index]() {
+      result[index] = 2;
+      index++;
+    };
+  auto callback3 = [&result, &index]() {
+      result[index] = 3;
+      index++;
+    };
+  auto callback4 = [&result, &index]() {
+      result[index] = 4;
+      index++;
+    };
+
+  context->add_pre_shutdown_callback(callback1);
+  auto callback_handle = context->add_pre_shutdown_callback(callback2);
+  context->add_pre_shutdown_callback(callback3);
+  context->add_pre_shutdown_callback(callback4);
+
+  EXPECT_TRUE(context->remove_pre_shutdown_callback(callback_handle));
+  EXPECT_FALSE(context->remove_pre_shutdown_callback(callback_handle));
+
+  context->shutdown("for test");
+
+  EXPECT_TRUE(result[0] == 1 && result[1] == 3 && result[2] == 4 && result[3] == 0);
+}
+
+TEST(TestContext, check_on_shutdown_callback_order_after_del) {
+  auto context = std::make_shared<rclcpp::Context>();
+  context->init(0, nullptr);
+
+  int result[4] = {0, 0, 0, 0};
+  int index = 0;
+
+  auto callback1 = [&result, &index]() {
+      result[index] = 1;
+      index++;
+    };
+  auto callback2 = [&result, &index]() {
+      result[index] = 2;
+      index++;
+    };
+  auto callback3 = [&result, &index]() {
+      result[index] = 3;
+      index++;
+    };
+  auto callback4 = [&result, &index]() {
+      result[index] = 4;
+      index++;
+    };
+
+  context->add_on_shutdown_callback(callback1);
+  auto callback_handle = context->add_on_shutdown_callback(callback2);
+  context->add_on_shutdown_callback(callback3);
+  context->add_on_shutdown_callback(callback4);
+
+  EXPECT_TRUE(context->remove_on_shutdown_callback(callback_handle));
+  EXPECT_FALSE(context->remove_on_shutdown_callback(callback_handle));
+
+  context->shutdown("for test");
+
+  EXPECT_TRUE(result[0] == 1 && result[1] == 3 && result[2] == 4 && result[3] == 0);
+}