fix(goal_planner): fix usage of last_previous_module_output (autoware…

…foundation#9811)

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp

@@ -288,7 +288,7 @@ bool hasEnoughDistance(
 std::vector<PullOverPath> selectPullOverPaths(
   const std::vector<PullOverPath> & pull_over_path_candidates,
   const GoalCandidates & goal_candidates, const std::shared_ptr<const PlannerData> planner_data,
-  const GoalPlannerParameters & parameters, const BehaviorModuleOutput & previous_module_output)
+  const GoalPlannerParameters & parameters, const BehaviorModuleOutput & upstream_module_output)
 {
   using autoware::behavior_path_planner::utils::getExtendedCurrentLanesFromPath;
   using autoware::motion_utils::calcSignedArcLength;
@@ -313,15 +313,15 @@ std::vector<PullOverPath> selectPullOverPaths(
   }
 
   const double prev_path_front_to_goal_dist = calcSignedArcLength(
-    previous_module_output.path.points,
-    previous_module_output.path.points.front().point.pose.position, goal_pose.position);
+    upstream_module_output.path.points,
+    upstream_module_output.path.points.front().point.pose.position, goal_pose.position);
   const auto & long_tail_reference_path = [&]() {
     if (prev_path_front_to_goal_dist > backward_length) {
-      return previous_module_output.path;
+      return upstream_module_output.path;
     }
     // get road lanes which is at least backward_length[m] behind the goal
     const auto road_lanes = getExtendedCurrentLanesFromPath(
-      previous_module_output.path, planner_data, backward_length, 0.0, false);
+      upstream_module_output.path, planner_data, backward_length, 0.0, false);
     const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length;
     return planner_data->route_handler->getCenterLinePath(
       road_lanes, std::max(0.0, goal_pose_length - backward_length),

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp

@@ -287,7 +287,7 @@ bool hasEnoughDistance(
 std::vector<PullOverPath> selectPullOverPaths(
   const std::vector<PullOverPath> & pull_over_path_candidates,
   const GoalCandidates & goal_candidates, const std::shared_ptr<const PlannerData> planner_data,
-  const GoalPlannerParameters & parameters, const BehaviorModuleOutput & previous_module_output)
+  const GoalPlannerParameters & parameters, const BehaviorModuleOutput & upstream_module_output)
 {
   using autoware::behavior_path_planner::utils::getExtendedCurrentLanesFromPath;
   using autoware::motion_utils::calcSignedArcLength;
@@ -312,15 +312,15 @@ std::vector<PullOverPath> selectPullOverPaths(
   }
 
   const double prev_path_front_to_goal_dist = calcSignedArcLength(
-    previous_module_output.path.points,
-    previous_module_output.path.points.front().point.pose.position, goal_pose.position);
+    upstream_module_output.path.points,
+    upstream_module_output.path.points.front().point.pose.position, goal_pose.position);
   const auto & long_tail_reference_path = [&]() {
     if (prev_path_front_to_goal_dist > backward_length) {
-      return previous_module_output.path;
+      return upstream_module_output.path;
     }
     // get road lanes which is at least backward_length[m] behind the goal
     const auto road_lanes = getExtendedCurrentLanesFromPath(
-      previous_module_output.path, planner_data, backward_length, 0.0, false);
+      upstream_module_output.path, planner_data, backward_length, 0.0, false);
     const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length;
     return planner_data->route_handler->getCenterLinePath(
       road_lanes, std::max(0.0, goal_pose_length - backward_length),

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

@@ -38,15 +38,15 @@ class FixedGoalPlannerBase
   virtual BehaviorModuleOutput plan(
     const std::shared_ptr<const PlannerData> & planner_data) const = 0;
 
-  void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output)
+  void setPreviousModuleOutput(const BehaviorModuleOutput & upstream_module_output)
   {
-    previous_module_output_ = previous_module_output;
+    upstream_module_output_ = upstream_module_output;
   }
 
-  BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; }
+  BehaviorModuleOutput getPreviousModuleOutput() const { return upstream_module_output_; }
 
 protected:
-  BehaviorModuleOutput previous_module_output_;
+  BehaviorModuleOutput upstream_module_output_;
 };
 }  // namespace autoware::behavior_path_planner
 

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

@@ -133,12 +133,12 @@ bool isOnModifiedGoal(
   const GoalPlannerParameters & parameters);
 
 bool hasPreviousModulePathShapeChanged(
-  const BehaviorModuleOutput & previous_module_output,
-  const BehaviorModuleOutput & last_previous_module_output);
+  const BehaviorModuleOutput & upstream_module_output,
+  const BehaviorModuleOutput & last_upstream_module_output);
 bool hasDeviatedFromLastPreviousModulePath(
-  const PlannerData & planner_data, const BehaviorModuleOutput & last_previous_module_output);
+  const PlannerData & planner_data, const BehaviorModuleOutput & last_upstream_module_output);
 bool hasDeviatedFromCurrentPreviousModulePath(
-  const PlannerData & planner_data, const BehaviorModuleOutput & previous_module_output);
+  const PlannerData & planner_data, const BehaviorModuleOutput & upstream_module_output);
 
 bool needPathUpdate(
   const Pose & current_pose, const double path_update_duration, const rclcpp::Time & now,
@@ -189,13 +189,18 @@ class LaneParkingPlanner
   void onTimer();
 
 private:
+  const GoalPlannerParameters parameters_;
   std::mutex & mutex_;
   const std::optional<LaneParkingRequest> & request_;
   LaneParkingResponse & response_;
   std::atomic<bool> & is_lane_parking_cb_running_;
   rclcpp::Logger logger_;
 
   std::vector<std::shared_ptr<PullOverPlannerBase>> pull_over_planners_;
+  BehaviorModuleOutput
+    original_upstream_module_output_;  //<! upstream_module_output used for generating last
+                                       // pull_over_path_candidates(only updated when new candidates
+                                       // are generated)
 };
 
 class FreespaceParkingPlanner

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

