apollo::planning::LaneEscapeParkStage类 参考

#include <lane_escape_stage.h>

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

apollo::planning::LaneEscapeParkStage 的协作图:

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.
bool	HasCollision (Frame *frame, const common::math::Box2d &adc_box)
bool	GenerateStraightReversePath (PathData *path_data, const common::TrajectoryPoint &init_point)
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_escape_stage.h 第 36 行定义.

成员函数说明

GenerateStraightReversePath()

bool apollo::planning::LaneEscapeParkStage::GenerateStraightReversePath	(	PathData *	path_data,
		const common::TrajectoryPoint &	init_point
	)

HasCollision()

bool apollo::planning::LaneEscapeParkStage::HasCollision	(	Frame *	frame,
		const common::math::Box2d &	adc_box
	)

Init()

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

virtual

重载 apollo::planning::Stage .

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

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

Process()

StageResult apollo::planning::LaneEscapeParkStage::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_escape_stage.cc 第 50 行定义.

                                                                                                       {
    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) {
            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_REVERSE;
        AINFO << "GENERATE REVERSE TRAJECTORY";
        need_to_stop_ = false;
        return StageResult(StageStatusType::RUNNING);
    }
    auto& reference_line_info = frame->mutable_reference_line_info()->back();
    const auto& reference_line = reference_line_info.reference_line();
    double adc_start_s = reference_line_info.AdcSlBoundary().start_s();
 
    if (stop_point_ == nullptr) {
        apollo::common::math::Vec2d stop_point;
        auto ref_point = reference_line.GetReferencePoint(adc_start_s - scenario_config_.escape_reverse_distance());
        stop_point_ = std::make_shared<common::PathPoint>(ref_point.ToPathPoint(0.0));
    }
    AINFO << "need to stop";
    common::SLPoint sl_point;
    reference_line.XYToSL(*stop_point_, &sl_point);
    std::vector<std::string> wait_for_obstacles;
    planning::util::BuildStopDecision(
            "reverse_stop",
            sl_point.s(),
            0.1,
            StopReasonCode::STOP_REASON_STOP_SIGN,
            wait_for_obstacles,
            "reverse stop",
            frame,
            &(frame->mutable_reference_line_info()->front()),
            10.0);
    StageResult result = ExecuteTaskOnReferenceLine(planning_init_point, frame);
    const auto& candidate_paths = reference_line_info.GetCandidatePathData();
    bool has_regular_path = false;
    for (const auto& path_data : candidate_paths) {
        if (path_data.path_label().find("regular") != std::string::npos) {
            AINFO << "path_label: true";
        }
        double distance_to_block_obs = std::numeric_limits<double>::max();
        if (!path_data.blocking_obstacle_id().empty()) {
            auto obstacle = reference_line_info.path_decision()->Find(path_data.blocking_obstacle_id());
            distance_to_block_obs
                    = obstacle->PerceptionSLBoundary().start_s() - reference_line_info.AdcSlBoundary().end_s();
            AINFO << "blocking_obstacle_id: " << path_data.blocking_obstacle_id()
                  << " distance_to_block_obs: " << distance_to_block_obs;
        }
        if (path_data.discretized_path().Length() > scenario_config_.forward_path_length()) {
            AINFO << "discretized_path().Length(): true";
        }
        if (path_data.path_label().find("regular") != std::string::npos
            && path_data.discretized_path().Length() > scenario_config_.forward_path_length()
            && distance_to_block_obs > scenario_config_.forward_path_length()) {
            AINFO << "find regular path: " << path_data.path_label();
            AINFO << "distance_to_block_obs: " << distance_to_block_obs;
            has_regular_path = true;
            break;
        }
    }
    if (has_regular_path) {
        success_path_times_++;
    } else {
        success_path_times_ = 0;
        AINFO << "reset";
        need_to_stop_ = false;
    }
    AINFO << "success_path_times_: " << success_path_times_;
    if (success_path_times_ > scenario_config_.success_path_count()
        && !reference_line_info.path_data().discretized_path().empty()
        && !need_to_stop_) {
        AINFO << "find square path change stage";
        need_to_stop_ = true;
        double ego_speed = frame->vehicle_state().linear_velocity();
        double deceleration_distance = ego_speed * ego_speed / (2 * 0.5);
        double stop_distance = std::min(
                std::max(deceleration_distance, scenario_config_.escape_stop_distance()),
                reference_line_info.mutable_path_data()->discretized_path().Length());
        *stop_point_ = reference_line_info.mutable_path_data()->discretized_path().Evaluate(stop_distance);
        AINFO << "length: " << reference_line_info.mutable_path_data()->discretized_path().Length()
              << " stop_distance: " << stop_distance << " deceleration_distance: " << deceleration_distance
              << std::setprecision(9) << "stop_point_:" << stop_point_->x() << "," << stop_point_->y();
    }
    stop_check_count_ = std::fabs(vehicle_state.linear_velocity()) < 0.05 ? stop_check_count_ + 1 : 0;
    if (success_path_times_ > scenario_config_.success_path_count()
        && stop_check_count_ > scenario_config_.stop_check_window()) {
        AINFO << "already stopped change stage" << vehicle_state.linear_velocity() << " " << vehicle_state.x() << ", "
              << vehicle_state.y();
        return FinishStage();
    }
    return result.SetStageStatus(StageStatusType::RUNNING);
}

---

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

• modules/planning/scenarios/lane_escape/lane_escape_stage.h
• modules/planning/scenarios/lane_escape/lane_escape_stage.cc