consider rss distance

Signed-off-by: kosuke55 <[email protected]>

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

@@ -256,10 +256,15 @@ void NormalLaneChange::insertStopPoint(
     // If the lane change space is occupied by a stationary obstacle, move the stop line closer.
     // TODO(Horibe): We need to loop this process because the new space could be occupied as well.
     stopping_distance = std::min(
-      distance_to_ego_lane_obj - lane_change_buffer - stop_point_buffer, stopping_distance);
+      distance_to_ego_lane_obj - lane_change_buffer - stop_point_buffer -
+        calcRssDistance(
+          0.0, planner_data_->parameters.minimum_lane_changing_velocity,
+          lane_change_parameters_->rss_params),
+      stopping_distance);
   }
 
   const auto stopping_distance_baselink = stopping_distance - getCommonParam().base_link2front;
+
   if (stopping_distance_baselink > 0.0) {
     const auto stop_point = utils::insertStopPoint(stopping_distance_baselink, path);
     setStopPose(stop_point.point.pose);