apollo::planning::NudgeCalculation类 参考

#include <nudge_calculation.h>

apollo::planning::NudgeCalculation 的协作图:

Public 成员函数
	NudgeCalculation (const ObstacleNudgeDeciderConfig &cross_line_nudge_config)
void	BuildNudgeDecisionWithObs (Frame *const current_frame, const Frame *last_frame, ReferenceLineInfo *const reference_line_info)
void	set_in_nudge_state (bool state)
void	BuildStaticTrackingObs (ReferenceLineInfo *const reference_line_info, const Obstacle &obstacle, NudgeInfo *const nudge_info)
bool	CalcObsRemainSpace (ReferenceLineInfo *const reference_line_info, const Obstacle &obstacle, RemainNudgeSpace *remain_space)
bool	MatchTrackingObsInfo (const Obstacle &obstacle, NudgeInfo *const nudge_info, bool is_passable, RemainNudgeSpace remain_nudge_space, bool is_all_on_lane)
bool	MergeObsPolygon (ReferenceLineInfo *const reference_line_info, NudgeInfo *const nudge_info)
void	EraseObsInNudgeinfo (NudgeInfo *const nudge_info, std::string id)
bool	IsProbEnoughToNudge (NudgeInfo *const nudge_info, bool is_merge_obs=false)
void	CalcNudgeExtraSpace (ReferenceLineInfo *const reference_line_info, NudgeInfo *const nudge_info)

详细描述

在文件 nudge_calculation.h 第 48 行定义.

构造及析构函数说明

NudgeCalculation()

apollo::planning::NudgeCalculation::NudgeCalculation ( const ObstacleNudgeDeciderConfig &  cross_line_nudge_config )

explicit

在文件 nudge_calculation.cc 第 37 行定义.

                                                                                 {
    config_.CopyFrom(nudge_config);
    const auto& vehicle_config = common::VehicleConfigHelper::Instance()->GetConfig();
    veh_config_.width = vehicle_config.vehicle_param().width();
    veh_config_.length = vehicle_config.vehicle_param().length();
    veh_config_.front_edge_to_center = vehicle_config.vehicle_param().front_edge_to_center();
}

成员函数说明

BuildNudgeDecisionWithObs()

void apollo::planning::NudgeCalculation::BuildNudgeDecisionWithObs	(	Frame *const	current_frame,
		const Frame *	last_frame,
		ReferenceLineInfo *const	reference_line_info
	)

