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

* fix(bvp): remove expired module safely

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix: remove module id set

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix: use itr to erase expired module

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix: remove unused function

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp

@@ -450,20 +450,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++;
     }
   }
 }

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp

@@ -198,8 +198,6 @@ class SceneModuleManagerInterface
 
   void registerModule(const std::shared_ptr<SceneModuleInterface> & scene_module);
 
-  void unregisterModule(const std::shared_ptr<SceneModuleInterface> & scene_module);
-
   size_t findEgoSegmentIndex(
     const std::vector<tier4_planning_msgs::msg::PathPointWithLaneId> & points) const;
 

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp

@@ -170,13 +170,12 @@ void SceneModuleManagerInterface::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)) {
-      unregisterModule(scene_module);
+  auto itr = scene_modules_.begin();
+  while (itr != scene_modules_.end()) {
+    if (isModuleExpired(*itr)) {
+      itr = scene_modules_.erase(itr);
+    } else {
+      itr++;
     }
   }
 }
@@ -191,16 +190,6 @@ void SceneModuleManagerInterface::registerModule(
   scene_modules_.insert(scene_module);
 }
 
-void SceneModuleManagerInterface::unregisterModule(
-  const std::shared_ptr<SceneModuleInterface> & scene_module)
-{
-  RCLCPP_DEBUG(
-    logger_, "unregister task: module = %s, id = %lu", getModuleName(),
-    scene_module->getModuleId());
-  registered_module_id_set_.erase(scene_module->getModuleId());
-  scene_modules_.erase(scene_module);
-}
-
 SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC(
   rclcpp::Node & node, const char * module_name, const bool enable_rtc)
 : SceneModuleManagerInterface(node, module_name),
@@ -278,18 +267,18 @@ void SceneModuleManagerInterfaceWithRTC::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)) {
-      const UUID uuid = getUUID(scene_module->getModuleId());
+  auto itr = scene_modules_.begin();
+  while (itr != scene_modules_.end()) {
+    if (isModuleExpired(*itr)) {
+      const UUID uuid = getUUID((*itr)->getModuleId());
       updateRTCStatus(
-        uuid, scene_module->isSafe(), State::SUCCEEDED, std::numeric_limits<double>::lowest(),
+        uuid, (*itr)->isSafe(), State::SUCCEEDED, std::numeric_limits<double>::lowest(),
         clock_->now());
-      removeUUID(scene_module->getModuleId());
-      unregisterModule(scene_module);
+      removeUUID((*itr)->getModuleId());
+      registered_module_id_set_.erase((*itr)->getModuleId());
+      itr = scene_modules_.erase(itr);
+    } else {
+      itr++;
     }
   }
 }