fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

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

@@ -292,10 +292,11 @@ bool selectSafePath(
 {
   for (const auto & path : paths) {
     const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-      path.path.points, current_pose, common_parameters.ego_nearestdist_threshold, common_parameters.ego_nearest_yaw_threshold);
+      path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold,
+      common_parameters.ego_nearest_yaw_threshold);
     if (isLaneChangePathSafe(
-          path.path, current_lanes, target_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist,
-          common_parameters, ros_parameters, true, path.acceleration)) {
+          path.path, current_lanes, target_lanes, dynamic_objects, current_pose, current_seg_idx,
+          current_twist, common_parameters, ros_parameters, true, path.acceleration)) {
       *selected_path = path;
       return true;
     }
@@ -345,8 +346,8 @@ bool isLaneChangePathSafe(
   const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes,
   const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
-  const size_t current_seg_idx,
-  const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters,
+  const size_t current_seg_idx, const Twist & current_twist,
+  const BehaviorPathPlannerParameters & common_parameters,
   const LaneChangeParameters & lane_change_parameters, const bool use_buffer,
   const double acceleration)
 {
@@ -371,8 +372,8 @@ bool isLaneChangePathSafe(
   const double target_lane_check_end_time = lane_change_prepare_duration + lane_changing_duration;
   constexpr double ego_predicted_path_min_speed{1.0};
   const auto vehicle_predicted_path = util::convertToPredictedPath(
-    path, current_twist, current_pose, target_lane_check_end_time, time_resolution, acceleration,
-    ego_predicted_path_min_speed);
+    path, current_twist, current_pose, current_seg_idx, target_lane_check_end_time, time_resolution,
+    acceleration, ego_predicted_path_min_speed);
 
   const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose);
 

planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp

@@ -111,7 +111,9 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
     p.forward_path_length, p);
 
   // clip backward length
-  util::clipPathLength(reference_path, current_pose, p.forward_path_length, p.backward_path_length);
+  const size_t current_seg_idx = findEgoSegmentIndex(reference_path.points);
+  util::clipPathLength(
+    reference_path, current_seg_idx, p.forward_path_length, p.backward_path_length);
 
   {
     double optional_lengths{0.0};

planning/behavior_path_planner/src/utilities.cpp

@@ -859,7 +859,7 @@ bool setGoal(
     // NOTE: goal does not have valid z, that will be calculated by interpolation here
     PathPointWithLaneId refined_goal{};
     const size_t closest_seg_idx_for_goal =
-      motion_utils::findNearestSegmentIndex(input.points, goal.position);
+      drivable_area_utils::findNearestSegmentIndex(input.points, goal, 3.0, M_PI_4);
     refined_goal.point.pose = goal;
     refined_goal.point.pose.position.z =
       calcInterpolatedZ(input, goal.position, closest_seg_idx_for_goal);
@@ -871,8 +871,8 @@ bool setGoal(
     constexpr double goal_to_pre_goal_distance = -1.0;
     pre_refined_goal.point.pose =
       tier4_autoware_utils::calcOffsetPose(goal, goal_to_pre_goal_distance, 0.0, 0.0);
-    const size_t closest_seg_idx_for_pre_goal =
-      motion_utils::findNearestSegmentIndex(input.points, pre_refined_goal.point.pose.position);
+    const size_t closest_seg_idx_for_pre_goal = drivable_area_utils::findNearestSegmentIndex(
+      input.points, pre_refined_goal.point.pose, 3.0, M_PI_4);
     pre_refined_goal.point.pose.position.z =
       calcInterpolatedZ(input, pre_refined_goal.point.pose.position, closest_seg_idx_for_pre_goal);
     pre_refined_goal.point.longitudinal_velocity_mps =