#include <extricate_stage.h>

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

apollo::planning::ExtricateStage 的协作图:

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_

详细描述

在文件 extricate_stage.h 第 35 行定义.

成员函数说明

◆ GenerateStraightReversePath()

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

在文件 extricate_stage.cc 第 148 行定义.

                                                                                                             {
    DiscretizedPath path;
    std::vector<common::PathPoint> path_points;
    static constexpr double kReversePathLength = 10.0;
    static constexpr double kReversePathUnit = 0.5;
    for (double s = 0; s <= kReversePathLength; s += kReversePathUnit) {
        common::PathPoint point;
        point.set_x(init_point.path_point().x() - s * std::cos(init_point.path_point().theta()));
        point.set_y(init_point.path_point().y() - s * std::sin(init_point.path_point().theta()));
        point.set_theta(init_point.path_point().theta());
        point.set_s(s);
        point.set_kappa(0.0);
        point.set_dkappa(0.0);
        point.set_ddkappa(0.0);
        path.emplace_back(point);
    }
    path_data->SetDiscretizedPath(path);
    path_data->set_path_label("reverse_straight_path");
    path_data->set_is_reverse_path(true);
    return true;
}

◆ HasCollision()

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

在文件 extricate_stage.cc 第 137 行定义.

                                                                              {
    auto obstacles_by_frame = frame->GetObstacleList();
    common::math::Polygon2d adc_polygon(adc_box);
    for (const auto& obstacle : obstacles_by_frame->Items()) {
        if (adc_polygon.HasOverlap(obstacle->PerceptionPolygon())) {
            return true;
        }
    }
    return false;
}

◆ Init()

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

virtual

重载 apollo::planning::Stage .

在文件 extricate_stage.cc 第 36 行定义.

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

◆ Process()

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

在文件 extricate_stage.cc 第 51 行定义.

                                                                                                  {
    AINFO << "In ExtricateStage";
    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);
    }
    AINFO << "gear is reverse";
    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();
    // GenerateStraightReversePath(reference_line_info.mutable_path_data(),
    //                             planning_init_point);
    if (stop_point_ == nullptr) {
        apollo::common::math::Vec2d stop_point;
        auto ref_point = reference_line.GetReferencePoint(adc_start_s - 10.0);
        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()));
    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 && path_data.blocking_obstacle_id().empty()) {
            AINFO << "find regular path: " << path_data.path_label();
            has_regular_path = true;
            break;
        }
    }
    if (has_regular_path) {
        success_path_times_++;
    } else {
        success_path_times_ = 0;
        need_to_stop_ = false;
    }
    if (success_path_times_ > 10 && !need_to_stop_) {
        AINFO << "find square path change stage";
        need_to_stop_ = true;
        *stop_point_ = reference_line_info.mutable_path_data()->discretized_path().Evaluate(5.0);
        AINFO << "stop_point_:" << stop_point_->x() << "," << stop_point_->y();
    }
    if (success_path_times_ > 10 && std::fabs(vehicle_state.linear_velocity()) < 0.1) {
        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/square/extricate_stage.h
modules/planning/scenarios/square/extricate_stage.cc