fix(bvp): remove expired module safely (autowarefoundation#9212)

* fix(bvp): remove expired module safely Signed-off-by: satoshi-ota <[email protected]> * fix: remove module id set Signed-off-by: satoshi-ota <[email protected]> * fix: use itr to erase expired module Signed-off-by: satoshi-ota <[email protected]> * fix: remove unused function Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
tier4 · Nov 6, 2024 · 61f4b5b · 61f4b5b
1 parent 200781f
commit 61f4b5b
Show file tree

Hide file tree

Showing 4 changed files with 625 additions and 38 deletions.
diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -303,20 +303,20 @@ void IntersectionModuleManager::deleteExpiredModules(
 {
   const auto isModuleExpired = getModuleExpiredFunction(path);
 
-  // Copy container to avoid iterator corruption
-  // due to scene_modules_.erase() in unregisterModule()
-  const auto copied_scene_modules = scene_modules_;
-
-  for (const auto & scene_module : copied_scene_modules) {
-    if (isModuleExpired(scene_module)) {
+  auto itr = scene_modules_.begin();
+  while (itr != scene_modules_.end()) {
+    if (isModuleExpired(*itr)) {
       // default
-      removeRTCStatus(getUUID(scene_module->getModuleId()));
-      removeUUID(scene_module->getModuleId());
+      removeRTCStatus(getUUID((*itr)->getModuleId()));
+      removeUUID((*itr)->getModuleId());
       // occlusion
-      const auto intersection_module = std::dynamic_pointer_cast<IntersectionModule>(scene_module);
+      const auto intersection_module = std::dynamic_pointer_cast<IntersectionModule>(*itr);
       const auto occlusion_uuid = intersection_module->getOcclusionUUID();
       occlusion_rtc_interface_.removeCooperateStatus(occlusion_uuid);
-      unregisterModule(scene_module);
+      registered_module_id_set_.erase((*itr)->getModuleId());
+      itr = scene_modules_.erase(itr);
+    } else {
+      itr++;
     }
   }
 }