fix

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

planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp

@@ -51,6 +51,9 @@ size_t getIdxByArclength(
 void clipPathLength(
   PathWithLaneId & path, const size_t target_idx, const double forward, const double backward);
 
+void clipPathLength(
+  PathWithLaneId & path, const size_t target_idx, const BehaviorPathPlannerParameters & params);
+
 std::pair<TurnIndicatorsCommand, double> getPathTurnSignal(
   const lanelet::ConstLanelets & current_lanes, const ShiftedPath & path,
   const ShiftPoint & shift_point, const Pose & pose, const double & velocity,

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -569,9 +569,7 @@ void BehaviorPathPlannerNode::run()
     clipped_path = modifyPathForSmoothGoalConnection(*path);
   }
   const size_t target_idx = findEgoIndex(clipped_path.points);
-  util::clipPathLength(
-    clipped_path, target_idx, planner_data_->parameters.forward_path_length,
-    planner_data_->parameters.backward_path_length);
+  util::clipPathLength(clipped_path, target_idx, planner_data_->parameters);
 
   if (!clipped_path.points.empty()) {
     path_publisher_->publish(clipped_path);

planning/behavior_path_planner/src/path_utilities.cpp

@@ -177,6 +177,12 @@ void clipPathLength(
   path.points = clipped_points;
 }
 
+void clipPathLength(
+  PathWithLaneId & path, const size_t target_idx, const BehaviorPathPlannerParameters & params)
+{
+  clipPathLength(path, target_idx, params.forward_path_length, params.backward_path_length);
+}
+
 std::pair<TurnIndicatorsCommand, double> getPathTurnSignal(
   const lanelet::ConstLanelets & current_lanes, const ShiftedPath & path,
   const ShiftPoint & shift_point, const Pose & pose, const double & velocity,

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -2062,9 +2062,7 @@ BehaviorModuleOutput AvoidanceModule::plan()
   output.path = std::make_shared<PathWithLaneId>(avoidance_path.path);
 
   const size_t ego_idx = findEgoIndex(output.path->points);
-  util::clipPathLength(
-    *output.path, ego_idx, planner_data_->parameters.forward_path_length,
-    planner_data_->parameters.backward_path_length);
+  util::clipPathLength(*output.path, ego_idx, planner_data_->parameters);
 
   DEBUG_PRINT("exit plan(): set prev output (back().lat = %f)", prev_output_.shift_length.back());
 
@@ -2106,9 +2104,7 @@ CandidateOutput AvoidanceModule::planCandidate() const
   }
 
   const size_t ego_idx = findEgoIndex(shifted_path.path.points);
-  util::clipPathLength(
-    shifted_path.path, ego_idx, planner_data_->parameters.forward_path_length,
-    planner_data_->parameters.backward_path_length);
+  util::clipPathLength(shifted_path.path, ego_idx, planner_data_->parameters);
 
   output.path_candidate = shifted_path.path;
 

planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp

@@ -252,9 +252,13 @@ void PathShifter::updateShiftPointIndices()
 {
   for (auto & p : shift_points_) {
     p.start_idx = motion_utils::findNearestIndex(
-      reference_path_.points, p.start.position);  // TODO(murooka) remove findNearestIndex
+      reference_path_.points,
+      p.start.position);  // TODO(planning/control team) remove findNearestIndex for except
+                          // lane_following to support u-turn & crossing path
     p.end_idx = motion_utils::findNearestIndex(
-      reference_path_.points, p.end.position);  // TODO(murooka) remove findNearestIndex
+      reference_path_.points,
+      p.end.position);  // TODO(planning/control team) remove findNearestIndex for except
+                        // lane_following to support u-turn & crossing path
   }
   is_index_aligned_ = true;
 }

planning/behavior_path_planner/src/utilities.cpp

