Merge pull request #1624 from tier4/fix/planning-process-death

fix(bvp): planning process death
tier4 · Nov 6, 2024 · 0d3012f · 0d3012f
2 parents c2abb5c + 61f4b5b
commit 0d3012f
Show file tree

Hide file tree

Showing 6 changed files with 626 additions and 44 deletions.
diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -197,8 +197,6 @@ CrosswalkModule::CrosswalkModule(
 
   collision_info_pub_ =
     node.create_publisher<tier4_debug_msgs::msg::StringStamped>("~/debug/collision_info", 1);
-
-  vehicle_stop_checker_ = std::make_unique<motion_utils::VehicleStopChecker>(&node);
 }
 
 bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
@@ -1221,8 +1219,7 @@ void CrosswalkModule::planStop(
 bool CrosswalkModule::checkRestartSuppression(
   const PathWithLaneId & ego_path, const std::optional<StopFactor> & stop_factor) const
 {
-  const auto is_vehicle_stopped = vehicle_stop_checker_->isVehicleStopped();
-  if (!is_vehicle_stopped) {
+  if (!planner_data_->isVehicleStopped()) {
     return false;
   }
 

diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
@@ -436,8 +436,6 @@ class CrosswalkModule : public SceneModuleInterface
   // Debug
   mutable DebugData debug_data_;
 
-  std::unique_ptr<motion_utils::VehicleStopChecker> vehicle_stop_checker_{nullptr};
-
   // Stop watch
   StopWatch<std::chrono::milliseconds> stop_watch_;
 

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++;
     }
   }
 }