refactor(goal_planner): add updateStatus and reduce variables

refactor Signed-off-by: kosuke55 <[email protected]> refactor Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Nov 9, 2023 · e242be0 · e242be0
1 parent e0c345e
commit e242be0
Show file tree

Hide file tree

Showing 3 changed files with 191 additions and 203 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
@@ -93,56 +93,30 @@ class PullOverStatus
   void reset()
   {
     lane_parking_pull_over_path_ = nullptr;
-    current_path_idx_ = 0;
-    require_increment_ = true;
     prev_stop_path_ = nullptr;
     prev_stop_path_after_approval_ = nullptr;
-    current_lanes_.clear();
-    pull_over_lanes_.clear();
-    lanes_.clear();
-    has_decided_path_ = false;
-    is_safe_static_objects_ = false;
-    is_safe_dynamic_objects_ = false;
+
+    is_safe_ = false;
     prev_found_path_ = false;
-    prev_is_safe_dynamic_objects_ = false;
-    has_decided_velocity_ = false;
-    has_requested_approval_ = false;
+    prev_is_safe_ = false;
   }
 
   DEFINE_SETTER_GETTER(std::shared_ptr<PullOverPath>, lane_parking_pull_over_path)
-  DEFINE_SETTER_GETTER(size_t, current_path_idx)
-  DEFINE_SETTER_GETTER(bool, require_increment)
   DEFINE_SETTER_GETTER(std::shared_ptr<PathWithLaneId>, prev_stop_path)
   DEFINE_SETTER_GETTER(std::shared_ptr<PathWithLaneId>, prev_stop_path_after_approval)
-  DEFINE_SETTER_GETTER(lanelet::ConstLanelets, current_lanes)
-  DEFINE_SETTER_GETTER(lanelet::ConstLanelets, pull_over_lanes)
-  DEFINE_SETTER_GETTER(std::vector<DrivableLanes>, lanes)
-  DEFINE_SETTER_GETTER(bool, has_decided_path)
-  DEFINE_SETTER_GETTER(bool, is_safe_static_objects)
-  DEFINE_SETTER_GETTER(bool, is_safe_dynamic_objects)
+  DEFINE_SETTER_GETTER(bool, is_safe)
   DEFINE_SETTER_GETTER(bool, prev_found_path)
-  DEFINE_SETTER_GETTER(bool, prev_is_safe_dynamic_objects)
-  DEFINE_SETTER_GETTER(bool, has_decided_velocity)
-  DEFINE_SETTER_GETTER(bool, has_requested_approval)
+  DEFINE_SETTER_GETTER(bool, prev_is_safe)
   DEFINE_SETTER_GETTER(Pose, refined_goal_pose)
   DEFINE_SETTER_GETTER(Pose, closest_goal_candidate_pose)
 
 private:
   std::shared_ptr<PullOverPath> lane_parking_pull_over_path_{nullptr};
-  size_t current_path_idx_{0};
-  bool require_increment_{true};
   std::shared_ptr<PathWithLaneId> prev_stop_path_{nullptr};
   std::shared_ptr<PathWithLaneId> prev_stop_path_after_approval_{nullptr};
-  lanelet::ConstLanelets current_lanes_{};
-  lanelet::ConstLanelets pull_over_lanes_{};
-  std::vector<DrivableLanes> lanes_{};
-  bool has_decided_path_{false};
-  bool is_safe_static_objects_{false};
-  bool is_safe_dynamic_objects_{false};
+  bool is_safe_{false};
   bool prev_found_path_{false};
-  bool prev_is_safe_dynamic_objects_{false};
-  bool has_decided_velocity_{false};
-  bool has_requested_approval_{false};
+  bool prev_is_safe_{false};
 
   Pose refined_goal_pose_{};
   Pose closest_goal_candidate_pose_{};
@@ -220,6 +194,16 @@ class ThreadSafeData
     return pull_over_path_->isValidPath();
   }
 
+  PullOverPlannerType getPullOverPlannerType() const
+  {
+    const std::lock_guard<std::recursive_mutex> lock(mutex_);
+    if (!pull_over_path_) {
+      return PullOverPlannerType::NONE;
+    }
+
+    return pull_over_path_->type;
+  };
+
   void reset()
   {
     const std::lock_guard<std::recursive_mutex> lock(mutex_);
@@ -389,10 +373,10 @@ 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();
-  PathWithLaneId generateFeasibleStopPath();
+  PathWithLaneId generateStopPath() const;
+  PathWithLaneId generateFeasibleStopPath() const;
 
-  void keepStoppedWithCurrentPath(PathWithLaneId & path);
+  void keepStoppedWithCurrentPath(PathWithLaneId & path) const;
   double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const;
 
   // status
@@ -407,6 +391,7 @@ class GoalPlannerModule : public SceneModuleInterface
   bool hasDecidedPath() const;
   void decideVelocity();
   bool foundPullOverPath() const;
+  void updateStatus(const BehaviorModuleOutput & output);
 
   // validation
   bool hasEnoughDistance(const PullOverPath & pull_over_path) const;
@@ -429,16 +414,13 @@ class GoalPlannerModule : public SceneModuleInterface
     const std::vector<PullOverPath> & pull_over_path_candidates,
     const GoalCandidates & goal_candidates) const;
 
-  // deal with pull over partial paths
-  void transitionToNextPathIfFinishingCurrentPath();
-
   // lanes and drivable area
-  void setLanes();
+  std::vector<DrivableLanes> generateDrivableLanes() const;
   void setDrivableAreaInfo(BehaviorModuleOutput & output) const;
 
   // output setter
-  void setOutput(BehaviorModuleOutput & output);
-  void setStopPath(BehaviorModuleOutput & output);
+  void setOutput(BehaviorModuleOutput & output) const;
+  void setStopPath(BehaviorModuleOutput & output) const;
 
   /**
    * @brief Sets a stop path in the current path based on safety conditions and previous paths.
@@ -449,7 +431,7 @@ class GoalPlannerModule : public SceneModuleInterface
    *
    * @param output BehaviorModuleOutput
    */
-  void setStopPathFromCurrentPath(BehaviorModuleOutput & output);
+  void setStopPathFromCurrentPath(BehaviorModuleOutput & output) const;
   void setModifiedGoal(BehaviorModuleOutput & output) const;
   void setTurnSignalInfo(BehaviorModuleOutput & output) const;
 

diff --git a/..._path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/..._path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp
@@ -53,6 +53,7 @@ struct PullOverPath
   Pose end_pose{};
   std::vector<Pose> debug_poses{};
   size_t goal_id{};
+  bool decided_velocity{false};
 
   PathWithLaneId getFullPath() const
   {