diff --git a/modules/map/relative_map/navigation_lane.cc b/modules/map/relative_map/navigation_lane.cc index 3e1315376f648a5edce6b0ff15dd69fc6312cf19..3277958025af953b72f51ab7547fd6bb4ed39370 100644 --- a/modules/map/relative_map/navigation_lane.cc +++ b/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,