@@ -36,11 +36,11 @@ class BezierPullOver : public PullOverPlannerBase
   std::optional<PullOverPath> plan(
     const GoalCandidate & modified_goal_pose, const size_t id,
     const std::shared_ptr<const PlannerData> planner_data,
-    const BehaviorModuleOutput & previous_module_output) override;
+    const BehaviorModuleOutput & upstream_module_output) override;
   std::vector<PullOverPath> plans(
     const GoalCandidate & modified_goal_pose, const size_t id,
     const std::shared_ptr<const PlannerData> planner_data,
-    const BehaviorModuleOutput & previous_module_output);
+    const BehaviorModuleOutput & upstream_module_output);
 
 private:
   const LaneDepartureChecker lane_departure_checker_;
@@ -50,7 +50,7 @@ class BezierPullOver : public PullOverPlannerBase
   std::vector<PullOverPath> generateBezierPath(
     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 BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes,
     const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const;
 
   PathWithLaneId generateReferencePath(

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

@@ -41,7 +41,7 @@ class FreespacePullOver : public PullOverPlannerBase
   std::optional<PullOverPath> plan(
     const GoalCandidate & modified_goal_pose, const size_t id,
     const std::shared_ptr<const PlannerData> planner_data,
-    const BehaviorModuleOutput & previous_module_output) override;
+    const BehaviorModuleOutput & upstream_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

@@ -45,7 +45,7 @@ class GeometricPullOver : public PullOverPlannerBase
   std::optional<PullOverPath> plan(
     const GoalCandidate & modified_goal_pose, const size_t id,
     const std::shared_ptr<const PlannerData> planner_data,
-    const BehaviorModuleOutput & previous_module_output) override;
+    const BehaviorModuleOutput & upstream_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

@@ -130,7 +130,7 @@ class PullOverPlannerBase
   virtual std::optional<PullOverPath> plan(
     const GoalCandidate & modified_goal_pose, const size_t id,
     const std::shared_ptr<const PlannerData> planner_data,
-    const BehaviorModuleOutput & previous_module_output) = 0;
+    const BehaviorModuleOutput & upstream_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

@@ -38,7 +38,7 @@ class ShiftPullOver : public PullOverPlannerBase
   std::optional<PullOverPath> plan(
     const GoalCandidate & modified_goal_pose, const size_t id,
     const std::shared_ptr<const PlannerData> planner_data,
-    const BehaviorModuleOutput & previous_module_output) override;
+    const BehaviorModuleOutput & upstream_module_output) override;
 
 protected:
   PathWithLaneId generateReferencePath(
@@ -49,7 +49,7 @@ class ShiftPullOver : public PullOverPlannerBase
   std::optional<PullOverPath> generatePullOverPath(
     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 BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes,
     const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const;
   static double calcBeforeShiftedArcLength(
     const PathWithLaneId & path, const double after_shifted_arc_length, const double dr);

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

@@ -32,44 +32,35 @@ class LaneParkingRequest
 {
 public:
   LaneParkingRequest(
-    const GoalPlannerParameters & parameters,
     const autoware::universe_utils::LinearRing2d & vehicle_footprint,
-    const GoalCandidates & goal_candidates, const BehaviorModuleOutput & previous_module_output)
-  : parameters_(parameters),
-    vehicle_footprint_(vehicle_footprint),
+    const GoalCandidates & goal_candidates, const BehaviorModuleOutput & upstream_module_output)
+  : vehicle_footprint_(vehicle_footprint),
     goal_candidates_(goal_candidates),
-    previous_module_output_(previous_module_output),
-    last_previous_module_output_(previous_module_output)
+    upstream_module_output_(upstream_module_output)
   {
   }
 
   void update(
     const PlannerData & planner_data, const ModuleStatus & current_status,
-    const BehaviorModuleOutput & previous_module_output,
+    const BehaviorModuleOutput & upstream_module_output,
     const std::optional<PullOverPath> & pull_over_path, const PathDecisionState & prev_data);
 
-  const GoalPlannerParameters parameters_;
   const autoware::universe_utils::LinearRing2d vehicle_footprint_;
   const GoalCandidates goal_candidates_;
 
   const std::shared_ptr<PlannerData> & get_planner_data() const { return planner_data_; }
   const ModuleStatus & get_current_status() const { return current_status_; }
-  const BehaviorModuleOutput & get_previous_module_output() const
-  {
-    return previous_module_output_;
-  }
-  const BehaviorModuleOutput & get_last_previous_module_output() const
+  const BehaviorModuleOutput & get_upstream_module_output() const
   {
-    return last_previous_module_output_;
+    return upstream_module_output_;
   }
   const std::optional<PullOverPath> & get_pull_over_path() const { return pull_over_path_; }
   const PathDecisionState & get_prev_data() const { return prev_data_; }
 
 private:
   std::shared_ptr<PlannerData> planner_data_;
   ModuleStatus current_status_;
-  BehaviorModuleOutput previous_module_output_;
-  BehaviorModuleOutput last_previous_module_output_;
+  BehaviorModuleOutput upstream_module_output_;
   std::optional<PullOverPath> pull_over_path_;  //<! pull over path selected by main thread
   PathDecisionState prev_data_;
 };