Relativemap: fixed a bug which the lane width may not be calculated correctly.

modules/map/relative_map/navigation_lane.cc

@@ -38,7 +38,6 @@ using apollo::hdmap::Lane;
 using apollo::common::util::operator+;
 using apollo::common::util::IsFloatEqual;
 
-
 NavigationLane::NavigationLane(const NavigationLaneConfig &config)
     : config_(config) {}
 
@@ -147,6 +146,17 @@ bool NavigationLane::GeneratePath() {
                                                     : default_left_width_;
     double right_width = perceived_right_width_ > 0.0 ? perceived_right_width_
                                                       : default_right_width_;
+    if (!IsFloatEqual(left_width, default_left_width_) &&
+        !IsFloatEqual(right_width, default_right_width_)) {
+      left_width = left_width > default_left_width_ ? left_width - min_d
+                                                    : left_width + min_d;
+      right_width = right_width > default_right_width_ ? right_width - min_d
+                                                       : right_width + min_d;
+    }
+
+    ADEBUG << "The left width of current lane is: " << left_width
+           << " and the right width of current lane is: " << right_width;
+
     std::get<1>(current_navi_path_tuple_) = left_width;
     std::get<2>(current_navi_path_tuple_) = right_width;
     auto curr_navi_path_iter = std::find_if(
@@ -613,13 +623,16 @@ void NavigationLane::ConvertLaneMarkerToPath(
     point->set_dkappa((k2 - k1) / 0.0002);
   }
 
-  perceived_left_width_ = (std::fabs(left_lane.c0_position()) +
-                           std::fabs(right_lane.c0_position())) /
-                          2.0;
-  perceived_left_width_ =
-      common::math::Clamp(perceived_left_width_, config_.min_lane_half_width(),
-                          config_.max_lane_half_width());
-  perceived_right_width_ = perceived_left_width_;
+  perceived_left_width_ = std::fabs(left_lane.c0_position());
+  perceived_right_width_ = std::fabs(right_lane.c0_position());
+  // If the perceived lane width is incorrect, use the default lane width
+  // directly.
+  double perceived_lane_width = perceived_left_width_ + perceived_right_width_;
+  if (perceived_lane_width < 2.0 * config_.min_lane_half_width() ||
+      perceived_lane_width > 2.0 * config_.max_lane_half_width()) {
+    perceived_left_width_ = default_left_width_;
+    perceived_right_width_ = default_right_width_;
+  }
 }
 
 bool NavigationLane::CreateMap(const MapGenerationParam &map_config,