apollo::planning::SquareLaneFollowStage类 参考

#include <square_lane_follow_stage.h>

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

apollo::planning::SquareLaneFollowStage 的协作图:

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)
StageResult	FinishStage ()
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_

详细描述

在文件 square_lane_follow_stage.h 第 39 行定义.

成员函数说明

FinishStage()

StageResult apollo::planning::SquareLaneFollowStage::FinishStage

(

)

在文件 square_lane_follow_stage.cc 第 123 行定义.

                                               {
    next_stage_ = "EXTRICATE_STAGE";
    return StageResult(StageStatusType::FINISHED);
}

GetStopSL()

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

Init()

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

virtual

重载 apollo::planning::Stage .

在文件 square_lane_follow_stage.cc 第 35 行定义.

                       {
    CHECK_NOTNULL(context);
    bool ret = Stage::Init(config, injector, config_dir, context);
    if (!ret) {
        AERROR << Name() << "init failed!";
        return false;
    }
    return ret;
}

PlanFallbackTrajectory()

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

PlanOnReferenceLine()

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

Process()

StageResult apollo::planning::SquareLaneFollowStage::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.

在文件 square_lane_follow_stage.cc 第 49 行定义.

                                                                                                          {
    AINFO << "In SquareLaneFollowStage";
    if (frame->reference_line_info().empty()) {
        return StageResult(StageStatusType::FINISHED);
    }
    apollo::common::VehicleState vehicle_state = injector_->vehicle_state()->vehicle_state();
    if (vehicle_state.gear() == canbus::Chassis::GEAR_REVERSE) {
        frame->mutable_open_space_info()->set_is_on_open_space_trajectory(true);
        auto* traj_pair = frame->mutable_open_space_info()->mutable_publishable_trajectory_data();
        static constexpr double gear_shift_max_t = 5.0;
        static constexpr double gear_shift_unit_t = 0.1;
        for (double t = 0.0; t < gear_shift_max_t; t += gear_shift_unit_t) {
            common::TrajectoryPoint point;
            point.mutable_path_point()->set_x(frame->vehicle_state().x());
            point.mutable_path_point()->set_y(frame->vehicle_state().y());
            point.mutable_path_point()->set_theta(frame->vehicle_state().heading());
            point.mutable_path_point()->set_s(0.0);
            point.mutable_path_point()->set_kappa(0);
            point.set_relative_time(t);
            point.set_v(0.0);
            point.set_a(0.0);
            traj_pair->first.emplace_back(point);
        }
        traj_pair->second = apollo::canbus::Chassis::GEAR_DRIVE;
        AINFO << "GEAR SHIFT TO DRIVE";
        return StageResult(StageStatusType::RUNNING);
    }
    std::string junction_id = GetContextAs<SquareContext>()->junction_id;
    StageResult result = ExecuteTaskOnReferenceLine(planning_start_point, frame);
    if (result.HasError()) {
        AERROR << "SquareLaneFollowStage planning error";
    }
    const auto& reference_line_info = frame->reference_line_info().back();
    hdmap::PathOverlap* current_pnc_junction
            = reference_line_info.GetOverlapOnReferenceLine(junction_id, ReferenceLineInfo::JUNCTION);
    double adc_end_s = reference_line_info.AdcSlBoundary().end_s();
    if (!current_pnc_junction || current_pnc_junction->end_s < adc_end_s) {
        return FinishScenario();
    }
    const auto& path_data = reference_line_info.path_data();
    bool is_path_blocked = false;
    if (path_data.path_label().find("regular") == std::string::npos) {
        is_path_blocked = true;
    } else if (!path_data.blocking_obstacle_id().empty()) {
        is_path_blocked = true;
    } else {
        is_path_blocked = false;
    }
    bool is_square_path_block = is_path_blocked && vehicle_state.linear_velocity() < 0.1;
 
    double trajectory_length = 0;
    if (reference_line_info.trajectory().size() > 0) {
        trajectory_length = reference_line_info.trajectory().back().path_point().s()
                - reference_line_info.trajectory().front().path_point().s();
    }
    is_square_path_block = is_square_path_block
            || (path_data.path_label().find("square") != std::string::npos && vehicle_state.linear_velocity() < 0.1
                && trajectory_length < 0.1);
    if (is_square_path_block) {
        blocking_times_ = std::min<int>(blocking_times_ + 1, 50);
    } else {
        blocking_times_ = 0;
    }
    AINFO << "label: " << path_data.path_label() << ", blocking_obstacle_id: " << path_data.blocking_obstacle_id()
          << ", is_path_blocked: " << is_path_blocked
          << ", vehicle_state.linear_velocity: " << vehicle_state.linear_velocity()
          << ", blocking_times_: " << blocking_times_;
    if (blocking_times_ >= 50) {
        auto* context = GetContextAs<SquareContext>();
        context->blocking_obstacle_id = path_data.blocking_obstacle_id();
        return FinishStage();
    }
    return StageResult(StageStatusType::RUNNING);
}

RecordObstacleDebugInfo()

void apollo::planning::SquareLaneFollowStage::RecordObstacleDebugInfo

(

ReferenceLineInfo *

reference_line_info

)

---

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

• modules/planning/scenarios/square/square_lane_follow_stage.h
• modules/planning/scenarios/square/square_lane_follow_stage.cc