refactor(goal_planner): refactor PullOverPlannseBase to instantiate only valid path

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt

@@ -10,6 +10,7 @@ autoware_package()
 pluginlib_export_plugin_description_file(autoware_behavior_path_planner plugins.xml)
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/pull_over_planner/pull_over_planner_base.cpp
   src/pull_over_planner/freespace_pull_over.cpp
   src/pull_over_planner/geometric_pull_over.cpp
   src/pull_over_planner/shift_pull_over.cpp

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp

@@ -265,7 +265,7 @@ int main(int argc, char ** argv)
   auto shift_pull_over_planner = autoware::behavior_path_planner::ShiftPullOver(
     *node, goal_planner_parameter, lane_departure_checker);
   const auto pull_over_path_opt =
-    shift_pull_over_planner.plan(planner_data, reference_path, route_msg.goal_pose);
+    shift_pull_over_planner.plan(0, 0, planner_data, reference_path, route_msg.goal_pose);
 
   pybind11::scoped_interpreter guard{};
   auto plt = matplotlibcpp17::pyplot::import();
@@ -282,7 +282,7 @@ int main(int argc, char ** argv)
   std::cout << pull_over_path_opt.has_value() << std::endl;
   if (pull_over_path_opt) {
     const auto & pull_over_path = pull_over_path_opt.value();
-    const auto full_path = pull_over_path.getFullPath();
+    const auto & full_path = pull_over_path.full_path;
     plot_path_with_lane_id(ax, full_path);
   }
   ax.set_aspect(Args("equal"));

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

@@ -160,17 +160,16 @@ class ThreadSafeData
       return false;
     }
 
-    return pull_over_path_->isValidPath();
+    return true;
   }
 
-  PullOverPlannerType getPullOverPlannerType() const
+  std::optional<PullOverPlannerType> getPullOverPlannerType() const
   {
     const std::lock_guard<std::recursive_mutex> lock(mutex_);
     if (!pull_over_path_) {
-      return PullOverPlannerType::NONE;
+      return std::nullopt;
     }
-
-    return pull_over_path_->type;
+    return pull_over_path_->type();
   };
 
   void reset()
