temp

Signed-off-by: kyoichi-sugahara <[email protected]>
kyoichi-sugahara · Oct 24, 2023 · eff7600 · eff7600
1 parent 7c3bde1
commit eff7600
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 8 deletions.
diff --git a/...planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/...planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp
@@ -100,6 +100,7 @@ class StartPlannerModule : public SceneModuleInterface
   void processOnEntry() override;
   void processOnExit() override;
   void updateData() override;
+  void postProcess() override;
 
   void setParameters(const std::shared_ptr<StartPlannerParameters> & parameters)
   {

diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
@@ -346,6 +346,35 @@ BehaviorModuleOutput StartPlannerModule::plan()
   return output;
 }
 
+void StartPlannerModule::postProcess()
+{
+  // .is_safe_static_objects
+  // .back_finished　の2つを更新する
+  const std::lock_guard<std::mutex> lock(mutex_);
+  switch (status_.planner_type) {
+    case PlannerType::STOP:
+      status_.back_finished = true;
+      // is_safe_static_objects can be set to false? in this case.
+      status_.is_safe_static_objects = false;
+      const Pose & current_pose = planner_data_->self_odometry->pose.pose;
+      status_.pull_out_start_pose = current_pose;
+      status_.pull_out_path.start_pose = current_pose;
+      status_.pull_out_path.end_pose = current_pose;
+      break;
+
+    case PlannerType::FREESPACE:
+    case PlannerType::SHIFT:
+    case PlannerType::GEOMETRIC:
+
+      status_.is_safe_static_objects = true;
+      status_.back_finished = true;
+      break;
+
+    default:
+      break;
+  }
+}
+
 CandidateOutput StartPlannerModule::planCandidate() const
 {
   return CandidateOutput{};
@@ -509,6 +538,7 @@ void StartPlannerModule::planWithPriority(
     const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose);
     // not found safe path
     if (!pull_out_path) {
+      status_.is_safe_static_objects = false;
       return false;
     }
     // use current path if back is not needed
@@ -1157,14 +1187,8 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput()
 
   {
     const std::lock_guard<std::mutex> lock(mutex_);
-    status_.back_finished = true;
-    status_.planner_type = PlannerType::STOP;
     status_.pull_out_path.partial_paths.clear();
     status_.pull_out_path.partial_paths.push_back(stop_path);
-    const Pose & current_pose = planner_data_->self_odometry->pose.pose;
-    status_.pull_out_start_pose = current_pose;
-    status_.pull_out_path.start_pose = current_pose;
-    status_.pull_out_path.end_pose = current_pose;
   }
 
   path_candidate_ = std::make_shared<PathWithLaneId>(stop_path);
@@ -1209,8 +1233,6 @@ bool StartPlannerModule::planFreespacePath()
     status_.pull_out_path = *freespace_path;
     status_.pull_out_start_pose = current_pose;
     status_.planner_type = freespace_planner_->getPlannerType();
-    status_.is_safe_static_objects = true;
-    status_.back_finished = true;
     return true;
   }