refactor(goal planner): hold modified_goal in PullOverPath ,copy modified goal once from background thread (autowarefoundation#9006)

refactor(goal_planner): save modified_goal_pose in PullOverPlannerBase

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

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

@@ -37,8 +37,9 @@ class FreespacePullOver : public PullOverPlannerBase
   PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; }
 
   std::optional<PullOverPath> plan(
-    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;
+    const GoalCandidate & modified_goal_pose, const size_t id,
+    const std::shared_ptr<const PlannerData> planner_data,
+    const BehaviorModuleOutput & previous_module_output) override;
 
 protected:
   const double velocity_;

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,8 +43,9 @@ class GeometricPullOver : public PullOverPlannerBase
   Pose getCl() const { return planner_.getCl(); }
 
   std::optional<PullOverPath> plan(
-    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;
+    const GoalCandidate & modified_goal_pose, const size_t id,
+    const std::shared_ptr<const PlannerData> planner_data,
+    const BehaviorModuleOutput & previous_module_output) override;
 
   std::vector<PullOverPath> generatePullOverPaths(
     const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,

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

@@ -15,6 +15,7 @@
 #pragma once
 
 #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp"
+#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp"
 #include "autoware/behavior_path_planner_common/data_manager.hpp"
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -41,19 +42,20 @@ struct PullOverPath
 {
 public:
   static std::optional<PullOverPath> create(
-    const PullOverPlannerType & type, const size_t goal_id, const size_t id,
+    const PullOverPlannerType & type, const size_t id,
     const std::vector<PathWithLaneId> & partial_paths, const Pose & start_pose,
-    const Pose & end_pose,
+    const GoalCandidate & modified_goal_pose,
     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_; }
+  size_t goal_id() const { return modified_goal_pose_.id; }
   size_t id() const { return id_; }
   Pose start_pose() const { return start_pose_; }
-  Pose end_pose() const { return end_pose_; }
+  Pose end_pose() const { return modified_goal_pose_.goal_pose; }
+  GoalCandidate modified_goal_pose() const { return modified_goal_pose_; }
 
   std::vector<PathWithLaneId> & partial_paths() { return partial_paths_; }
   const std::vector<PathWithLaneId> & partial_paths() const { return partial_paths_; }
@@ -86,19 +88,18 @@ struct PullOverPath
 
 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 PullOverPlannerType & type, const size_t id, const Pose & start_pose,
+    const GoalCandidate & modified_goal_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_;
+  GoalCandidate modified_goal_pose_;
   size_t id_;
   Pose start_pose_;
-  Pose end_pose_;
 
   std::vector<PathWithLaneId> partial_paths_;
   PathWithLaneId full_path_;
@@ -126,8 +127,9 @@ class PullOverPlannerBase
 
   virtual PullOverPlannerType getPlannerType() const = 0;
   virtual std::optional<PullOverPath> plan(
-    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;
+    const GoalCandidate & modified_goal_pose, const size_t id,
+    const std::shared_ptr<const PlannerData> planner_data,
+    const BehaviorModuleOutput & previous_module_output) = 0;
 
 protected:
   const autoware::vehicle_info_utils::VehicleInfo vehicle_info_;

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,8 +36,9 @@ class ShiftPullOver : public PullOverPlannerBase
     const LaneDepartureChecker & lane_departure_checker);
   PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; };
   std::optional<PullOverPath> plan(
-    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;
+    const GoalCandidate & modified_goal_pose, const size_t id,
+    const std::shared_ptr<const PlannerData> planner_data,
+    const BehaviorModuleOutput & previous_module_output) override;
 
 protected:
   PathWithLaneId generateReferencePath(
@@ -46,10 +47,10 @@ class ShiftPullOver : public PullOverPlannerBase
   std::optional<PathWithLaneId> cropPrevModulePath(
     const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const;
   std::optional<PullOverPath> generatePullOverPath(
-    const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
+    const GoalCandidate & goal_candidate, 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;
+    const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const;
   static double calcBeforeShiftedArcLength(
     const PathWithLaneId & path, const double after_shifted_arc_length, const double dr);
   static std::vector<double> splineTwoPoints(

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -303,8 +303,7 @@ void GoalPlannerModule::onTimer()
                                     const std::shared_ptr<PullOverPlannerBase> & planner,
                                     const GoalCandidate & goal_candidate) {
     const auto pull_over_path = planner->plan(
-      goal_candidate.id, path_candidates.size(), local_planner_data, previous_module_output,
-      goal_candidate.goal_pose);
+      goal_candidate, path_candidates.size(), local_planner_data, previous_module_output);
     if (pull_over_path) {
       // calculate absolute maximum curvature of parking path(start pose to end pose) for path
       // priority
@@ -803,9 +802,8 @@ bool GoalPlannerModule::planFreespacePath(
       continue;
     }
     const auto freespace_path = freespace_planner_->plan(
-      goal_candidate.id, 0, planner_data,
-      BehaviorModuleOutput{},  // NOTE: not used so passing {} is OK
-      goal_candidate.goal_pose);
+      goal_candidate, 0, planner_data, BehaviorModuleOutput{}  // NOTE: not used so passing {} is OK
+    );
     if (!freespace_path) {
       continue;
     }
@@ -1824,7 +1822,7 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData &
     return ignore_signal_.value() == id;
   };
 
-  const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) {
+  const auto update_ignore_signal = [](const lanelet::Id & id, const bool is_ignore) {
     return is_ignore ? std::make_optional(id) : std::nullopt;
   };
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp

@@ -57,8 +57,9 @@ FreespacePullOver::FreespacePullOver(
 }
 
 std::optional<PullOverPath> FreespacePullOver::plan(
-  const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
-  [[maybe_unused]] const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose)
+  const GoalCandidate & modified_goal_pose, const size_t id,
+  const std::shared_ptr<const PlannerData> planner_data,
+  [[maybe_unused]] const BehaviorModuleOutput & previous_module_output)
 {
   const Pose & current_pose = planner_data->self_odometry->pose.pose;
 
@@ -67,6 +68,7 @@ std::optional<PullOverPath> FreespacePullOver::plan(
   // offset goal pose to make straight path near goal for improving parking precision
   // todo: support straight path when using back
   constexpr double straight_distance = 1.0;
+  const auto & goal_pose = modified_goal_pose.goal_pose;
   const Pose end_pose =
     use_back_ ? goal_pose
               : autoware::universe_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0);
@@ -146,7 +148,7 @@ std::optional<PullOverPath> FreespacePullOver::plan(
   }
 
   auto pull_over_path_opt = PullOverPath::create(
-    getPlannerType(), goal_id, id, partial_paths, current_pose, goal_pose,
+    getPlannerType(), id, partial_paths, current_pose, modified_goal_pose,
     pairs_terminal_velocity_and_accel);
   if (!pull_over_path_opt) {
     return {};

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp

@@ -38,11 +38,13 @@ GeometricPullOver::GeometricPullOver(
 }
 
 std::optional<PullOverPath> GeometricPullOver::plan(
-  const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
-  [[maybe_unused]] const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose)
+  const GoalCandidate & modified_goal_pose, const size_t id,
+  const std::shared_ptr<const PlannerData> planner_data,
+  [[maybe_unused]] const BehaviorModuleOutput & previous_module_output)
 {
   const auto & route_handler = planner_data->route_handler;
 
+  const auto & goal_pose = modified_goal_pose.goal_pose;
   // prepare road nad shoulder lanes
   const auto road_lanes = utils::getExtendedCurrentLanes(
     planner_data, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length,
@@ -73,8 +75,8 @@ std::optional<PullOverPath> GeometricPullOver::plan(
   if (lane_departure_checker_.checkPathWillLeaveLane(lanes, arc_path)) return {};
 
   auto pull_over_path_opt = PullOverPath::create(
-    getPlannerType(), goal_id, id, planner_.getPaths(), planner_.getStartPose(),
-    planner_.getArcEndPose(), planner_.getPairsTerminalVelocityAndAccel());
+    getPlannerType(), id, planner_.getPaths(), planner_.getStartPose(), modified_goal_pose,
+    planner_.getPairsTerminalVelocityAndAccel());
   if (!pull_over_path_opt) {
     return {};
   }

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp

@@ -18,8 +18,9 @@ namespace autoware::behavior_path_planner
 {
 
 std::optional<PullOverPath> 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 PullOverPlannerType & type, const size_t id,
+  const std::vector<PathWithLaneId> & partial_paths, const Pose & start_pose,
+  const GoalCandidate & modified_goal_pose,
   const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel)
 {
   if (partial_paths.empty()) {
@@ -76,17 +77,16 @@ std::optional<PullOverPath> PullOverPath::create(
     calculateCurvaturesAndMax(parking_path);
 
   return PullOverPath(
-    type, goal_id, id, start_pose, end_pose, partial_paths, full_path, parking_path,
+    type, id, start_pose, modified_goal_pose, partial_paths, full_path, parking_path,
     full_path_curvatures, parking_path_curvatures, full_path_max_curvature,
     parking_path_max_curvature, pairs_terminal_velocity_and_accel);
 }
 
 PullOverPath::PullOverPath(const PullOverPath & other)
 : type_(other.type_),
-  goal_id_(other.goal_id_),
+  modified_goal_pose_(other.modified_goal_pose_),
   id_(other.id_),
   start_pose_(other.start_pose_),
-  end_pose_(other.end_pose_),
   partial_paths_(other.partial_paths_),
   full_path_(other.full_path_),
   parking_path_(other.parking_path_),
@@ -100,18 +100,17 @@ PullOverPath::PullOverPath(const PullOverPath & other)
 }
 
 PullOverPath::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 PullOverPlannerType & type, const size_t id, const Pose & start_pose,
+  const GoalCandidate & modified_goal_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)
 : type_(type),
-  goal_id_(goal_id),
+  modified_goal_pose_(modified_goal_pose),
   id_(id),
   start_pose_(start_pose),
-  end_pose_(end_pose),
   partial_paths_(partial_paths),
   full_path_(full_path),
   parking_path_(parking_path),

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp

@@ -35,8 +35,9 @@ ShiftPullOver::ShiftPullOver(
 {
 }
 std::optional<PullOverPath> ShiftPullOver::plan(
-  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)
+  const GoalCandidate & modified_goal_pose, const size_t id,
+  const std::shared_ptr<const PlannerData> planner_data,
+  const BehaviorModuleOutput & previous_module_output)
 {
   const auto & route_handler = planner_data->route_handler;
   const double min_jerk = parameters_.minimum_lateral_jerk;
@@ -59,7 +60,7 @@ std::optional<PullOverPath> ShiftPullOver::plan(
   // find safe one from paths with different jerk
   for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) {
     const auto pull_over_path = generatePullOverPath(
-      goal_id, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, goal_pose,
+      modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes,
       lateral_jerk);
     if (!pull_over_path) continue;
     return *pull_over_path;
@@ -127,14 +128,16 @@ std::optional<PathWithLaneId> ShiftPullOver::cropPrevModulePath(
 }
 
 std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
-  const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
+  const GoalCandidate & goal_candidate, 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
+  const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const
 {
   const double pull_over_velocity = parameters_.pull_over_velocity;
   const double after_shift_straight_distance = parameters_.after_shift_straight_distance;
 
+  const auto & goal_pose = goal_candidate.goal_pose;
+
   // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy
   const Pose shift_end_pose =
     autoware::universe_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0);
@@ -256,8 +259,8 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
 
   // set pull over path
   auto pull_over_path_opt = PullOverPath::create(
-    getPlannerType(), goal_id, id, {shifted_path.path}, path_shifter.getShiftLines().front().start,
-    path_shifter.getShiftLines().front().end, {std::make_pair(pull_over_velocity, 0)});
+    getPlannerType(), id, {shifted_path.path}, path_shifter.getShiftLines().front().start,
+    goal_candidate, {std::make_pair(pull_over_velocity, 0)});
 
   if (!pull_over_path_opt) {
     return {};