fix(behavior_path_planner): fix execution request condition (#657)

* Revert "fix(behavior_path_planner): insert stop point in root reference path (#653)"

This reverts commit 9350e87.

* fix(behavior_path_planner): fix execution request condition

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

---------

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp

@@ -125,6 +125,8 @@ class LaneChangeBase
 
   virtual void updateSpecialData() {}
 
+  virtual void insertStopPoint([[maybe_unused]] PathWithLaneId & path) {}
+
   const LaneChangeStatus & getLaneChangeStatus() const { return status_; }
 
   const LaneChangePaths & getDebugValidPath() const { return debug_valid_path_; }

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp

@@ -55,6 +55,8 @@ class NormalLaneChange : public LaneChangeBase
 
   void extendOutputDrivableArea(BehaviorModuleOutput & output) override;
 
+  void insertStopPoint(PathWithLaneId & path) override;
+
   PathWithLaneId getReferencePath() const override;
 
   void resetParameters() override;

planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp

@@ -237,6 +237,7 @@ BehaviorModuleOutput LaneChangeInterface::plan()
 BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
 {
   *prev_approved_path_ = *getPreviousModuleOutput().path;
+  module_type_->insertStopPoint(*prev_approved_path_);
 
   BehaviorModuleOutput out;
   out.path = std::make_shared<PathWithLaneId>(*prev_approved_path_);
@@ -251,15 +252,17 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
   // change turn signal when the vehicle reaches at the end of the path for waiting lane change
   out.turn_signal_info = getCurrentTurnSignalInfo(*out.path, out.turn_signal_info);
 
+  path_reference_ = getPreviousModuleOutput().reference_path;
+
   if (!module_type_->isValidPath()) {
+    removeRTCStatus();
     path_candidate_ = std::make_shared<PathWithLaneId>();
     return out;
   }
 
   const auto candidate = planCandidate();
   path_candidate_ = std::make_shared<PathWithLaneId>(candidate.path_candidate);
 
-  path_reference_ = getPreviousModuleOutput().reference_path;
   updateRTCStatus(
     candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change);
   is_abort_path_approved_ = false;

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -76,6 +76,7 @@ std::pair<bool, bool> NormalLaneChange::getSafePath(LaneChangePath & safe_path)
     getLaneChangePaths(current_lanes, target_lanes, direction_, &valid_paths);
 
   if (valid_paths.empty()) {
+    safe_path.reference_lanelets = current_lanes;
     return {false, false};
   }
 
@@ -99,16 +100,7 @@ bool NormalLaneChange::isLaneChangeRequired() const
 
   const auto target_lanes = getLaneChangeLanes(current_lanes, direction_);
 
-  if (target_lanes.empty()) {
-    return false;
-  }
-
-  // find candidate paths
-  LaneChangePaths valid_paths{};
-  [[maybe_unused]] const auto found_safe_path =
-    getLaneChangePaths(current_lanes, target_lanes, direction_, &valid_paths, false);
-
-  return !valid_paths.empty();
+  return !target_lanes.empty();
 }
 
 LaneChangePath NormalLaneChange::getLaneChangePath() const
@@ -170,6 +162,19 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output)
     *output.path, expanded_lanes, false, common_parameters.vehicle_length, planner_data_);
 }
 
+void NormalLaneChange::insertStopPoint(PathWithLaneId & path)
+{
+  const auto shift_intervals = getRouteHandler()->getLateralIntervalsToPreferredLane(
+    status_.lane_change_path.reference_lanelets.back());
+  const double lane_change_buffer =
+    utils::calcMinimumLaneChangeLength(getCommonParam(), shift_intervals, 0.0);
+  constexpr double stop_point_buffer{1.0};
+  const auto stopping_distance = std::max(
+    motion_utils::calcArcLength(path.points) - lane_change_buffer - stop_point_buffer, 0.0);
+
+  const auto stop_point = utils::insertStopPoint(stopping_distance, path);
+}
+
 PathWithLaneId NormalLaneChange::getReferencePath() const
 {
   return utils::getCenterLinePathFromRootLanelet(status_.lane_change_lanes.front(), planner_data_);

planning/behavior_path_planner/src/utils/utils.cpp

@@ -2518,22 +2518,6 @@ BehaviorModuleOutput getReferencePath(
     *route_handler, current_lanes_with_backward_margin, current_pose, backward_length,
     p.forward_path_length, p);
 
-  // Set stop point for lane change
-  if (route_handler->getNumLaneToPreferredLane(current_lanes_with_backward_margin.back()) != 0) {
-    const auto shift_intervals =
-      route_handler->getLateralIntervalsToPreferredLane(current_lanes_with_backward_margin.back());
-    const double lane_change_buffer = utils::calcMinimumLaneChangeLength(p, shift_intervals, 0.0);
-    const double length_to_dead_end = utils::getDistanceToEndOfLane(
-      reference_path.points.back().point.pose, current_lanes_with_backward_margin);
-    constexpr double stop_point_buffer{1.0};
-    const auto stopping_distance = std::max(
-      motion_utils::calcArcLength(reference_path.points) -
-        (lane_change_buffer + stop_point_buffer - length_to_dead_end),
-      0.0);
-
-    const auto stop_point = utils::insertStopPoint(stopping_distance, reference_path);
-  }
-
   // clip backward length
   // NOTE: In order to keep backward_path_length at least, resampling interval is added to the
   // backward.