apollo::planning::GridSearch类 参考

#include <grid_search.h>

apollo::planning::GridSearch 的协作图:

Public 成员函数
	GridSearch (const PlannerOpenSpaceConfig &open_space_conf)
virtual	~GridSearch ()=default
bool	GenerateAStarPath (const double sx, const double sy, const double ex, const double ey, const std::vector< double > &XYbounds, const std::vector< std::vector< common::math::LineSegment2d > > &obstacles_linesegments_vec, GridAStartResult *result)
bool	GenerateDpMap (const double ex, const double ey, const std::vector< double > &XYbounds, const std::vector< std::vector< common::math::LineSegment2d > > &obstacles_linesegments_vec, const std::vector< std::vector< common::math::LineSegment2d > > &soft_boundary_linesegments_vec={{}})
double	CheckDpMap (const double sx, const double sy)
	GridSearch (const WarmStartConfig &warm_start_conf)
double	GetObstacleDistance (const double x, const double y)

详细描述

在文件 grid_search.h 第 132 行定义.

构造及析构函数说明

GridSearch() [1/2]

apollo::planning::GridSearch::GridSearch ( const PlannerOpenSpaceConfig &  open_space_conf )

explicit

在文件 grid_search.cc 第 28 行定义.

                                                                    {
    xy_grid_resolution_ = open_space_conf.warm_start_config().grid_a_star_xy_resolution();
    node_radius_ = open_space_conf.warm_start_config().node_radius();
}

~GridSearch()

virtual apollo::planning::GridSearch::~GridSearch ( )

virtualdefault

GridSearch() [2/2]

apollo::planning::GridSearch::GridSearch ( const WarmStartConfig &  warm_start_conf )

explicit

在文件 grid_search.cc 第 33 行定义.

                                                             {
    xy_grid_resolution_ = warm_start_conf.grid_a_star_xy_resolution();
    node_radius_ = warm_start_conf.node_radius();
    esdf_range_ = warm_start_conf.esdf_range();
    use_esdf_ = warm_start_conf.use_esdf();
}

成员函数说明

CheckDpMap()

double apollo::planning::GridSearch::CheckDpMap	(	const double	sx,
		const double	sy
	)

在文件 grid_search.cc 第 267 行定义.

                                                              {
    std::string index = Node2d::CalcIndex(sx, sy, xy_grid_resolution_, XYbounds_);
    if (dp_map_.find(index) != dp_map_.end()) {
        return dp_map_[index]->GetCost() * xy_grid_resolution_;
    } else {
        return std::numeric_limits<double>::infinity();
    }
}

GenerateAStarPath()

bool apollo::planning::GridSearch::GenerateAStarPath	(	const double	sx,
		const double	sy,
		const double	ex,
		const double	ey,
		const std::vector< double > &	XYbounds,
		const std::vector< std::vector< common::math::LineSegment2d > > &	obstacles_linesegments_vec,
		GridAStartResult *	result
	)

在文件 grid_search.cc 第 155 行定义.

                                  {
    std::priority_queue<std::pair<std::string, double>, std::vector<std::pair<std::string, double>>, cmp> open_pq;
    std::unordered_map<std::string, std::shared_ptr<Node2d>> open_set;
    std::unordered_map<std::string, std::shared_ptr<Node2d>> close_set;
    XYbounds_ = XYbounds;
    std::shared_ptr<Node2d> start_node = std::make_shared<Node2d>(sx, sy, xy_grid_resolution_, XYbounds_);
    std::shared_ptr<Node2d> end_node = std::make_shared<Node2d>(ex, ey, xy_grid_resolution_, XYbounds_);
    std::shared_ptr<Node2d> final_node_ = nullptr;
    obstacles_linesegments_vec_ = obstacles_linesegments_vec;
    open_set.emplace(start_node->GetIndex(), start_node);
    open_pq.emplace(start_node->GetIndex(), start_node->GetCost());
 
    // Grid a star begins
    size_t explored_node_num = 0;
    while (!open_pq.empty()) {
        std::string current_id = open_pq.top().first;
        open_pq.pop();
        std::shared_ptr<Node2d> current_node = open_set[current_id];
        // Check destination
        if (*(current_node) == *(end_node)) {
            final_node_ = current_node;
            break;
        }
        close_set.emplace(current_node->GetIndex(), current_node);
        std::vector<std::shared_ptr<Node2d>> next_nodes = std::move(GenerateNextNodes(current_node));
        for (auto& next_node : next_nodes) {
            if (!CheckConstraints(next_node)) {
                continue;
            }
            if (close_set.find(next_node->GetIndex()) != close_set.end()) {
                continue;
            }
            if (open_set.find(next_node->GetIndex()) == open_set.end()) {
                ++explored_node_num;
                next_node->SetHeuristic(EuclidDistance(
                        next_node->GetGridX(), next_node->GetGridY(), end_node->GetGridX(), end_node->GetGridY()));
                next_node->SetPreNode(current_node);
                open_set.emplace(next_node->GetIndex(), next_node);
                open_pq.emplace(next_node->GetIndex(), next_node->GetCost());
            }
        }
    }
 
    if (final_node_ == nullptr) {
        AERROR << "Grid A searching return null ptr(open_set ran out)";
        return false;
    }
    LoadGridAStarResult(result);
    ADEBUG << "explored node num is " << explored_node_num;
    return true;
}

