fix(lane_change): prevent empty path when rerouting (autowarefoundation#7717)

fix(lane_change): prevent empty path when routing

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Author: kosuke55

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -467,8 +467,16 @@ void NormalLaneChange::insertStopPoint(
 
 PathWithLaneId NormalLaneChange::getReferencePath() const
 {
-  return utils::getCenterLinePathFromLanelet(
-    status_.lane_change_path.info.target_lanes.front(), planner_data_);
+  lanelet::ConstLanelet closest_lanelet;
+  if (!lanelet::utils::query::getClosestLanelet(
+        status_.lane_change_path.info.target_lanes, getEgoPose(), &closest_lanelet)) {
+    return prev_module_output_.reference_path;
+  }
+  const auto reference_path = utils::getCenterLinePathFromLanelet(closest_lanelet, planner_data_);
+  if (reference_path.points.empty()) {
+    return prev_module_output_.reference_path;
+  }
+  return reference_path;
 }
 
 std::optional<PathWithLaneId> NormalLaneChange::extendPath()