refactor(goal_planner): use the PullOverPath, PullOverPathCandidates …

…copied from ThreadData to reduce access (autowarefoundation#8994)

Signed-off-by: Mamoru Sobue <[email protected]>

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -110,15 +110,22 @@ struct PullOverContextData
   PullOverContextData() = delete;
   explicit PullOverContextData(
     const bool is_stable_safe_path, const PredictedObjects & static_objects,
-    const PredictedObjects & dynamic_objects)
+    const PredictedObjects & dynamic_objects, std::optional<PullOverPath> && pull_over_path_opt,
+    const std::vector<PullOverPath> & pull_over_path_candidates)
   : is_stable_safe_path(is_stable_safe_path),
     static_target_objects(static_objects),
-    dynamic_target_objects(dynamic_objects)
+    dynamic_target_objects(dynamic_objects),
+    pull_over_path_opt(pull_over_path_opt),
+    pull_over_path_candidates(pull_over_path_candidates)
   {
   }
   const bool is_stable_safe_path;
   const PredictedObjects static_target_objects;
   const PredictedObjects dynamic_target_objects;
+  // TODO(soblin): due to deceleratePath(), this cannot be const member(velocity is modified by it)
+  std::optional<PullOverPath> pull_over_path_opt;
+  const std::vector<PullOverPath> pull_over_path_candidates;
+  // TODO(soblin): goal_candidate_from_thread, modifed_goal_pose_from_thread, closest_start_pose
 };
 
 class GoalPlannerModule : public SceneModuleInterface
@@ -354,7 +361,7 @@ class GoalPlannerModule : public SceneModuleInterface
   void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const;
   void decelerateBeforeSearchStart(
     const Pose & search_start_offset_pose, PathWithLaneId & path) const;
-  PathWithLaneId generateStopPath() const;
+  PathWithLaneId generateStopPath(const PullOverContextData & context_data) const;
   PathWithLaneId generateFeasibleStopPath(const PathWithLaneId & path) const;
 
   void keepStoppedWithCurrentPath(PathWithLaneId & path) const;
@@ -364,7 +371,7 @@ class GoalPlannerModule : public SceneModuleInterface
   bool isStopped();
   bool isStopped(
     std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> & odometry_buffer, const double time);
-  bool hasFinishedCurrentPath();
+  bool hasFinishedCurrentPath(const PullOverContextData & ctx_data);
   bool isOnModifiedGoal(const Pose & current_pose, const GoalPlannerParameters & parameters) const;
   double calcModuleRequestLength() const;
   bool needPathUpdate(
@@ -406,16 +413,18 @@ class GoalPlannerModule : public SceneModuleInterface
 
   // lanes and drivable area
   std::vector<DrivableLanes> generateDrivableLanes() const;
-  void setDrivableAreaInfo(BehaviorModuleOutput & output) const;
+  void setDrivableAreaInfo(
+    const PullOverContextData & context_data, BehaviorModuleOutput & output) const;
 
   // output setter
   void setOutput(const PullOverContextData & context_data, BehaviorModuleOutput & output);
 
-  void setModifiedGoal(BehaviorModuleOutput & output) const;
-  void setTurnSignalInfo(BehaviorModuleOutput & output);
+  void setModifiedGoal(
+    const PullOverContextData & context_data, BehaviorModuleOutput & output) const;
+  void setTurnSignalInfo(const PullOverContextData & context_data, BehaviorModuleOutput & output);
 
   // new turn signal
-  TurnSignalInfo calcTurnSignalInfo();
+  TurnSignalInfo calcTurnSignalInfo(const PullOverContextData & context_data);
   std::optional<lanelet::Id> ignore_signal_{std::nullopt};
 
   bool hasPreviousModulePathShapeChanged(const BehaviorModuleOutput & previous_module_output) const;
@@ -431,10 +440,12 @@ class GoalPlannerModule : public SceneModuleInterface
 
   // steering factor
   void updateSteeringFactor(
-    const std::array<Pose, 2> & pose, const std::array<double, 2> distance, const uint16_t type);
+    const PullOverContextData & context_data, const std::array<Pose, 2> & pose,
+    const std::array<double, 2> distance, const uint16_t type);
 
   // rtc
-  std::pair<double, double> calcDistanceToPathChange() const;
+  std::pair<double, double> calcDistanceToPathChange(
+    const PullOverContextData & context_data) const;
 
   // safety check
   void initializeSafetyCheckParameters();

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp

@@ -47,6 +47,7 @@ struct PullOverPath
     const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel);
 
   PullOverPath(const PullOverPath & other);
+  PullOverPath & operator=(const PullOverPath & other) = default;
 
   PullOverPlannerType type() const { return type_; }
   size_t goal_id() const { return goal_id_; }
@@ -58,7 +59,7 @@ struct PullOverPath
   const std::vector<PathWithLaneId> & partial_paths() const { return partial_paths_; }
 
   // TODO(soblin): use reference to avoid copy once thread-safe design is finished
-  PathWithLaneId full_path() const { return full_path_; }
+  const PathWithLaneId & full_path() const { return full_path_; }
   PathWithLaneId parking_path() const { return parking_path_; }
   std::vector<double> full_path_curvatures() { return full_path_curvatures_; }
   std::vector<double> parking_path_curvatures() const { return parking_path_curvatures_; }