fix(behavior_path_planner): skip unnecessary process in updateDate() …

…when module_status is not RUNNING (autowarefoundation#4031) * update Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * skip unnecessary process in updateDate() when module_status is not RUNNING Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * skip unnecessary process in updateDate() when module_status is not RUNNING Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> --------- Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
h-ohta · Jun 22, 2023 · 6dc708c · 6dc708c
1 parent a9cb76c
commit 6dc708c
Showing 1 changed file with 16 additions and 12 deletions.
diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp
@@ -186,6 +186,22 @@ ModuleStatus SideShiftModule::updateState()
 
 void SideShiftModule::updateData()
 {
+#ifndef USE_OLD_ARCHITECTURE
+  if (
+    planner_data_->lateral_offset != nullptr &&
+    planner_data_->lateral_offset->stamp != latest_lateral_offset_stamp_) {
+    if (isReadyForNextRequest(parameters_->shift_request_time_limit)) {
+      lateral_offset_change_request_ = true;
+      requested_lateral_offset_ = planner_data_->lateral_offset->lateral_offset;
+      latest_lateral_offset_stamp_ = planner_data_->lateral_offset->stamp;
+    }
+  }
+#endif
+
+  if (current_state_ != ModuleStatus::RUNNING) {
+    return;
+  }
+
   // special for avoidance: take behind distance upt ot shift-start-point if it exist.
   const auto longest_dist_to_shift_line = [&]() {
     double max_dist = 0.0;
@@ -238,18 +254,6 @@ void SideShiftModule::updateData()
 
   const size_t nearest_idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points);
   path_shifter_.removeBehindShiftLineAndSetBaseOffset(nearest_idx);
-
-#ifndef USE_OLD_ARCHITECTURE
-  if (
-    planner_data_->lateral_offset != nullptr &&
-    planner_data_->lateral_offset->stamp != latest_lateral_offset_stamp_) {
-    if (isReadyForNextRequest(parameters_->shift_request_time_limit)) {
-      lateral_offset_change_request_ = true;
-      requested_lateral_offset_ = planner_data_->lateral_offset->lateral_offset;
-      latest_lateral_offset_stamp_ = planner_data_->lateral_offset->stamp;
-    }
-  }
-#endif
 }
 
 void SideShiftModule::replaceShiftLine()