在文件 nudge_calculation.cc 第 45 行定义.

                                                      {
    auto* nudge_info = ParkDataCenter::Instance()->mutable_current_nudge_info(reference_line_info->key());
    nudge_info->mutable_update_ids()->clear();
    nudge_info->mutable_block_sl_polygons()->clear();
    adc_boundary_ = reference_line_info->AdcSlBoundary();
    auto* tracking_nudge_obs_info = nudge_info->mutable_tracking_nudge_obs_info();
 
    double ego_v = std::abs(reference_line_info->vehicle_state().linear_velocity());
    check_forward_dis_ = common::math::Clamp(ego_v, 0.0, 8.0) / 8.0
                    * (config_.max_forward_check_dis() - config_.min_forward_check_dis())
            + config_.min_forward_check_dis();
 
    dis2end_ = reference_line_info->SDistanceToDestination();
 
    if (nullptr != last_frame && nullptr != last_frame->DriveReferenceLineInfo()
        && ParkDataCenter::Instance()->GetLastFrameNudgeTrackInfo(
                last_frame->SequenceNum(), last_frame->DriveReferenceLineInfo()->key(), tracking_nudge_obs_info)) {
        AINFO << "NudgeCalculation: update tracking_nudge_obs_info from last frame";
    }
 
    for (auto& iter : nudge_info->tracking_nudge_obs_info()) {
        AINFO << "last frame tracking info: " << iter.first << ", P(nudge): " << iter.second.nudge_probability;
    }
 
    for (const auto* obstacle : reference_line_info->path_decision()->obstacles().Items()) {
        const std::string& obstacle_id = obstacle->Id();
        const std::string obstacle_type_name = PerceptionObstacle_Type_Name(obstacle->Perception().type());
 
        AINFO << "NudgeCalculation: check obstacle id [ " << obstacle_id << " ], type [ " << obstacle_type_name << " ]";
 
        if (!IsWithinNudgeScopeObstacle(reference_line_info, *obstacle)) {
            AINFO << "NudgeCalculation: Not IsWithinNudgeScopeObstacle, Skip";
            continue;
        }
 
        // update_static_tracking_obs
        if (obstacle->IsStatic()) {
            BuildStaticTrackingObs(reference_line_info, *obstacle, nudge_info);
        } else {
            // To Do
            ADEBUG << "NudgeCalculation: obstacle [ " << obstacle->Id() << "] is dynamic";
        }
    }
 
    // clear tracking obs which is not updated
    for (auto iter = tracking_nudge_obs_info->begin(); iter != tracking_nudge_obs_info->end();) {
        auto obs_id_iter = nudge_info->update_ids().find(iter->first);
        if (obs_id_iter != nudge_info->update_ids().end()) {
            ++iter;
            continue;
        }
 
        double current_time = ::apollo::cyber::Clock::NowInSeconds();
        if (current_time - iter->second.last_track_time > config_.obs_clean_time_out_threshold()) {
            AINFO << "NudgeCalculation: erase tracking_nudge_obs_info: obs [ " << iter->first << " ]";
            iter = tracking_nudge_obs_info->erase(iter);
            continue;
        }
        ++iter;
    }
 
    // update nudge
    for (const auto& obs_id : nudge_info->update_ids()) {
        auto track_obs_iter = tracking_nudge_obs_info->find(obs_id);
        if (track_obs_iter != tracking_nudge_obs_info->end()) {
            nudge_info->mutable_block_sl_polygons()->emplace_back(track_obs_iter->second.sl_boundary, obs_id);
            // track_obs_iter->second.PrintPolygonInfo(obs_id);
            AINFO << "update nudge [ " << obs_id << " ], "
                  << "nudge_probability: " << track_obs_iter->second.nudge_probability;
        }
    }
 
    // sort sl polygons
    nudge_info->SortBlockSLPolygons();
 
    // merge first block obs width nearby obstacle
    bool is_merge_obs_polygon = MergeObsPolygon(reference_line_info, nudge_info);
    nudge_info->set_is_merge_block_obs(is_merge_obs_polygon);
    AINFO << "NudgeCalculation: is_merge_obs_polygon: " << is_merge_obs_polygon;
    AINFO << "block_sl_polygons size : " << nudge_info->block_sl_polygons().size();
 
    // calculate nudge_probability to make nudge decision
    if (IsProbEnoughToNudge(nudge_info, is_merge_obs_polygon)) {
        CalcNudgeExtraSpace(reference_line_info, nudge_info);
        nudge_info->set_enable(true);
        AINFO << "NudgeCalculation: Enable Nudge";
    } else {
        nudge_info->set_enable(false);
        AINFO << "NudgeCalculation: Not Enable Nudge";
    }
    // TODO(fengzhiqi): nudge safety check
}

BuildStaticTrackingObs()

void apollo::planning::NudgeCalculation::BuildStaticTrackingObs	(	ReferenceLineInfo *const	reference_line_info,
		const Obstacle &	obstacle,
		NudgeInfo *const	nudge_info
	)