@@ -26,6 +26,20 @@
 
 namespace drivable_area_utils
 {
+template <class T>
+size_t findNearestSegmentIndex(
+  const std::vector<T> & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold,
+  const double yaw_threshold)
+{
+  const auto nearest_idx =
+    motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold);
+  if (nearest_idx) {
+    return nearest_idx.get();
+  }
+
+  return motion_utils::findNearestSegmentIndex(points, pose.position);
+}
+
 double quantize(const double val, const double resolution)
 {
   return std::round(val / resolution) * resolution;
@@ -157,8 +171,8 @@ std::array<double, 4> getPathScope(
         points, current_pose, max_dist, max_yaw);
 
     // forward lanelet
-    const auto forward_offset_length =
-      motion_utils::calcSignedArcLength(points, current_pose.position, nearest_segment_idx);
+    const auto forward_offset_length = motion_utils::calcSignedArcLength(
+      points, current_pose.position, nearest_segment_idx, nearest_segment_idx);
     double sum_length = std::min(forward_offset_length, 0.0);
     size_t current_lane_idx = nearest_lane_idx;
     auto current_lane = path_lanes.at(current_lane_idx);
@@ -205,8 +219,8 @@ std::array<double, 4> getPathScope(
 
     // backward lanelet
     current_point_idx = nearest_segment_idx + 1;
-    const auto backward_offset_length =
-      motion_utils::calcSignedArcLength(points, nearest_segment_idx + 1, current_pose.position);
+    const auto backward_offset_length = motion_utils::calcSignedArcLength(
+      points, nearest_segment_idx + 1, current_pose.position, nearest_segment_idx);
     sum_length = std::min(backward_offset_length, 0.0);
     current_lane_idx = nearest_lane_idx;
     current_lane = path_lanes.at(current_lane_idx);
@@ -860,7 +874,7 @@ bool setGoal(
     PathPointWithLaneId refined_goal{};
     {  // NOTE: goal does not have valid z, that will be calculated by interpolation here
       const size_t closest_seg_idx =
-        motion_utils::findNearestSegmentIndex(input.points, goal.position);
+        drivable_area_utils::findNearestSegmentIndex(input.points, goal, 3.0, M_PI / 4.0);
       const double closest_to_goal_dist = motion_utils::calcSignedArcLength(
         input.points, closest_seg_idx, goal.position, closest_seg_idx);
       const double seg_dist =
@@ -893,8 +907,8 @@ bool setGoal(
     pre_refined_goal.point.pose.orientation = goal.orientation;
 
     {  // NOTE: interpolate z and velocity of pre_refined_goal
-      const size_t closest_seg_idx =
-        motion_utils::findNearestSegmentIndex(input.points, pre_refined_goal.point.pose.position);
+      const size_t closest_seg_idx = drivable_area_utils::findNearestSegmentIndex(
+        input.points, pre_refined_goal.point.pose, 3.0, M_PI / 4.0);
       const double closest_to_pre_goal_dist = motion_utils::calcSignedArcLength(
         input.points, input.points.at(closest_seg_idx).point.pose.position,
         pre_refined_goal.point.pose.position);
@@ -1736,6 +1750,7 @@ bool checkLaneIsInIntersection(
   return true;
 }
 
+// for lane following
 PathWithLaneId setDecelerationVelocity(
   const RouteHandler & route_handler, const PathWithLaneId & input,
   const lanelet::ConstLanelets & lanelet_sequence, const double lane_change_prepare_duration,
@@ -1796,6 +1811,8 @@ PathWithLaneId setDecelerationVelocity(
   return reference_path;
 }
 
+// TODO(planning/control team) remove calcSignedArcLength using findNearestSegmentIndex inside the
+// function
 PathWithLaneId setDecelerationVelocity(
   const PathWithLaneId & input, const double target_velocity, const Pose target_pose,
   const double buffer, const double deceleration_interval)
@@ -1825,6 +1842,8 @@ PathWithLaneId setDecelerationVelocity(
   return reference_path;
 }
 
+// TODO(planning/control team) remove calcSignedArcLength using findNearestSegmentIndex inside the
+// function
 PathWithLaneId setDecelerationVelocityForTurnSignal(
   const PathWithLaneId & input, const Pose target_pose, const double turn_light_on_threshold_time)
 {