fix(behavior_path_planner): insert stop point in root reference path (a…

…utowarefoundation#653) * fix(behavior_path_planner): insert stop point in root reference path Signed-off-by: Fumiya Watanabe <[email protected]> * fix(behavior_path_planner): fix insert stop point calculation Signed-off-by: Fumiya Watanabe <[email protected]> --------- Signed-off-by: Fumiya Watanabe <[email protected]>
soblin · Jul 10, 2023 · 9350e87 · 9350e87
1 parent 8444898
commit 9350e87
Show file tree

Hide file tree

Showing 5 changed files with 16 additions and 18 deletions.
diff --git a/...havior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/...havior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp
@@ -125,8 +125,6 @@ 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_; }

diff --git a/...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
@@ -55,8 +55,6 @@ class NormalLaneChange : public LaneChangeBase
 
   void extendOutputDrivableArea(BehaviorModuleOutput & output) override;
 
-  void insertStopPoint(PathWithLaneId & path) override;
-
   PathWithLaneId getReferencePath() const override;
 
   void resetParameters() override;

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
@@ -237,7 +237,6 @@ 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_);

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -170,19 +170,6 @@ 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_);

diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp
@@ -2518,6 +2518,22 @@ 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.