在文件 nudge_calculation.cc 第 141 行定义.

                                     {
    std::string obs_id = obstacle.Id();
    RemainNudgeSpace remain_nudge_space;
    double lane_left_width = 0.0;
    double lane_right_width = 0.0;
    SLBoundary obs_boundary = obstacle.PerceptionSLBoundary();
    GetRefLineLaneWidth(
            reference_line_info->reference_line(),
            adc_boundary_.start_s(),
            0.5 * (obs_boundary.start_s() + obs_boundary.end_s()),
            &lane_left_width,
            &lane_right_width);
    bool obs_is_all_on_lane = obs_boundary.start_l() > -lane_right_width && obs_boundary.end_l() < lane_left_width;
    bool obs_far_from_lane_boundary = obs_boundary.start_l() - lane_left_width > veh_config_.width
            || obs_boundary.end_l() + lane_right_width < -veh_config_.width;
 
    bool is_obs_passable = CalcObsRemainSpace(reference_line_info, obstacle, &remain_nudge_space);
    AINFO << "NudgeCalculation: BuildStaticTrackingObs: [ " << obs_id << " ], is passable: " << is_obs_passable
          << ", obs_is_all_on_lane: " << obs_is_all_on_lane;
    auto* tracking_nudge_obs_info = nudge_info->mutable_tracking_nudge_obs_info();
    auto iter = tracking_nudge_obs_info->find(obs_id);
    if (iter != tracking_nudge_obs_info->end()) {
        AINFO << "NudgeCalculation: MatchTrackingObsInfo: [ " << obs_id << " ] with last tracking obs [ " << iter->first
              << " ]";
        UpdateTrackingObsInfo(&iter->second, obstacle, is_obs_passable, remain_nudge_space, obs_is_all_on_lane);
        nudge_info->mutable_update_ids()->insert(obs_id);
    } else {
        // Match obs and update obs info
        if (MatchTrackingObsInfo(obstacle, nudge_info, is_obs_passable, remain_nudge_space, obs_is_all_on_lane)) {
            return;
        }
        if (!is_obs_passable
            || ((obs_is_all_on_lane || !obs_far_from_lane_boundary)
                && (obstacle.Perception().type() == apollo::perception::PerceptionObstacle::UNKNOWN_UNMOVABLE
                    || obstacle.Perception().type() == apollo::perception::PerceptionObstacle::UNKNOWN))) {
            NudgeObstacleInfo new_track_obs_info;
            new_track_obs_info.start_track_time = ::apollo::cyber::Clock::NowInSeconds();
            UpdateTrackingObsInfo(
                    &new_track_obs_info, obstacle, is_obs_passable, remain_nudge_space, obs_is_all_on_lane);
            nudge_info->mutable_update_ids()->insert(obs_id);
            tracking_nudge_obs_info->emplace(obs_id, new_track_obs_info);
        }
    }
}

CalcNudgeExtraSpace()

void apollo::planning::NudgeCalculation::CalcNudgeExtraSpace	(	ReferenceLineInfo *const	reference_line_info,
		NudgeInfo *const	nudge_info
	)

