grid_search.h

浏览该文件的文档.

/******************************************************************************
 * Copyright 2019 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
 
/*
 * @file
 */
 
#pragma once
 
#include <limits>
#include <memory>
#include <queue>
#include <string>
#include <unordered_map>
#include <utility>
#include <unordered_set>
#include <vector>
 
#include "absl/strings/str_cat.h"
 
#include "modules/planning/planning_open_space/proto/planner_open_space_config.pb.h"
 
#include "cyber/common/log.h"
#include "modules/common/math/line_segment2d.h"
 
namespace apollo {
namespace planning {
 
class Node2d {
public:
    Node2d(const double x, const double y, const double xy_resolution, const std::vector<double>& XYbounds) {
        // XYbounds with xmin, xmax, ymin, ymax
        grid_x_ = static_cast<int>((x - XYbounds[0]) / xy_resolution);
        grid_y_ = static_cast<int>((y - XYbounds[2]) / xy_resolution);
        index_ = ComputeStringIndex(grid_x_, grid_y_);
    }
    Node2d(const int grid_x, const int grid_y, const std::vector<double>& XYbounds) {
        grid_x_ = grid_x;
        grid_y_ = grid_y;
        index_ = ComputeStringIndex(grid_x_, grid_y_);
    }
    void SetPathCost(const double path_cost) {
        path_cost_ = path_cost;
        cost_ = path_cost_ + heuristic_;
    }
    void SetHeuristic(const double heuristic) {
        heuristic_ = heuristic;
        cost_ = path_cost_ + heuristic_;
    }
    void SetCost(const double cost) {
        cost_ = cost;
    }
    void SetDistanceToObstacle(const double dist) {
        distance_to_obstacle_ = dist;
    }
    void SetPreNode(std::shared_ptr<Node2d> pre_node) {
        pre_node_ = pre_node;
    }
    double GetGridX() const {
        return grid_x_;
    }
    double GetGridY() const {
        return grid_y_;
    }
    double GetPathCost() const {
        return path_cost_;
    }
    double GetHeuCost() const {
        return heuristic_;
    }
    double GetCost() const {
        return cost_;
    }
    double GetDistanceToObstacle() const {
        return distance_to_obstacle_;
    }
    const std::string& GetIndex() const {
        return index_;
    }
    std::shared_ptr<Node2d> GetPreNode() const {
        return pre_node_;
    }
    static std::string CalcIndex(
            const double x,
            const double y,
            const double xy_resolution,
            const std::vector<double>& XYbounds) {
        // XYbounds with xmin, xmax, ymin, ymax
        int grid_x = static_cast<int>((x - XYbounds[0]) / xy_resolution);
        int grid_y = static_cast<int>((y - XYbounds[2]) / xy_resolution);
        return ComputeStringIndex(grid_x, grid_y);
    }
    bool operator==(const Node2d& right) const {
        return right.GetIndex() == index_;
    }
 
private:
    static std::string ComputeStringIndex(int x_grid, int y_grid) {
        return absl::StrCat(x_grid, "_", y_grid);
    }
 
private:
    int grid_x_ = 0;
    int grid_y_ = 0;
    double path_cost_ = 0.0;
    double heuristic_ = 0.0;
    double cost_ = 0.0;
    double distance_to_obstacle_ = std::numeric_limits<double>::max();
    std::string index_;
    std::shared_ptr<Node2d> pre_node_ = nullptr;
};
 
struct GridAStartResult {
    std::vector<double> x;
    std::vector<double> y;
    double path_cost = 0.0;
};
 
class GridSearch {
public:
    explicit 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);
 
private:
    double EuclidDistance(const double x1, const double y1, const double x2, const double y2);
    std::vector<std::shared_ptr<Node2d>> GenerateNextNodes(std::shared_ptr<Node2d> node);
    bool CheckConstraints(std::shared_ptr<Node2d> node);
    void LoadGridAStarResult(GridAStartResult* result);
 
private:
    double xy_grid_resolution_ = 0.0;
    double node_radius_ = 0.0;
    std::vector<double> XYbounds_;
    double max_grid_x_ = 0.0;
    double max_grid_y_ = 0.0;
    std::shared_ptr<Node2d> start_node_;
    std::shared_ptr<Node2d> end_node_;
    std::shared_ptr<Node2d> final_node_;
    std::vector<std::vector<common::math::LineSegment2d>> obstacles_linesegments_vec_;
 
    struct cmp {
        bool operator()(const std::pair<std::string, double>& left, const std::pair<std::string, double>& right) const {
            return left.second >= right.second;
        }
    };
    std::unordered_map<std::string, std::shared_ptr<Node2d>> dp_map_;
 
    // park generic
public:
    explicit GridSearch(const WarmStartConfig& warm_start_conf);
    double GetObstacleDistance(const double x, const double y);
 
private:
    void AddSoftCost(const std::vector<std::vector<common::math::LineSegment2d>>& soft_boundary);
    void ExtendNode(
            const int& x,
            const int& y,
            common::math::Vec2d origin_index,
            std::unordered_set<std::string>& close_set);
    std::vector<std::vector<common::math::LineSegment2d>> soft_boundary_linesegments_vec_;
    double esdf_range_ = 0.0;
    bool use_esdf_ = false;
    std::vector<int> dx_{1, 0, -1, 0};
    std::vector<int> dy_{0, -1, 0, 1};
};
}  // namespace planning
}  // namespace apollo