@@ -206,7 +205,7 @@ class ThreadSafeData
   void set_pull_over_path_no_lock(const PullOverPath & path)
   {
     pull_over_path_ = std::make_shared<PullOverPath>(path);
-    if (path.type != PullOverPlannerType::NONE && path.type != PullOverPlannerType::FREESPACE) {
+    if (path.type() != PullOverPlannerType::FREESPACE) {
       lane_parking_pull_over_path_ = std::make_shared<PullOverPath>(path);
     }
 
@@ -216,7 +215,7 @@ class ThreadSafeData
   void set_pull_over_path_no_lock(const std::shared_ptr<PullOverPath> & path)
   {
     pull_over_path_ = path;
-    if (path->type != PullOverPlannerType::NONE && path->type != PullOverPlannerType::FREESPACE) {
+    if (path->type() != PullOverPlannerType::FREESPACE) {
       lane_parking_pull_over_path_ = path;
     }
     last_path_update_time_ = clock_->now();

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

@@ -42,7 +42,7 @@ class FreespacePullOver : public PullOverPlannerBase
   PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; }
 
   std::optional<PullOverPath> plan(
-    const std::shared_ptr<const PlannerData> planner_data,
+    const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
     const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override;
 
 protected:

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

@@ -43,7 +43,7 @@ class GeometricPullOver : public PullOverPlannerBase
   Pose getCl() const { return planner_.getCl(); }
 
   std::optional<PullOverPath> plan(
-    const std::shared_ptr<const PlannerData> planner_data,
+    const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
     const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override;
 
   std::vector<PullOverPath> generatePullOverPaths(

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

@@ -31,7 +31,6 @@ using tier4_planning_msgs::msg::PathWithLaneId;
 namespace autoware::behavior_path_planner
 {
 enum class PullOverPlannerType {
-  NONE = 0,
   SHIFT,
   ARC_FORWARD,
   ARC_BACKWARD,
@@ -40,137 +39,77 @@ enum class PullOverPlannerType {
 
 struct PullOverPath
 {
-  PullOverPlannerType type{PullOverPlannerType::NONE};
-  std::vector<PathWithLaneId> partial_paths{};
-  size_t path_idx{0};
-  // accelerate with constant acceleration to the target velocity
-  std::vector<std::pair<double, double>> pairs_terminal_velocity_and_accel{};
-  Pose start_pose{};
-  Pose end_pose{};
-  std::vector<Pose> debug_poses{};
-  size_t goal_id{};
-  size_t id{};
-  bool decided_velocity{false};
-
-  /**
-   * @brief Set paths and start/end poses
-   * By setting partial_paths, full_path, parking_path and curvature are also set at the same time
-   * @param input_partial_paths partial paths
-   * @param input_start_pose start pose
-   * @param input_end_pose end pose
-   */
-  void setPaths(
-    const std::vector<PathWithLaneId> input_partial_paths, const Pose & input_start_pose,
-    const Pose & input_end_pose)
-  {
-    partial_paths = input_partial_paths;
-    start_pose = input_start_pose;
-    end_pose = input_end_pose;
+public:
+  static std::optional<PullOverPath> create(
+    const PullOverPlannerType & type, const size_t goal_id, const size_t id,
+    const std::vector<PathWithLaneId> & partial_paths, const Pose & start_pose,
+    const Pose & end_pose,
+    const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel);
 
-    updatePathData();
-  }
+  PullOverPath(const PullOverPath & other);
 
-  // Note: return copy value (not const&) because the value is used in multi threads
-  PathWithLaneId getFullPath() const { return full_path; }
+  PullOverPlannerType type() const { return type_; }
+  size_t goal_id() const { return goal_id_; }
+  size_t id() const { return id_; }
+  Pose start_pose() const { return start_pose_; }
+  Pose end_pose() const { return end_pose_; }
 
-  PathWithLaneId getParkingPath() const { return parking_path; }
+  std::vector<PathWithLaneId> & partial_paths() { return partial_paths_; }
+  const std::vector<PathWithLaneId> & partial_paths() const { return partial_paths_; }
 
-  PathWithLaneId getCurrentPath() const
-  {
-    if (partial_paths.empty()) {
-      return PathWithLaneId{};
-    } else if (partial_paths.size() <= path_idx) {
-      return partial_paths.back();
-    }
-    return partial_paths.at(path_idx);
-  }
+  // TODO(soblin): use reference to avoid copy once thread-safe design is finished
+  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_; }
+  double full_path_max_curvature() const { return full_path_max_curvature_; }
+  double parking_path_max_curvature() const { return parking_path_max_curvature_; }
+  size_t path_idx() const { return path_idx_; }
 
-  bool incrementPathIndex()
-  {
-    if (partial_paths.size() - 1 <= path_idx) {
-      return false;
-    }
-    path_idx += 1;
-    return true;
-  }
+  bool incrementPathIndex();
 
-  bool isValidPath() const { return type != PullOverPlannerType::NONE; }
+  // TODO(soblin): this cannot be const due to decelerateBeforeSearchStart
+  PathWithLaneId & getCurrentPath();
 
-  std::vector<double> getFullPathCurvatures() const { return full_path_curvatures; }
-  std::vector<double> getParkingPathCurvatures() const { return parking_path_curvatures; }
-  double getFullPathMaxCurvature() const { return full_path_max_curvature; }
-  double getParkingPathMaxCurvature() const { return parking_path_max_curvature; }
+  const PathWithLaneId & getCurrentPath() const;
 
-private:
-  void updatePathData()
+  std::pair<double, double> getPairsTerminalVelocityAndAccel() const
   {
-    updateFullPath();
-    updateParkingPath();
-    updateCurvatures();
-  }
-
-  void updateFullPath()
-  {
-    PathWithLaneId path{};
-    for (size_t i = 0; i < partial_paths.size(); ++i) {
-      if (i == 0) {
-        path.points.insert(
-          path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end());
-      } else {
-        // skip overlapping point
-        path.points.insert(
-          path.points.end(), next(partial_paths.at(i).points.begin()),
-          partial_paths.at(i).points.end());
-      }
+    if (pairs_terminal_velocity_and_accel_.size() <= path_idx_) {
+      return std::make_pair(0.0, 0.0);
     }
-    full_path.points = autoware::motion_utils::removeOverlapPoints(path.points);
+    return pairs_terminal_velocity_and_accel_.at(path_idx_);
   }
 
-  void updateParkingPath()
-  {
-    if (full_path.points.empty()) {
-      updateFullPath();
-    }
-    const size_t start_idx =
-      autoware::motion_utils::findNearestIndex(full_path.points, start_pose.position);
-
-    PathWithLaneId path{};
-    std::copy(
-      full_path.points.begin() + start_idx, full_path.points.end(),
-      std::back_inserter(path.points));
-    parking_path = path;
-  }
+  std::vector<Pose> debug_poses{};
 
-  void updateCurvatures()
-  {
-    const auto calculateCurvaturesAndMax =
-      [](const auto & path) -> std::pair<std::vector<double>, double> {
-      std::vector<double> curvatures = autoware::motion_utils::calcCurvature(path.points);
-      double max_curvature = 0.0;
-      if (!curvatures.empty()) {
-        max_curvature = std::abs(*std::max_element(
-          curvatures.begin(), curvatures.end(),
-          [](const double & a, const double & b) { return std::abs(a) < std::abs(b); }));
-      }
-      return std::make_pair(curvatures, max_curvature);
-    };
-    std::tie(full_path_curvatures, full_path_max_curvature) =
-      calculateCurvaturesAndMax(getFullPath());
-    std::tie(parking_path_curvatures, parking_path_max_curvature) =
-      calculateCurvaturesAndMax(getParkingPath());
-  }
+private:
+  PullOverPath(
+    const PullOverPlannerType & type, const size_t goal_id, const size_t id,
+    const Pose & start_pose, const Pose & end_pose,
+    const std::vector<PathWithLaneId> & partial_paths, const PathWithLaneId & full_path,
+    const PathWithLaneId & parking_path, const std::vector<double> & full_path_curvatures,
+    const std::vector<double> & parking_path_curvatures, const double full_path_max_curvature,
+    const double parking_path_max_curvature,
+    const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel);
+
+  PullOverPlannerType type_;
+  size_t goal_id_;
+  size_t id_;
+  Pose start_pose_;
+  Pose end_pose_;
+
+  std::vector<PathWithLaneId> partial_paths_;
+  PathWithLaneId full_path_;
+  PathWithLaneId parking_path_;
+  std::vector<double> full_path_curvatures_;
+  std::vector<double> parking_path_curvatures_;
+  double full_path_max_curvature_;
+  double parking_path_max_curvature_;
 
-  // curvatures
-  std::vector<double> full_path_curvatures{};
-  std::vector<double> parking_path_curvatures{};
-  std::vector<double> current_path_curvatures{};
-  double parking_path_max_curvature{0.0};
-  double full_path_max_curvature{0.0};
-  double current_path_max_curvature{0.0};
-
-  // path
-  PathWithLaneId full_path{};
-  PathWithLaneId parking_path{};
+  // accelerate with constant acceleration to the target velocity
+  size_t path_idx_;
+  std::vector<std::pair<double, double>> pairs_terminal_velocity_and_accel_;
 };
 
 class PullOverPlannerBase
@@ -186,7 +125,7 @@ class PullOverPlannerBase
 
   virtual PullOverPlannerType getPlannerType() const = 0;
   virtual std::optional<PullOverPath> plan(
-    const std::shared_ptr<const PlannerData> planner_data,
+    const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
     const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) = 0;
 
 protected:

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

@@ -36,7 +36,7 @@ class ShiftPullOver : public PullOverPlannerBase
     const LaneDepartureChecker & lane_departure_checker);
   PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; };
   std::optional<PullOverPath> plan(
-    const std::shared_ptr<const PlannerData> planner_data,
+    const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
     const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override;
 
 protected:
@@ -46,7 +46,7 @@ class ShiftPullOver : public PullOverPlannerBase
   std::optional<PathWithLaneId> cropPrevModulePath(
     const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const;
   std::optional<PullOverPath> generatePullOverPath(
-    const std::shared_ptr<const PlannerData> planner_data,
+    const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
     const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes,
     const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose,
     const double lateral_jerk) const;