在文件 nudge_calculation.cc 第 522 行定义.

                                                                                                                    {
    nudge_info->mutable_extra_nudge_key_points()->clear();
    if (nudge_info->block_sl_polygons().size() < 1) {
        AINFO << "No block_sl_polygons, return";
        return;
    }
    double start_s = 1000.0;
    double end_s = -1000.0;
    double ego_whole_pass_width = veh_config_.width;
    // need update
    double extend_length = veh_config_.length * config_.nudge_key_point_extend_length_coefficient();
 
    int box_index = 0;
    bool is_left = false;
    for (auto& sl_polygon : nudge_info->block_sl_polygons()) {
        if (box_index > 0 && (sl_polygon.MinS() - end_s) > 3.0 * veh_config_.length) {
            break;
        }
        if (box_index < 3) {
            ego_whole_pass_width = 0.5 * veh_config_.width
                    + util::CalculateEquivalentEgoWidth(*reference_line_info, sl_polygon.MinS(), &is_left);
            ++box_index;
        }
        start_s = std::min(sl_polygon.MinS() - extend_length, start_s);
        end_s = std::max(sl_polygon.MaxS() + extend_length, end_s);
    }
    start_s = std::max(adc_boundary_.start_s(), start_s);
    end_s = std::max(adc_boundary_.start_s(), end_s);
 
    std::vector<SLPoint> left_nudge_pts, right_nudge_pts;
    for (double s = start_s; s <= end_s; s += 0.1) {
        double left_max_nudge_l = -1000.0;
        double right_min_nudge_l = 1000.0;
        for (size_t index = 0; index < nudge_info->block_sl_polygons().size(); ++index) {
            SLPolygon current_polygon = nudge_info->block_sl_polygons().at(index);
            if ((index > 0 && s < current_polygon.MaxS()) || end_s < current_polygon.MinS()) {
                continue;
            }
            // if ((index < nudge_info->block_sl_polygons().size() - 1 && s > current_polygon.MaxS())
            //     || end_s < current_polygon.MinS()) {
            //     continue;
            // }
            left_max_nudge_l = std::max(
                    current_polygon.MaxL() + 0.5 * ego_whole_pass_width + FLAGS_static_obstacle_nudge_l_buffer,
                    left_max_nudge_l);
            right_min_nudge_l = std::min(
                    current_polygon.MinL() - 0.5 * ego_whole_pass_width - FLAGS_static_obstacle_nudge_l_buffer,
                    right_min_nudge_l);
            if (s < current_polygon.MinS()) {
                break;
            }
        }
        SLPoint left_pt, right_pt;
 
        left_pt.set_s(s);
        left_pt.set_l(left_max_nudge_l);
        right_pt.set_s(s);
        right_pt.set_l(right_min_nudge_l);
        left_nudge_pts.emplace_back(left_pt);
        right_nudge_pts.emplace_back(right_pt);
    }
    nudge_info->mutable_extra_nudge_key_points()->emplace_back(left_nudge_pts);
    nudge_info->mutable_extra_nudge_key_points()->emplace_back(right_nudge_pts);
 
    PrintCurves left_print_curve;
    for (auto& pt : nudge_info->extra_nudge_key_points().at(0)) {
        left_print_curve.AddPoint("LeftKeyPoints_NudgePoints", pt.s(), pt.l());
    }
    left_print_curve.PrintToLog();
 
    PrintCurves right_print_curve;
    for (auto& pt : nudge_info->extra_nudge_key_points().at(1)) {
        right_print_curve.AddPoint("RightKeyPoints_NudgePoints", pt.s(), pt.l());
    }
    right_print_curve.PrintToLog();
}

CalcObsRemainSpace()

bool apollo::planning::NudgeCalculation::CalcObsRemainSpace	(	ReferenceLineInfo *const	reference_line_info,
		const Obstacle &	obstacle,
		RemainNudgeSpace *	remain_space
	)

在文件 nudge_calculation.cc 第 223 行定义.

                                        {
    SLBoundary adc_boundary = reference_line_info->AdcSlBoundary();
    SLBoundary obs_boundary = obstacle.PerceptionSLBoundary();
    bool is_left = false;
    double ego_whole_pass_width = 0.5 * veh_config_.width
            + util::CalculateEquivalentEgoWidth(*reference_line_info, obs_boundary.start_s(), &is_left);
 
    double lane_left_width = 0.0;
    double lane_right_width = 0.0;
    double road_left_width = 0.0;
    double road_right_width = 0.0;
    hdmap::LaneBoundaryType::Type left_boundary_type = hdmap::LaneBoundaryType::UNKNOWN;
    hdmap::LaneBoundaryType::Type right_boundary_type = hdmap::LaneBoundaryType::UNKNOWN;
 
    reference_line_info->reference_line().GetLaneWidth(obs_boundary.start_s(), &lane_left_width, &lane_right_width);
    reference_line_info->reference_line().GetRoadWidth(obs_boundary.start_s(), &road_left_width, &road_right_width);
    reference_line_info->reference_line().GetLaneBoundaryType(
            obs_boundary.start_s(), &left_boundary_type, &right_boundary_type);
    remain_space->left_lane_boundary_type = left_boundary_type;
    remain_space->right_lane_boundary_type = right_boundary_type;
 
    remain_space->left_lane_remain
            = lane_left_width - ego_whole_pass_width - obs_boundary.end_l() - FLAGS_static_obstacle_nudge_l_buffer;
    remain_space->right_lane_remain
            = lane_right_width - ego_whole_pass_width + obs_boundary.start_l() - FLAGS_static_obstacle_nudge_l_buffer;
 
    if (remain_space->left_lane_boundary_type == hdmap::LaneBoundaryType::CURB) {
        remain_space->left_road_remain = remain_space->left_lane_remain;
    } else {
        remain_space->left_road_remain
                = road_left_width - ego_whole_pass_width - obs_boundary.end_l() - FLAGS_static_obstacle_nudge_l_buffer;
    }
 
    if (remain_space->right_lane_boundary_type == hdmap::LaneBoundaryType::CURB) {
        remain_space->right_road_remain = remain_space->right_lane_remain;
    } else {
        remain_space->right_road_remain = road_right_width - ego_whole_pass_width + obs_boundary.start_l()
                - FLAGS_static_obstacle_nudge_l_buffer;
    }
 
    bool is_passable = remain_space->left_lane_remain > config_.obs_passable_buffer()
            || remain_space->right_lane_remain > config_.obs_passable_buffer();
 
    ADEBUG << "CalcObsRemainSpace: [ " << obstacle.Id() << " ]" << ", left right lane_remain: [ "
           << remain_space->left_lane_remain << ", " << remain_space->right_lane_remain << " ]"
           << ", lane left right width: " << lane_left_width << ", " << lane_right_width << " ]"
           << ", ego_whole_pass_width: " << ego_whole_pass_width << ", obs_boundary l: [ " << obs_boundary.start_l()
           << ", " << obs_boundary.end_l() << " ]";
 
    // TODO(fengzhiqi) special buffer for different type obstacles
 
    return is_passable;
}

