move to updatedata

Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Nov 10, 2023 · 504d1ae · 504d1ae
1 parent 9a705dc
commit 504d1ae
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 12 deletions.
diff --git a/...h_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/...h_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp
@@ -157,6 +157,10 @@ class ThreadSafeData
   bool incrementPathIndex()
   {
     const std::lock_guard<std::recursive_mutex> lock(mutex_);
+    if (!pull_over_path_) {
+      return false;
+    }
+
     if (pull_over_path_->incrementPathIndex()) {
       last_path_idx_increment_time_ = clock_->now();
       return true;
@@ -242,7 +246,6 @@ class ThreadSafeData
 #undef DEFINE_GETTER_WITH_MUTEX
 #undef DEFINE_SETTER_GETTER_WITH_MUTEX
 
-
 struct FreespacePlannerDebugData
 {
   bool is_planning{false};

diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
@@ -256,6 +256,20 @@ void GoalPlannerModule::updateData()
   updateOccupancyGrid();
 
   generateGoalCandidates();
+
+  if (!isActivated()) {
+    return;
+  }
+
+  if (hasFinishedCurrentPath()) {
+    thread_safe_data_.incrementPathIndex();
+  }
+
+  if (!last_approval_data_) {
+    last_approval_data_ =
+      std::make_unique<LastApprovalData>(clock_->now(), planner_data_->self_odometry->pose.pose);
+    decideVelocity();
+  }
 }
 
 void GoalPlannerModule::initializeOccupancyGridMap()
@@ -932,16 +946,6 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification()
 
   setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath());
 
-  if (hasFinishedCurrentPath()) {
-    thread_safe_data_.incrementPathIndex();
-  }
-
-  if (isActivated() && !last_approval_data_) {
-    last_approval_data_ =
-      std::make_unique<LastApprovalData>(clock_->now(), planner_data_->self_odometry->pose.pose);
-    decideVelocity();
-  }
-
   updateStatus(output);
 
   return output;
@@ -1248,7 +1252,7 @@ bool GoalPlannerModule::isStuck()
 
 bool GoalPlannerModule::hasFinishedCurrentPath()
 {
-  if (!isActivated() || !last_approval_data_) {
+  if (!last_approval_data_) {
     return false;
   }