apollo::planning::StageFreeSpace类 参考

#include <stage_free_space.h>

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

apollo::planning::StageFreeSpace 的协作图:

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.
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_

详细描述

在文件 stage_free_space.h 第 32 行定义.

成员函数说明

Init()

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

virtual

重载 apollo::planning::Stage .

在文件 stage_free_space.cc 第 31 行定义.

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

Process()

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

在文件 stage_free_space.cc 第 46 行定义.

                                                                                                  {
    auto* mutable_open_space_info = frame->mutable_open_space_info();
    const auto& free_space_command = GetContextAs<FreeSpaceContext>()->free_space_command;
    // Set free space  drivable roi boundary
    std::vector<std::vector<common::math::Vec2d>> roi_boundary;
    roi_boundary.emplace_back();
    const auto& parking_roi = free_space_command.drivable_roi();
    for (int j = 0; j < parking_roi.point_size(); j++) {
        roi_boundary.back().emplace_back(parking_roi.point(j).x(), parking_roi.point(j).y());
    }
    if (roi_boundary.back().begin()->DistanceTo(*roi_boundary.back().rbegin()) > 0.01) {
        roi_boundary.back().push_back(*roi_boundary.begin()->begin());
    }
    // Set XY boundary
    OpenSpaceRoiUtil::GetRoiXYBoundary(roi_boundary, mutable_open_space_info->mutable_ROI_xy_boundary());
    // Set free space non drivable roi boundary
    auto roi_x_min = mutable_open_space_info->mutable_ROI_xy_boundary()->at(0);
    auto roi_x_max = mutable_open_space_info->mutable_ROI_xy_boundary()->at(1);
    auto roi_y_min = mutable_open_space_info->mutable_ROI_xy_boundary()->at(2);
    auto roi_y_max = mutable_open_space_info->mutable_ROI_xy_boundary()->at(3);
    for (int i = 0; i < free_space_command.non_drivable_roi_size(); i++) {
        roi_boundary.emplace_back();
        const auto& parking_roi = free_space_command.non_drivable_roi(i);
        for (int j = 0; j < parking_roi.point_size(); j++) {
            auto x = parking_roi.point(j).x();
            auto y = parking_roi.point(j).y();
            if (x < roi_x_min || x > roi_x_max || y < roi_y_min || y > roi_y_max) {
                AERROR << "non drivable roi should be contained by drivable roi";
                return StageResult(StageStatusType::ERROR);
            }
            roi_boundary.back().emplace_back(x, y);
        }
    }
    if (roi_boundary.back().begin()->DistanceTo(*roi_boundary.back().rbegin()) > 0.01) {
        roi_boundary.back().push_back(*roi_boundary.begin()->begin());
    }
    // Set end pose
    auto* open_space_end_pose = mutable_open_space_info->mutable_open_space_end_pose();
    open_space_end_pose->push_back(free_space_command.parking_spot_pose().x());
    open_space_end_pose->push_back(free_space_command.parking_spot_pose().y());
    open_space_end_pose->push_back(free_space_command.parking_spot_pose().heading());
    open_space_end_pose->push_back(0);
 
    double d_x = frame->vehicle_state().x() - free_space_command.parking_spot_pose().x();
    double d_y = frame->vehicle_state().y() - free_space_command.parking_spot_pose().y();
    double d_heading = frame->vehicle_state().heading() - free_space_command.parking_spot_pose().heading();
    AINFO << "Current position diff with end pose! x= " << d_x << ";y= " << d_y << ";heading" << d_heading
          << ";Distance with end pose:" << std::sqrt(d_x * d_x + d_y * d_y);
    // Add obstacle boundary to roi_bound
    if (scenario_config_.enabled_perception_obstacles()) {
        OpenSpaceRoiUtil::LoadObstacles(
                scenario_config_.filtering_distance(),
                scenario_config_.perception_obstacle_buffer(),
                mutable_open_space_info->ROI_xy_boundary(),
                frame,
                &roi_boundary);
    }
    *mutable_open_space_info->mutable_obstacles_vertices_vec() = roi_boundary;
    // transform by origin point
    const auto& xy_boundary = mutable_open_space_info->ROI_xy_boundary();
    Vec2d origin_point(xy_boundary[0], xy_boundary[2]);
    OpenSpaceRoiUtil::TransformByOriginPoint(origin_point, 0.0, mutable_open_space_info);
    // Formulate as Ax < b
    if (!OpenSpaceRoiUtil::FormulateBoundaryConstraints(mutable_open_space_info)) {
        return StageResult(StageStatusType::ERROR);
    }
    mutable_open_space_info->set_is_on_open_space_trajectory(true);
 
    StageResult result = ExecuteTaskOnOpenSpace(frame);
    if (result.HasError()) {
        AERROR << "StageFreeSpace planning error";
        return result.SetStageStatus(StageStatusType::ERROR);
    }
    return result.SetStageStatus(StageStatusType::RUNNING);
}

---

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

• modules/planning/scenarios/free_space/stage_free_space.h
• modules/planning/scenarios/free_space/stage_free_space.cc