EraseObsInNudgeinfo()

void apollo::planning::NudgeCalculation::EraseObsInNudgeinfo	(	NudgeInfo *const	nudge_info,
		std::string	id
	)

在文件 nudge_calculation.cc 第 487 行定义.

                                                                                    {
    nudge_info->mutable_update_ids()->erase(id);
    nudge_info->mutable_tracking_nudge_obs_info()->erase(id);
}

IsProbEnoughToNudge()

bool apollo::planning::NudgeCalculation::IsProbEnoughToNudge	(	NudgeInfo *const	nudge_info,
		bool	is_merge_obs = false
	)

在文件 nudge_calculation.cc 第 492 行定义.

                                                                                         {
    if (nudge_info->block_sl_polygons().empty()) {
        return false;
    }
    SLPolygon first_block_polygon = nudge_info->block_sl_polygons().at(0);
    double max_nudge_probability = 0.0;
    if (is_merge_obs) {
        std::vector<std::string> block_ids;
        std::string id;
        std::istringstream ids_stream(first_block_polygon.id());
        while (std::getline(ids_stream, id, '+')) {
            block_ids.push_back(id);
        }
        for (const auto& obs_id : block_ids) {
            auto track_iter = nudge_info->tracking_nudge_obs_info().find(obs_id);
            if (track_iter == nudge_info->tracking_nudge_obs_info().end()) {
                continue;
            }
            max_nudge_probability = std::max(track_iter->second.nudge_probability, max_nudge_probability);
        }
    } else {
        auto track_iter = nudge_info->tracking_nudge_obs_info().find(first_block_polygon.id());
        if (track_iter != nudge_info->tracking_nudge_obs_info().end()) {
            max_nudge_probability = std::max(track_iter->second.nudge_probability, max_nudge_probability);
        }
    }
    AINFO << "max_nudge_probability: " << max_nudge_probability;
    return max_nudge_probability > 0.8;
}

MatchTrackingObsInfo()

bool apollo::planning::NudgeCalculation::MatchTrackingObsInfo	(	const Obstacle &	obstacle,
		NudgeInfo *const	nudge_info,
		bool	is_passable,
		RemainNudgeSpace	remain_nudge_space,
		bool	is_all_on_lane
	)

