apollo::planning::LaneFollowParkStage类 参考

#include <lane_follow_park_stage.h>

类 apollo::planning::LaneFollowParkStage 继承关系图:

apollo::planning::LaneFollowParkStage 的协作图:

Public 成员函数
bool	Init (const StagePipeline &config, const std::shared_ptr< DependencyInjector > &injector, const std::string &config_dir, void *context)
StageResult	Process (const common::TrajectoryPoint &planning_init_point, Frame *frame) override
	Each stage does its business logic inside Process function.
StageResult	PlanOnReferenceLine (const common::TrajectoryPoint &planning_start_point, Frame *frame, ReferenceLineInfo *reference_line_info)
void	PlanFallbackTrajectory (const common::TrajectoryPoint &planning_start_point, Frame *frame, ReferenceLineInfo *reference_line_info)
common::SLPoint	GetStopSL (const ObjectStop &stop_decision, const ReferenceLine &reference_line) const
void	RecordObstacleDebugInfo (ReferenceLineInfo *reference_line_info)
Public 成员函数 继承自 apollo::planning::Stage
	Stage ()
virtual	~Stage ()=default
const std::string &	Name () const
template<typename T >
T *	GetContextAs () const
const std::string &	NextStage () const

额外继承的成员函数
Protected 成员函数 继承自 apollo::planning::Stage
StageResult	ExecuteTaskOnReferenceLine (const common::TrajectoryPoint &planning_start_point, Frame *frame)
StageResult	ExecuteTaskOnReferenceLineForOnlineLearning (const common::TrajectoryPoint &planning_start_point, Frame *frame)
StageResult	ExecuteTaskOnOpenSpace (Frame *frame)
virtual StageResult	FinishScenario ()
void	RecordDebugInfo (ReferenceLineInfo *reference_line_info, const std::string &name, const double time_diff_ms)
Protected 属性 继承自 apollo::planning::Stage
std::vector< std::shared_ptr< Task > >	task_list_
std::shared_ptr< Task >	fallback_task_
std::string	next_stage_
void *	context_
std::shared_ptr< DependencyInjector >	injector_
StagePipeline	pipeline_config_

详细描述

在文件 lane_follow_park_stage.h 第 44 行定义.

成员函数说明

GetStopSL()

SLPoint apollo::planning::LaneFollowParkStage::GetStopSL	(	const ObjectStop &	stop_decision,
		const ReferenceLine &	reference_line
	)		const

在文件 lane_follow_park_stage.cc 第 271 行定义.

                                                                                                                 {
    SLPoint sl_point;
    reference_line.XYToSL(stop_decision.stop_point(), &sl_point);
    return sl_point;
}

Init()

bool apollo::planning::LaneFollowParkStage::Init ( const StagePipeline &  config, const std::shared_ptr< DependencyInjector > &  injector, const std::string &  config_dir, void *  context  )

virtual

重载 apollo::planning::Stage .

在文件 lane_follow_park_stage.cc 第 54 行定义.

                       {
    CHECK_NOTNULL(context);
    bool ret = Stage::Init(config, injector, config_dir, context);
    if (!ret) {
        AERROR << Name() << "init failed!";
        return false;
    }
    scenario_config_.CopyFrom(GetContextAs<LaneFollowParkContext>()->scenario_config);
    return ret;
}

PlanFallbackTrajectory()

void apollo::planning::LaneFollowParkStage::PlanFallbackTrajectory	(	const common::TrajectoryPoint &	planning_start_point,
		Frame *	frame,
		ReferenceLineInfo *	reference_line_info
	)

PlanOnReferenceLine()

StageResult apollo::planning::LaneFollowParkStage::PlanOnReferenceLine	(	const common::TrajectoryPoint &	planning_start_point,
		Frame *	frame,
		ReferenceLineInfo *	reference_line_info
	)

