apollo::planning::HybridAStar类 参考

#include <hybrid_a_star.h>

apollo::planning::HybridAStar 的协作图:

Public 成员函数
	HybridAStar (const PlannerOpenSpaceConfig &open_space_conf)
virtual	~HybridAStar ()=default
bool	TrajectoryPartition (const HybridAStartResult &result, std::vector< HybridAStartResult > *partitioned_result)
	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)

详细描述

在文件 hybrid_a_star.h 第 59 行定义.

构造及析构函数说明

HybridAStar() [1/2]

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

explicit

在文件 hybrid_a_star.cc 第 38 行定义.

                                                                      {
    planner_open_space_config_.CopyFrom(open_space_conf);
    reed_shepp_generator_ = std::make_unique<ReedShepp>(vehicle_param_, planner_open_space_config_);
    grid_a_star_heuristic_generator_ = std::make_unique<GridSearch>(planner_open_space_config_);
    next_node_num_ = planner_open_space_config_.warm_start_config().next_node_num();
    max_steer_angle_ = vehicle_param_.max_steer_angle() / vehicle_param_.steer_ratio()
            * planner_open_space_config_.warm_start_config().traj_kappa_contraint_ratio();
    step_size_ = planner_open_space_config_.warm_start_config().step_size();
    xy_grid_resolution_ = planner_open_space_config_.warm_start_config().xy_grid_resolution();
    arc_length_ = planner_open_space_config_.warm_start_config().phi_grid_resolution() * vehicle_param_.wheel_base()
            / std::tan(max_steer_angle_ * 2 / (next_node_num_ / 2 - 1));
    if (arc_length_ < std::sqrt(2) * xy_grid_resolution_) {
        arc_length_ = std::sqrt(2) * xy_grid_resolution_;
    }
    AINFO << "arc_length" << arc_length_;
    delta_t_ = planner_open_space_config_.delta_t();
    traj_forward_penalty_ = planner_open_space_config_.warm_start_config().traj_forward_penalty();
    traj_back_penalty_ = planner_open_space_config_.warm_start_config().traj_back_penalty();
    traj_gear_switch_penalty_ = planner_open_space_config_.warm_start_config().traj_gear_switch_penalty();
    traj_steer_penalty_ = planner_open_space_config_.warm_start_config().traj_steer_penalty();
    traj_steer_change_penalty_ = planner_open_space_config_.warm_start_config().traj_steer_change_penalty();
    acc_weight_ = planner_open_space_config_.iterative_anchoring_smoother_config().s_curve_config().acc_weight();
    jerk_weight_ = planner_open_space_config_.iterative_anchoring_smoother_config().s_curve_config().jerk_weight();
    kappa_penalty_weight_
            = planner_open_space_config_.iterative_anchoring_smoother_config().s_curve_config().kappa_penalty_weight();
    ref_s_weight_ = planner_open_space_config_.iterative_anchoring_smoother_config().s_curve_config().ref_s_weight();
    ref_v_weight_ = planner_open_space_config_.iterative_anchoring_smoother_config().s_curve_config().ref_v_weight();
    max_forward_v_ = planner_open_space_config_.iterative_anchoring_smoother_config().max_forward_v();
    max_reverse_v_ = planner_open_space_config_.iterative_anchoring_smoother_config().max_reverse_v();
    max_forward_acc_ = planner_open_space_config_.iterative_anchoring_smoother_config().max_forward_acc();
    max_reverse_acc_ = planner_open_space_config_.iterative_anchoring_smoother_config().max_reverse_acc();
    max_acc_jerk_ = planner_open_space_config_.iterative_anchoring_smoother_config().max_acc_jerk();
}

~HybridAStar()

virtual apollo::planning::HybridAStar::~HybridAStar ( )

virtualdefault

HybridAStar() [2/2]

apollo::planning::HybridAStar::HybridAStar ( const WarmStartConfig &  open_space_conf )