GenerateDpMap()

bool apollo::planning::GridSearch::GenerateDpMap	(	const double	ex,
		const double	ey,
		const std::vector< double > &	XYbounds,
		const std::vector< std::vector< common::math::LineSegment2d > > &	obstacles_linesegments_vec,
		const std::vector< std::vector< common::math::LineSegment2d > > &	soft_boundary_linesegments_vec = {{}}
	)

在文件 grid_search.cc 第 214 行定义.

                                                                                             {
    std::priority_queue<std::pair<std::string, double>, std::vector<std::pair<std::string, double>>, cmp> open_pq;
    std::unordered_map<std::string, std::shared_ptr<Node2d>> open_set;
    dp_map_ = decltype(dp_map_)();
    XYbounds_ = XYbounds;
    // XYbounds with xmin, xmax, ymin, ymax
    max_grid_y_ = std::round((XYbounds_[3] - XYbounds_[2]) / xy_grid_resolution_);
    max_grid_x_ = std::round((XYbounds_[1] - XYbounds_[0]) / xy_grid_resolution_);
    std::shared_ptr<Node2d> end_node = std::make_shared<Node2d>(ex, ey, xy_grid_resolution_, XYbounds_);
    obstacles_linesegments_vec_ = obstacles_linesegments_vec;
    open_set.emplace(end_node->GetIndex(), end_node);
    open_pq.emplace(end_node->GetIndex(), end_node->GetCost());
 
    // Grid a star begins
    size_t explored_node_num = 0;
    while (!open_pq.empty()) {
        const std::string current_id = open_pq.top().first;
        open_pq.pop();
        std::shared_ptr<Node2d> current_node = open_set[current_id];
        dp_map_.emplace(current_node->GetIndex(), current_node);
        std::vector<std::shared_ptr<Node2d>> next_nodes = std::move(GenerateNextNodes(current_node));
        for (auto& next_node : next_nodes) {
            if (!CheckConstraints(next_node)) {
                continue;
            }
            if (dp_map_.find(next_node->GetIndex()) != dp_map_.end()) {
                continue;
            }
            if (open_set.find(next_node->GetIndex()) == open_set.end()) {
                ++explored_node_num;
                next_node->SetPreNode(current_node);
                open_set.emplace(next_node->GetIndex(), next_node);
                open_pq.emplace(next_node->GetIndex(), next_node->GetCost());
            } else {
                if (open_set[next_node->GetIndex()]->GetCost() > next_node->GetCost()) {
                    open_set[next_node->GetIndex()]->SetCost(next_node->GetCost());
                    open_set[next_node->GetIndex()]->SetPreNode(current_node);
                }
            }
        }
    }
    if (use_esdf_) {
        AddSoftCost(soft_boundary_linesegments_vec);
    }
    ADEBUG << "explored node num is " << explored_node_num;
    return true;
}

GetObstacleDistance()

double apollo::planning::GridSearch::GetObstacleDistance	(	const double	x,
		const double	y
	)

在文件 grid_search.cc 第 276 行定义.

                                                                     {
    std::string index = Node2d::CalcIndex(x, y, xy_grid_resolution_, XYbounds_);
    if (dp_map_.find(index) != dp_map_.end()) {
        return dp_map_[index]->GetDistanceToObstacle();
    } else {
        return 0.0;
    }
}

---

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

• modules/planning/planning_open_space/coarse_trajectory_generator/grid_search.h
• modules/planning/planning_open_space/coarse_trajectory_generator/grid_search.cc