hybrid_a_star.h

浏览该文件的文档.

/******************************************************************************
 * Copyright 2018 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 <algorithm>
#include <memory>
#include <queue>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>
 
#include "modules/common_msgs/config_msgs/vehicle_config.pb.h"
#include "modules/planning/planning_open_space/proto/planner_open_space_config.pb.h"
 
#include "cyber/common/log.h"
#include "cyber/common/macros.h"
#include "cyber/time/clock.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/math_utils.h"
#include "modules/planning/planning_base/common/obstacle.h"
#include "modules/planning/planning_base/gflags/planning_gflags.h"
#include "modules/planning/planning_open_space/coarse_trajectory_generator/grid_search.h"
#include "modules/planning/planning_open_space/coarse_trajectory_generator/node3d.h"
#include "modules/planning/planning_open_space/coarse_trajectory_generator/reeds_shepp_path.h"
 
namespace apollo {
namespace planning {
 
struct HybridAStartResult {
    std::vector<double> x;
    std::vector<double> y;
    std::vector<double> phi;
    std::vector<double> v;
    std::vector<double> a;
    std::vector<double> steer;
    std::vector<double> accumulated_s;
};
 
class HybridAStar {
public:
    explicit HybridAStar(const PlannerOpenSpaceConfig& open_space_conf);
    virtual ~HybridAStar() = default;
    bool TrajectoryPartition(const HybridAStartResult& result, std::vector<HybridAStartResult>* partitioned_result);
 
private:
    bool AnalyticExpansion(std::shared_ptr<Node3d> current_node, std::shared_ptr<Node3d>* candidate_final_node);
    // check collision and validity
    bool ValidityCheck(std::shared_ptr<Node3d> node);
    // check Reeds Shepp path collision and validity
    bool RSPCheck(const std::shared_ptr<ReedSheppPath> reeds_shepp_to_end);
    // load the whole RSP as nodes and add to the close set
    std::shared_ptr<Node3d> LoadRSPinCS(
            const std::shared_ptr<ReedSheppPath> reeds_shepp_to_end,
            std::shared_ptr<Node3d> current_node);
    std::shared_ptr<Node3d> Next_node_generator(std::shared_ptr<Node3d> current_node, size_t next_node_index);
    void CalculateNodeCost(std::shared_ptr<Node3d> current_node, std::shared_ptr<Node3d> next_node);
    double TrajCost(std::shared_ptr<Node3d> current_node, std::shared_ptr<Node3d> next_node);
    double HoloObstacleHeuristic(std::shared_ptr<Node3d> next_node);
    bool GetResult(HybridAStartResult* result);
    bool GetTemporalProfile(HybridAStartResult* result);
    bool GenerateSpeedAcceleration(HybridAStartResult* result);
    bool GenerateSCurveSpeedAcceleration(HybridAStartResult* result);
 
private:
    PlannerOpenSpaceConfig planner_open_space_config_;
    common::VehicleParam vehicle_param_ = common::VehicleConfigHelper::GetConfig().vehicle_param();
    size_t next_node_num_ = 0;
    double max_steer_angle_ = 0.0;
    double max_kappa_ = 0.0;
    double step_size_ = 0.0;
    double xy_grid_resolution_ = 0.0;
    double delta_t_ = 0.0;
    double traj_forward_penalty_ = 0.0;
    double traj_back_penalty_ = 0.0;
    double traj_gear_switch_penalty_ = 0.0;
    double traj_steer_penalty_ = 0.0;
    double traj_steer_change_penalty_ = 0.0;
    double soft_boundary_penalty_ = 0.0;
    double heu_rs_forward_penalty_ = 0.0;
    double heu_rs_back_penalty_ = 0.0;
    double heu_rs_gear_switch_penalty_ = 0.0;
    double heu_rs_steer_penalty_ = 0.0;
    double heu_rs_steer_change_penalty_ = 0.0;
    double acc_weight_ = 0.0;
    double jerk_weight_ = 0.0;
    double kappa_penalty_weight_ = 0.0;
    double ref_s_weight_ = 0.0;
    double ref_v_weight_ = 0.0;
    double max_forward_v_ = 0.0;
    double max_reverse_v_ = 0.0;
    double max_forward_acc_ = 0.0;
    double max_reverse_acc_ = 0.0;
    double max_acc_jerk_ = 0.0;
    double arc_length_ = 0.0;
    std::vector<double> XYbounds_;
    std::shared_ptr<Node3d> start_node_;
    std::shared_ptr<Node3d> end_node_;
    std::shared_ptr<Node3d> final_node_;
    std::vector<std::vector<common::math::LineSegment2d>> obstacles_linesegments_vec_;
 
    struct cmp {
        bool operator()(
                const std::pair<std::shared_ptr<Node3d>, double>& left,
                const std::pair<std::shared_ptr<Node3d>, double>& right) const {
            return left.second >= right.second;
        }
    };
    std::priority_queue<
            std::pair<std::shared_ptr<Node3d>, double>,
            std::vector<std::pair<std::shared_ptr<Node3d>, double>>,
            cmp>
            open_pq_;
    std::unordered_set<std::string> open_set_;
    std::unordered_set<std::string> close_set_;
    std::unique_ptr<ReedShepp> reed_shepp_generator_;
    std::unique_ptr<GridSearch> grid_a_star_heuristic_generator_;
 
    // park generic
public:
    explicit HybridAStar(const WarmStartConfig& open_space_conf);
 
    bool Plan(
            double sx,
            double sy,
            double sphi,
            double ex,
            double ey,
            double ephi,
            const std::vector<double>& XYbounds,
            const std::vector<std::vector<common::math::Vec2d>>& obstacles_vertices_vec,
            HybridAStartResult* result,
            const std::vector<std::vector<common::math::Vec2d>>& soft_boundary_vertices_vec = {},
            bool reeds_sheep_last_straight = false);
 
private:
    double GetSoftBoundaryCost(const std::shared_ptr<ReedSheppPath>& reeds_shepp_to_end);
    bool RSPLengthCheck(const std::shared_ptr<ReedSheppPath> reeds_shepp_path);
 
    WarmStartConfig planner_warm_start_config_;
    std::vector<std::vector<common::math::LineSegment2d>> soft_boundary_linesegments_vec_;
    double traj_expected_shortest_length_ = 0.0;
    double traj_short_length_penalty_ = 0.0;
};
 
}  // namespace planning
}  // namespace apollo