explicit

在文件 hybrid_a_star.cc 第 72 行定义.

                                                               {
    planner_warm_start_config_.CopyFrom(warm_start_conf);
    reed_shepp_generator_ = std::make_unique<ReedShepp>(vehicle_param_, planner_warm_start_config_);
    grid_a_star_heuristic_generator_ = std::make_unique<GridSearch>(planner_warm_start_config_);
    next_node_num_ = planner_warm_start_config_.next_node_num();
    max_steer_angle_ = vehicle_param_.max_steer_angle() / vehicle_param_.steer_ratio()
            * planner_warm_start_config_.traj_kappa_contraint_ratio();
    max_kappa_ = std::tan(max_steer_angle_) / vehicle_param_.wheel_base();
    step_size_ = planner_warm_start_config_.step_size();
    xy_grid_resolution_ = planner_warm_start_config_.xy_grid_resolution();
    arc_length_ = std::sqrt(2) * xy_grid_resolution_;
    AINFO << "arc_length" << arc_length_;
    delta_t_ = FLAGS_open_space_delta_t;
    traj_forward_penalty_ = planner_warm_start_config_.traj_forward_penalty();
    traj_back_penalty_ = planner_warm_start_config_.traj_back_penalty();
    traj_gear_switch_penalty_ = planner_warm_start_config_.traj_gear_switch_penalty();
    traj_steer_penalty_ = planner_warm_start_config_.traj_steer_penalty();
    traj_steer_change_penalty_ = planner_warm_start_config_.traj_steer_change_penalty();
    soft_boundary_penalty_ = planner_warm_start_config_.soft_boundary_penalty();
    acc_weight_ = FLAGS_open_space_acc_weight;
    jerk_weight_ = FLAGS_open_space_jerk_weight;
    kappa_penalty_weight_ = FLAGS_open_space_kappa_weight;
    ref_s_weight_ = FLAGS_open_space_reference_s_weight;
    ref_v_weight_ = FLAGS_open_space_reference_v_weight;
    max_forward_v_ = FLAGS_open_space_max_forward_v;
    max_reverse_v_ = FLAGS_open_space_max_reverse_v;
    max_forward_acc_ = FLAGS_open_space_max_forward_acc;
    max_reverse_acc_ = FLAGS_open_space_max_reverse_acc;
    max_acc_jerk_ = FLAGS_open_space_max_jerk;
    traj_expected_shortest_length_ = planner_warm_start_config_.traj_expected_shortest_length();
    traj_short_length_penalty_ = planner_warm_start_config_.traj_short_length_penalty();
}

成员函数说明

Plan()

bool apollo::planning::HybridAStar::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
	)