在文件 lane_follow_park_stage.cc 第 163 行定义.

                                                {
    if (!reference_line_info->IsChangeLanePath()) {
        reference_line_info->AddCost(kStraightForwardLineCost);
    }
    ADEBUG << "planning start point:" << planning_start_point.DebugString();
    ADEBUG << "Current reference_line_info is IsChangeLanePath: " << reference_line_info->IsChangeLanePath();
 
    StageResult ret;
    for (auto task : task_list_) {
        const double start_timestamp = Clock::NowInSeconds();
        const auto start_planning_perf_timestamp
                = std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count();
 
        ret.SetTaskStatus(task->Execute(frame, reference_line_info));
 
        const double end_timestamp = Clock::NowInSeconds();
        const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
        ADEBUG << "after task[" << task->Name() << "]:" << reference_line_info->PathSpeedDebugString();
        ADEBUG << task->Name() << " time spend: " << time_diff_ms << " ms.";
        RecordDebugInfo(reference_line_info, task->Name(), time_diff_ms);
 
        const auto end_planning_perf_timestamp
                = std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count();
        const auto plnning_perf_ms = (end_planning_perf_timestamp - start_planning_perf_timestamp) * 1000;
        AINFO << "Planning Perf: task name [" << task->Name() << "], " << plnning_perf_ms << " ms.";
 
        if (ret.IsTaskError()) {
            AERROR << "Failed to run tasks[" << task->Name()
                   << "], Error message: " << ret.GetTaskStatus().error_message();
            break;
        }
 
        // TODO(SHU): disable reference line order changes for now
        // updated reference_line_info, because it is changed in
        // lane_change_decider by PrioritizeChangeLane().
        // reference_line_info = &frame->mutable_reference_line_info()->front();
        // ADEBUG << "Current reference_line_info is IsChangeLanePath: "
        //        << reference_line_info->IsChangeLanePath();
    }
 
    RecordObstacleDebugInfo(reference_line_info);
 
    // check path and speed results for path or speed fallback
    reference_line_info->set_trajectory_type(ADCTrajectory::NORMAL);
    if (ret.IsTaskError()) {
        fallback_task_->Execute(frame, reference_line_info);
    }
 
    DiscretizedTrajectory trajectory;
    if (!reference_line_info->CombinePathAndSpeedProfile(
                planning_start_point.relative_time(), planning_start_point.path_point().s(), &trajectory)) {
        const std::string msg = "Fail to aggregate planning trajectory.";
        AERROR << msg;
        return ret.SetStageStatus(StageStatusType::ERROR, msg);
    }
 
    // determine if there is a destination on reference line.
    double dest_stop_s = -1.0;
    for (const auto* obstacle : reference_line_info->path_decision()->obstacles().Items()) {
        if (obstacle->LongitudinalDecision().has_stop()
            && obstacle->LongitudinalDecision().stop().reason_code() == STOP_REASON_DESTINATION) {
            SLPoint dest_sl = GetStopSL(obstacle->LongitudinalDecision().stop(), reference_line_info->reference_line());
            dest_stop_s = dest_sl.s();
        }
    }
 
    for (const auto* obstacle : reference_line_info->path_decision()->obstacles().Items()) {
        if (obstacle->IsVirtual()) {
            continue;
        }
        if (!obstacle->IsStatic()) {
            continue;
        }
        if (obstacle->LongitudinalDecision().has_stop()) {
            bool add_stop_obstacle_cost = false;
            if (dest_stop_s < 0.0) {
                add_stop_obstacle_cost = true;
            } else {
                SLPoint stop_sl
                        = GetStopSL(obstacle->LongitudinalDecision().stop(), reference_line_info->reference_line());
                if (stop_sl.s() < dest_stop_s && (dest_stop_s - reference_line_info->AdcSlBoundary().end_s()) < 20.0) {
                    add_stop_obstacle_cost = true;
                }
            }
            if (add_stop_obstacle_cost) {
                static constexpr double kReferenceLineStaticObsCost = 1e3;
                reference_line_info->AddCost(kReferenceLineStaticObsCost);
            }
        }
    }
 
    if (FLAGS_enable_trajectory_check) {
        if (ConstraintChecker::ValidTrajectory(trajectory) != ConstraintChecker::Result::VALID) {
            const std::string msg = "Current planning trajectory is not valid.";
            AERROR << msg;
            return ret.SetStageStatus(StageStatusType::ERROR, msg);
        }
    }
 
    reference_line_info->SetTrajectory(trajectory);
    reference_line_info->SetDrivable(true);
    ret.SetStageStatus(StageStatusType::RUNNING);
    return ret;
}

Process()

StageResult apollo::planning::LaneFollowParkStage::Process ( const common::TrajectoryPoint &  planning_init_point, Frame *  frame  )