在文件 nudge_calculation.cc 第 189 行定义.

                             {
    std::string obs_id = obstacle.Id();
    for (auto iter = nudge_info->mutable_tracking_nudge_obs_info()->begin();
         iter != nudge_info->mutable_tracking_nudge_obs_info()->end();
         iter++) {
        // skip the tracking obs which is updated
        auto obs_id_iter = nudge_info->update_ids().find(iter->first);
        if (obs_id_iter != nudge_info->update_ids().end()) {
            continue;
        }
        // polygon match
        // case 1. tracking obs polygon contains obstacle // case 2. polygon IoU >
        // 0.6
        if (iter->second.expand_polygon.Contains(obstacle.PerceptionPolygon())
            || iter->second.expand_polygon.ComputeIoU(obstacle.PerceptionPolygon()) > 0.6) {
            NudgeObstacleInfo new_obs_info = iter->second;
            UpdateTrackingObsInfo(&new_obs_info, obstacle, is_passable, remain_nudge_space, is_all_on_lane);
            AINFO << "NudgeCalculation: MatchTrackingObsInfo [ " << obs_id << " ] with last tracking obs [ "
                  << iter->first << " ]";
            iter = nudge_info->mutable_tracking_nudge_obs_info()->erase(iter);
            nudge_info->mutable_tracking_nudge_obs_info()->emplace(obs_id, new_obs_info);
            nudge_info->mutable_update_ids()->insert(obs_id);
            return true;
        }
    }
    AINFO << "NudgeCalculation: MatchTrackingObsInfo, can not match any obs for [ " << obs_id << " ]";
    return false;
}

MergeObsPolygon()

bool apollo::planning::NudgeCalculation::MergeObsPolygon	(	ReferenceLineInfo *const	reference_line_info,
		NudgeInfo *const	nudge_info
	)