在文件 hybrid_a_star.cc 第 693 行定义.

                                        {
    reed_shepp_generator_->reeds_sheep_last_straight_ = reeds_sheep_last_straight;
    // clear containers
    open_set_.clear();
    close_set_.clear();
    open_pq_ = decltype(open_pq_)();
    final_node_ = nullptr;
    PrintCurves print_curves;
    std::vector<std::vector<common::math::LineSegment2d>> obstacles_linesegments_vec;
    for (const auto& obstacle_vertices : obstacles_vertices_vec) {
        size_t vertices_num = obstacle_vertices.size();
        std::vector<common::math::LineSegment2d> obstacle_linesegments;
        for (size_t i = 0; i < vertices_num - 1; ++i) {
            common::math::LineSegment2d line_segment
                    = common::math::LineSegment2d(obstacle_vertices[i], obstacle_vertices[i + 1]);
            obstacle_linesegments.emplace_back(line_segment);
        }
        obstacles_linesegments_vec.emplace_back(obstacle_linesegments);
    }
    obstacles_linesegments_vec_ = std::move(obstacles_linesegments_vec);
    std::vector<std::vector<common::math::LineSegment2d>> soft_boundary_linesegments_vec;
    for (const auto& soft_boundary_vertices : soft_boundary_vertices_vec) {
        size_t vertices_num = soft_boundary_vertices.size();
        std::vector<common::math::LineSegment2d> soft_boundary_linesegments;
        for (size_t i = 0; i < vertices_num - 1; ++i) {
            common::math::LineSegment2d line_segment
                    = common::math::LineSegment2d(soft_boundary_vertices[i], soft_boundary_vertices[i + 1]);
            soft_boundary_linesegments.emplace_back(line_segment);
        }
        soft_boundary_linesegments_vec.emplace_back(soft_boundary_linesegments);
    }
    soft_boundary_linesegments_vec_ = std::move(soft_boundary_linesegments_vec);
    for (size_t i = 0; i < obstacles_linesegments_vec_.size(); i++) {
        for (auto linesg : obstacles_linesegments_vec_[i]) {
            std::string name = std::to_string(i) + "roi_boundary";
            print_curves.AddPoint(name, linesg.start());
            print_curves.AddPoint(name, linesg.end());
        }
    }
    Vec2d sposition(sx, sy);
    Vec2d svec_to_center(
            (vehicle_param_.front_edge_to_center() - vehicle_param_.back_edge_to_center()) / 2.0,
            (vehicle_param_.left_edge_to_center() - vehicle_param_.right_edge_to_center()) / 2.0);
    Vec2d scenter(sposition + svec_to_center.rotate(sphi));
    Box2d sbox(scenter, sphi, vehicle_param_.length(), vehicle_param_.width());
    print_curves.AddPoint("vehicle_start_box", sbox.GetAllCorners());
    Vec2d eposition(ex, ey);
    print_curves.AddPoint("end_position", eposition);
    Vec2d evec_to_center(
            (vehicle_param_.front_edge_to_center() - vehicle_param_.back_edge_to_center()) / 2.0,
            (vehicle_param_.left_edge_to_center() - vehicle_param_.right_edge_to_center()) / 2.0);
    Vec2d ecenter(eposition + evec_to_center.rotate(ephi));
    Box2d ebox(ecenter, ephi, vehicle_param_.length(), vehicle_param_.width());
    print_curves.AddPoint("vehicle_end_box", ebox.GetAllCorners());
    XYbounds_ = XYbounds;
    // load nodes and obstacles
    start_node_.reset(new Node3d({sx}, {sy}, {sphi}, XYbounds_, planner_warm_start_config_));
    end_node_.reset(new Node3d({ex}, {ey}, {ephi}, XYbounds_, planner_warm_start_config_));
    AINFO << "start node" << sx << "," << sy << "," << sphi;
    AINFO << "end node " << ex << "," << ey << "," << ephi;
    if (!ValidityCheck(start_node_)) {
        AERROR << "start_node in collision with obstacles";
        AERROR << start_node_->GetX() << "," << start_node_->GetY() << "," << start_node_->GetPhi();
        print_curves.PrintToLog();
        return false;
    }
    if (!ValidityCheck(end_node_)) {
        AERROR << "end_node in collision with obstacles";
        print_curves.PrintToLog();
        return false;
    }
    double map_time = Clock::NowInSeconds();
    grid_a_star_heuristic_generator_->GenerateDpMap(
            ex, ey, XYbounds_, obstacles_linesegments_vec_, soft_boundary_linesegments_vec_);
    ADEBUG << "map time " << Clock::NowInSeconds() - map_time;
    // load open set, pq
    open_set_.insert(start_node_->GetIndex());
    open_pq_.emplace(start_node_, start_node_->GetCost());
    // Hybrid A* begins
    size_t explored_node_num = 0;
    size_t available_result_num = 0;
    auto best_explored_num = explored_node_num;
    auto best_available_result_num = available_result_num;
    double astar_start_time = Clock::NowInSeconds();
    double heuristic_time = 0.0;
    double rs_time = 0.0;
    double node_generator_time = 0.0;
    double validity_check_time = 0.0;
    size_t max_explored_num = planner_warm_start_config_.max_explored_num();
    size_t desired_explored_num = std::min(
            planner_warm_start_config_.desired_explored_num(), planner_warm_start_config_.max_explored_num());
    static constexpr int kMaxNodeNum = 200000;
    std::vector<std::shared_ptr<Node3d>> candidate_final_nodes;
    while (!open_pq_.empty() && open_pq_.size() < kMaxNodeNum && available_result_num < desired_explored_num
           && explored_node_num < max_explored_num) {
        std::shared_ptr<Node3d> current_node = open_pq_.top().first;
        if (current_node->GetPreNode() != nullptr) {
            AINFO << "print_open_pq:" << "(" << current_node->GetPreNode()->GetX() << ", "
                  << current_node->GetPreNode()->GetY() << ")" << ";" << "(" << current_node->GetX() << ", "
                  << current_node->GetY() << ");";
        }
        open_pq_.pop();
        const double rs_start_time = Clock::NowInSeconds();
        std::shared_ptr<Node3d> final_node = nullptr;
        if (AnalyticExpansion(current_node, &final_node)) {
            if (final_node_ == nullptr || final_node_->GetTrajCost() > final_node->GetTrajCost()) {
                final_node_ = final_node;
                best_explored_num = explored_node_num + 1;
                best_available_result_num = available_result_num + 1;
            }
            available_result_num++;
        }
        explored_node_num++;
        const double rs_end_time = Clock::NowInSeconds();
        rs_time += rs_end_time - rs_start_time;
        close_set_.insert(current_node->GetIndex());
 
        if (Clock::NowInSeconds() - astar_start_time > planner_warm_start_config_.astar_max_search_time()
            && available_result_num > 0) {
            break;
        }
 
        size_t begin_index = 0;
        size_t end_index = next_node_num_;
        std::unordered_set<std::string> temp_set;
        for (size_t i = begin_index; i < end_index; ++i) {
            const double gen_node_time = Clock::NowInSeconds();
            std::shared_ptr<Node3d> next_node = Next_node_generator(current_node, i);
            node_generator_time += Clock::NowInSeconds() - gen_node_time;
 
            // boundary check failure handle
            if (next_node == nullptr) {
                continue;
            }
            // check if the node is already in the close set
            if (close_set_.count(next_node->GetIndex()) > 0) {
                continue;
            }
            // collision check
            const double validity_check_start_time = Clock::NowInSeconds();
            if (!ValidityCheck(next_node)) {
                continue;
            }
            validity_check_time += Clock::NowInSeconds() - validity_check_start_time;
            if (open_set_.count(next_node->GetIndex()) == 0) {
                const double start_time = Clock::NowInSeconds();
                CalculateNodeCost(current_node, next_node);
                const double end_time = Clock::NowInSeconds();
                heuristic_time += end_time - start_time;
                temp_set.insert(next_node->GetIndex());
                open_pq_.emplace(next_node, next_node->GetCost());
            }
        }
        open_set_.insert(temp_set.begin(), temp_set.end());
    }
 
    if (final_node_ == nullptr) {
        AERROR << "Hybird A* cannot find a valid path";
        print_curves.PrintToLog();
        return false;
    }
 
    AINFO << "open_pq_.empty()" << (open_pq_.empty() ? "true" : "false");
    AINFO << "open_pq_.size()" << open_pq_.size();
    AINFO << "desired_explored_num" << desired_explored_num;
    AINFO << "min cost is : " << final_node_->GetTrajCost();
    AINFO << "max_explored_num is " << max_explored_num;
    AINFO << "explored node num is " << explored_node_num << "available_result_num " << available_result_num;
    AINFO << "best_explored_num is " << best_explored_num << "best_available_result_num is "
          << best_available_result_num;
    AINFO << "cal node time is " << heuristic_time << "validity_check_time " << validity_check_time
          << "node_generator_time " << node_generator_time;
    AINFO << "reed shepp time is " << rs_time;
    AINFO << "hybrid astar total time is " << Clock::NowInSeconds() - astar_start_time;
 
    print_curves.AddPoint("rs_point", final_node_->GetXs().front(), final_node_->GetYs().front());
    if (!GetResult(result)) {
        AERROR << "GetResult failed";
        print_curves.PrintToLog();
        return false;
    }
    for (size_t i = 0; i < result->x.size(); i++) {
        print_curves.AddPoint("warm_path", result->x[i], result->y[i]);
    }
    // PrintBox print_box("warm_path_box");
    // for (size_t i = 0; i < result->x.size(); i = i + 5) {
    //   print_box.AddAdcBox(result->x[i], result->y[i], result->phi[i]);
    // }
    // print_box.PrintToLog();
    print_curves.PrintToLog();
    return true;
}

