planning: fix effective width calculation bug

modules/planning/reference_line/reference_line_provider.cc

@@ -519,26 +519,31 @@ AnchorPoint ReferenceLineProvider::GetAnchorPoint(
     const ReferenceLine &reference_line, double s) const {
   AnchorPoint anchor;
   anchor.longitudinal_bound = smoother_config_.longitudinal_boundary_bound();
-  const auto adc_half_width =
-      VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0;
   auto ref_point = reference_line.GetReferencePoint(s);
+  if (ref_point.lane_waypoints().empty()) {
+    anchor.path_point = ref_point.ToPathPoint(s);
+    anchor.lateral_bound = smoother_config_.lateral_boundary_bound();
+    return anchor;
+  }
+  const auto adc_width =
+      VehicleConfigHelper::GetConfig().vehicle_param().width();
   const Vec2d left_vec =
       Vec2d::CreateUnitVec2d(ref_point.heading() + M_PI / 2.0);
   const Vec2d right_vec =
       Vec2d::CreateUnitVec2d(ref_point.heading() - M_PI / 2.0);
+  auto waypoint = ref_point.lane_waypoints().front();
 
   Vec2d all_shifts(0.0, 0.0);
 
   // shift to center
   double left_width = 0.0;
   double right_width = 0.0;
-  reference_line.GetLaneWidth(s, &left_width, &right_width);
+  waypoint.lane->GetWidth(waypoint.s, &left_width, &right_width);
   auto shift_to_center = (left_width - right_width) / 2.0 * left_vec;
   all_shifts += shift_to_center;
 
-  // avoid curb boundary
+  // shift away from curb boundary
   if (!ref_point.lane_waypoints().empty()) {
-    const LaneWaypoint &waypoint = ref_point.lane_waypoints()[0];
     auto left_type = hdmap::LeftBoundaryType(waypoint);
     if (left_type == hdmap::LaneBoundaryType::CURB) {
       all_shifts += smoother_config_.curb_shift() * right_vec;
@@ -550,10 +555,12 @@ AnchorPoint ReferenceLineProvider::GetAnchorPoint(
   }
 
   ref_point += all_shifts;
+  auto left_shifted_width = left_width + all_shifts.InnerProd(left_vec);
+  auto right_shifted_width = right_width + all_shifts.InnerProd(right_vec);
   anchor.path_point = ref_point.ToPathPoint(s);
-  double effective_width = (left_width + right_width) / 2.0 - adc_half_width -
-                           FLAGS_reference_line_lateral_buffer -
-                           all_shifts.Length();
+  double effective_width = std::min(left_shifted_width, right_shifted_width) -
+                           adc_width / 2.0 -
+                           FLAGS_reference_line_lateral_buffer;
   anchor.lateral_bound =
       std::max(smoother_config_.lateral_boundary_bound(), effective_width);
   return anchor;