overridevirtual

Each stage does its business logic inside Process function.

If the stage want to transit to a different stage after finish, it should set the type of 'next_stage_'.

实现了 apollo::planning::Stage.

在文件 lane_follow_park_stage.cc 第 95 行定义.

                                                                                                  {
    if (frame->reference_line_info().empty()) {
        return StageResult(StageStatusType::FINISHED);
    }
 
    bool has_drivable_reference_line = false;
 
    ADEBUG << "Number of reference lines:\t" << frame->mutable_reference_line_info()->size();
 
    unsigned int count = 0;
    StageResult result;
    int need_escape = 0;
    for (auto& reference_line_info : *frame->mutable_reference_line_info()) {
        // TODO(SHU): need refactor
        if (count++ == frame->mutable_reference_line_info()->size()) {
            break;
        }
        ADEBUG << "No: [" << count << "] Reference Line.";
        ADEBUG << "IsChangeLanePath: " << reference_line_info.IsChangeLanePath();
 
        if (has_drivable_reference_line) {
            reference_line_info.SetDrivable(false);
            break;
        }
 
        result = PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);
 
        bool escape = LaneEscapeUtil::IsNeedEscape(
                reference_line_info,
                scenario_config_.min_distance_block_obs_to_junction(),
                scenario_config_.enable_junction_borrow(),
                scenario_config_.passby_min_gap(),
                scenario_config_.passby_kappa_ratio(),
                scenario_config_.queue_check_count(),
                scenario_config_.stable_block_count());
        need_escape = (escape && need_escape != -1) ? 1 : -1;
        AINFO << "need escape: " << need_escape;
 
        if (!result.HasError()) {
            if (!reference_line_info.IsChangeLanePath()) {
                ADEBUG << "reference line is NOT lane change ref.";
                has_drivable_reference_line = true;
                continue;
            }
            if (reference_line_info.Cost() < kStraightForwardLineCost) {
                // If the path and speed optimization succeed on target lane while
                // under smart lane-change or IsClearToChangeLane under older version
                has_drivable_reference_line = true;
                reference_line_info.SetDrivable(true);
            } else {
                reference_line_info.SetDrivable(false);
                ADEBUG << "\tlane change failed";
            }
        } else {
            reference_line_info.SetDrivable(false);
        }
    }
    AINFO << "need escape: " << need_escape;
    if (need_escape == 1) {
        ParkDataCenter::Instance()->set_need_escape(true);
        next_stage_.clear();
        return StageResult(StageStatusType::FINISHED);
    }
 
    return has_drivable_reference_line ? result.SetStageStatus(StageStatusType::RUNNING)
                                       : result.SetStageStatus(StageStatusType::ERROR);
}

RecordObstacleDebugInfo()

void apollo::planning::LaneFollowParkStage::RecordObstacleDebugInfo

(

ReferenceLineInfo *

reference_line_info

)

在文件 lane_follow_park_stage.cc 第 69 行定义.

                                                                                        {
    if (!FLAGS_enable_record_debug) {
        ADEBUG << "Skip record debug info";
        return;
    }
    auto ptr_debug = reference_line_info->mutable_debug();
 
    const auto path_decision = reference_line_info->path_decision();
    for (const auto obstacle : path_decision->obstacles().Items()) {
        auto obstacle_debug = ptr_debug->mutable_planning_data()->add_obstacle();
        obstacle_debug->set_id(obstacle->Id());
        obstacle_debug->mutable_sl_boundary()->CopyFrom(obstacle->PerceptionSLBoundary());
        const auto& decider_tags = obstacle->decider_tags();
        const auto& decisions = obstacle->decisions();
        if (decider_tags.size() != decisions.size()) {
            AERROR << "decider_tags size: " << decider_tags.size()
                   << " different from decisions size:" << decisions.size();
        }
        for (size_t i = 0; i < decider_tags.size(); ++i) {
            auto decision_tag = obstacle_debug->add_decision_tag();
            decision_tag->set_decider_tag(decider_tags[i]);
            decision_tag->mutable_decision()->CopyFrom(decisions[i]);
        }
    }
}

---

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

• modules/planning/scenarios/lane_follow_park/lane_follow_park_stage.h
• modules/planning/scenarios/lane_follow_park/lane_follow_park_stage.cc