TrajectoryPartition()

bool apollo::planning::HybridAStar::TrajectoryPartition	(	const HybridAStartResult &	result,
		std::vector< HybridAStartResult > *	partitioned_result
	)

在文件 hybrid_a_star.cc 第 603 行定义.

                                                           {
    const auto& x = result.x;
    const auto& y = result.y;
    const auto& phi = result.phi;
    if (x.size() != y.size() || x.size() != phi.size()) {
        AERROR << "states sizes are not equal when do trajectory partitioning of "
                  "Hybrid A Star result";
        return false;
    }
 
    size_t horizon = x.size();
    partitioned_result->clear();
    partitioned_result->emplace_back();
    auto* current_traj = &(partitioned_result->back());
    double heading_angle = phi.front();
    const Vec2d init_tracking_vector(x[1] - x[0], y[1] - y[0]);
    double tracking_angle = init_tracking_vector.Angle();
    bool current_gear = std::abs(common::math::NormalizeAngle(tracking_angle - heading_angle)) < (M_PI_2);
    AINFO << x[0] << " " << y[0] << " " << phi[0];
    for (size_t i = 0; i < horizon - 1; ++i) {
        heading_angle = phi[i];
        const Vec2d tracking_vector(x[i + 1] - x[i], y[i + 1] - y[i]);
        tracking_angle = tracking_vector.Angle();
        bool gear = std::abs(common::math::NormalizeAngle(tracking_angle - heading_angle)) < (M_PI_2);
        if (gear != current_gear) {
            current_traj->x.push_back(x[i]);
            current_traj->y.push_back(y[i]);
            current_traj->phi.push_back(phi[i]);
            partitioned_result->emplace_back();
            current_traj = &(partitioned_result->back());
            current_gear = gear;
        }
        current_traj->x.push_back(x[i]);
        current_traj->y.push_back(y[i]);
        current_traj->phi.push_back(phi[i]);
    }
    AINFO << x.back() << " " << y.back() << " " << phi.back();
    current_traj->x.push_back(x.back());
    current_traj->y.push_back(y.back());
    current_traj->phi.push_back(phi.back());
 
    const auto start_timestamp = std::chrono::system_clock::now();
 
    // Retrieve v, a and steer from path
    for (auto& result : *partitioned_result) {
        if (FLAGS_use_s_curve_speed_smooth) {
            if (!GenerateSCurveSpeedAcceleration(&result)) {
                AERROR << "GenerateSCurveSpeedAcceleration fail";
                return false;
            }
        } else {
            if (!GenerateSpeedAcceleration(&result)) {
                AERROR << "GenerateSpeedAcceleration fail";
                return false;
            }
        }
    }
 
    const auto end_timestamp = std::chrono::system_clock::now();
    std::chrono::duration<double> diff = end_timestamp - start_timestamp;
    ADEBUG << "speed profile total time: " << diff.count() * 1000.0 << " ms.";
    return true;
}

---

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

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