在文件 nudge_calculation.cc 第 354 行定义.

                                                                                                                {
    if (nudge_info->block_sl_polygons().size() < 1) {
        AINFO << "MergeObsPolygon: block_sl_polygons is empty, return";
        return false;
    }
 
    // Find first polygon all on road
    SLPolygon first_sl_polygon;
    for (size_t i = 0; i < nudge_info->block_sl_polygons().size(); ++i) {
        auto iter = nudge_info->tracking_nudge_obs_info().find(nudge_info->block_sl_polygons().at(i).id());
        if (iter != nudge_info->tracking_nudge_obs_info().end() && iter->second.is_all_on_lane) {
            first_sl_polygon = nudge_info->block_sl_polygons().at(0);
            break;
        }
    }
    if (first_sl_polygon.id().empty()) {
        AINFO << "MergeObsPolygon: not found first_sl_polygon";
        for (auto sl_polygon_iter = nudge_info->mutable_block_sl_polygons()->begin();
             sl_polygon_iter != nudge_info->mutable_block_sl_polygons()->end();) {
            auto track_iter = nudge_info->tracking_nudge_obs_info().find((*sl_polygon_iter).id());
            if (track_iter == nudge_info->tracking_nudge_obs_info().end()) {
                break;
            }
            if (track_iter->second.is_passable && !track_iter->second.is_all_on_lane) {
                EraseObsInNudgeinfo(nudge_info, (*sl_polygon_iter).id());
                sl_polygon_iter = nudge_info->mutable_block_sl_polygons()->erase(sl_polygon_iter);
            } else {
                ++sl_polygon_iter;
            }
        }
        return false;
    }
    // Merge from first polygon
    std::string merge_polygon_id = "";
    std::vector<Vec2d> merge_points;
    for (auto sl_polygon_iter = nudge_info->mutable_block_sl_polygons()->begin();
         sl_polygon_iter != nudge_info->mutable_block_sl_polygons()->end();) {
        if (sl_polygon_iter->MinS() - first_sl_polygon.MaxS() > 0.8) {
            break;
        }
        auto track_iter = nudge_info->tracking_nudge_obs_info().find((*sl_polygon_iter).id());
        if (track_iter == nudge_info->tracking_nudge_obs_info().end()) {
            break;
        }
        if (!track_iter->second.is_all_on_lane) {
            ++sl_polygon_iter;
            continue;
        }
        merge_points.insert(
                merge_points.end(),
                track_iter->second.origin_polygon.points().begin(),
                track_iter->second.origin_polygon.points().end());
        merge_polygon_id += (*sl_polygon_iter).id() + "+";
        // erase obs which is merged
        sl_polygon_iter = nudge_info->mutable_block_sl_polygons()->erase(sl_polygon_iter);
    }
 
    if (merge_points.empty()) {
        return false;
    }
    Polygon2d merge_polygon;
    if (!common::math::Polygon2d::ComputeConvexHull(merge_points, &merge_polygon)) {
        return false;
    }
 
    // Merge the obstacle near lane boundary
    for (auto sl_polygon_iter = nudge_info->mutable_block_sl_polygons()->begin();
         sl_polygon_iter != nudge_info->mutable_block_sl_polygons()->end();) {
        auto track_iter = nudge_info->tracking_nudge_obs_info().find((*sl_polygon_iter).id());
        if (track_iter == nudge_info->tracking_nudge_obs_info().end()) {
            break;
        }
        if (!track_iter->second.is_passable) {
            ++sl_polygon_iter;
            continue;
        } else if (sl_polygon_iter->MinS() - first_sl_polygon.MaxS() > 0.8) {
            AINFO << "MergeObsPolygon: erase obs far and passable: " << (*sl_polygon_iter).id();
            EraseObsInNudgeinfo(nudge_info, (*sl_polygon_iter).id());
            sl_polygon_iter = nudge_info->mutable_block_sl_polygons()->erase(sl_polygon_iter);
            continue;
        }
 
        if (!track_iter->second.is_all_on_lane
            && merge_polygon.DistanceTo(track_iter->second.origin_polygon)
                    < veh_config_.width + FLAGS_static_obstacle_nudge_l_buffer * 2.0) {
            merge_points.insert(
                    merge_points.end(),
                    track_iter->second.origin_polygon.points().begin(),
                    track_iter->second.origin_polygon.points().end());
            merge_polygon_id += (*sl_polygon_iter).id() + "+";
        } else {
            // erase obs far from lane boundary
            AINFO << "MergeObsPolygon: erase obs far from lane boundary: " << (*sl_polygon_iter).id();
            EraseObsInNudgeinfo(nudge_info, (*sl_polygon_iter).id());
        }
        // erase obs which is merged
        sl_polygon_iter = nudge_info->mutable_block_sl_polygons()->erase(sl_polygon_iter);
    }
    Polygon2d merge_polygon_update;
    if (!common::math::Polygon2d::ComputeConvexHull(merge_points, &merge_polygon_update)) {
        return false;
    }
 
    SLBoundary merge_sl_boundary;
    if (!reference_line_info->reference_line().GetSLBoundary(merge_polygon_update, &merge_sl_boundary)) {
        nudge_info->mutable_block_sl_polygons()->insert(
                nudge_info->mutable_block_sl_polygons()->begin(), first_sl_polygon);
        AINFO << "MergeObsPolygon: GetSLBoundary failed";
        return false;
    }
 
    SLPolygon merge_sl_polygon(merge_sl_boundary, merge_polygon_id);
    nudge_info->mutable_block_sl_polygons()->insert(nudge_info->mutable_block_sl_polygons()->begin(), merge_sl_polygon);
 
    if (merge_polygon_update.points().size() > 0) {
        PrintCurves print_curve;
        for (const auto& p : merge_polygon_update.points()) {
            print_curve.AddPoint(merge_polygon_id + "_BlockObsPolygon", p.x(), p.y());
        }
        print_curve.AddPoint(
                merge_polygon_id + "_BlockObsPolygon", merge_polygon.points()[0].x(), merge_polygon.points()[0].y());
        print_curve.PrintToLog();
    }
    AINFO << "MergeObsPolygon: " << merge_polygon_id;
 
    // sort sl polygons again
    nudge_info->SortBlockSLPolygons();
    if (nudge_info->block_sl_polygons().at(0).id() == merge_polygon_id) {
        return true;
    }
    return false;
}

set_in_nudge_state()

void apollo::planning::NudgeCalculation::set_in_nudge_state

(

bool

state

)

在文件 nudge_calculation.cc 第 280 行定义.

                                                    {
    is_in_nudge_state_ = state;
}

---

该类的文档由以下文件生成:

• modules/planning/tasks/obstacle_nudge_decider/nudge_calculation.h
• modules/planning/tasks/obstacle_nudge_decider/nudge_calculation.cc