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 第 114 行定义.

构造及析构函数说明

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

成员函数说明

CheckDpMap()

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

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

                                                              {
  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 第 107 行定义.

                              {
  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 第 170 行定义.

                                            {
  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);
        }
      }
    }
  }
  ADEBUG << "explored node num is " << explored_node_num;
  return true;
}

GetObstacleDistance()

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

---

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

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