reeds_shepp_path.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 <limits>
#include <memory>
#include <string>
#include <utility>
#include <vector>
 
#include <omp.h>
 
#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 "modules/common/math/math_utils.h"
#include "modules/planning/planning_base/gflags/planning_gflags.h"
#include "modules/planning/planning_open_space/coarse_trajectory_generator/node3d.h"
 
namespace apollo {
namespace planning {
 
struct ReedSheppPath {
    std::vector<double> segs_lengths;
    std::vector<char> segs_types;
    double total_length = 0.0;
    double cost = 0.0;
    std::vector<double> x;
    std::vector<double> y;
    std::vector<double> phi;
    // true for driving forward and false for driving backward
    std::vector<bool> gear;
    double radius = 1.0;
};
 
struct RSPParam {
    bool flag = false;
    double t = 0.0;
    double u = 0.0;
    double v = 0.0;
};
 
class ReedShepp {
public:
    ReedShepp(const common::VehicleParam& vehicle_param, const PlannerOpenSpaceConfig& open_space_conf);
    virtual ~ReedShepp() = default;
    // Pick the shortest path from all possible
    // combination of movement primitives
    // by Reed Shepp
    bool ShortestRSP(
            const std::shared_ptr<Node3d> start_node,
            const std::shared_ptr<Node3d> end_node,
            std::shared_ptr<ReedSheppPath> optimal_path);
 
protected:
    // Generate all possible combination of
    // movement primitives by Reed Shepp and
    // interpolate them
    bool GenerateRSPs(
            const std::shared_ptr<Node3d> start_node,
            const std::shared_ptr<Node3d> end_node,
            std::vector<ReedSheppPath>* all_possible_paths);
    // Set the general profile of the movement primitives
    bool GenerateRSP(
            const std::shared_ptr<Node3d> start_node,
            const std::shared_ptr<Node3d> end_node,
            std::vector<ReedSheppPath>* all_possible_paths);
    // Set the general profile of the movement primitives,
    // parallel implementation
    bool GenerateRSPPar(
            const std::shared_ptr<Node3d> start_node,
            const std::shared_ptr<Node3d> end_node,
            std::vector<ReedSheppPath>* all_possible_paths);
    // Set local exact configurations profile of
    // each movement primitive
    bool GenerateLocalConfigurations(
            const std::shared_ptr<Node3d> start_node,
            const std::shared_ptr<Node3d> end_node,
            ReedSheppPath* shortest_path);
    // Interpolation usde in GenetateLocalConfiguration
    void Interpolation(
            const int index,
            const double pd,
            const char m,
            const double ox,
            const double oy,
            const double ophi,
            const double radius,
            std::vector<double>* px,
            std::vector<double>* py,
            std::vector<double>* pphi,
            std::vector<bool>* pgear);
    // motion primitives combination setup function
    bool SetRSP(
            const int size,
            const double* lengths,
            const char* types,
            std::vector<ReedSheppPath>* all_possible_paths,
            double radius = 1.0);
    // setRSP parallel version
    bool SetRSPPar(
            const int size,
            const double* lengths,
            const std::string& types,
            std::vector<ReedSheppPath>* all_possible_paths,
            const int idx);
    bool CS(const double x, const double y, const double phi, std::vector<ReedSheppPath>* all_possible_paths);
    // Six different combination of motion primitive in Reed Shepp path used in
    // GenerateRSP()
    bool SCS(const double x, const double y, const double phi, std::vector<ReedSheppPath>* all_possible_paths);
    bool CSC(const double x, const double y, const double phi, std::vector<ReedSheppPath>* all_possible_paths);
    bool CCC(const double x, const double y, const double phi, std::vector<ReedSheppPath>* all_possible_paths);
    bool CCCC(const double x, const double y, const double phi, std::vector<ReedSheppPath>* all_possible_paths);
    bool CCSC(const double x, const double y, const double phi, std::vector<ReedSheppPath>* all_possible_paths);
    bool CCSCC(const double x, const double y, const double phi, std::vector<ReedSheppPath>* all_possible_paths);
    // different options for different combination of motion primitives
    void LSL(const double x, const double y, const double phi, RSPParam* param);
    void LSR(const double x, const double y, const double phi, RSPParam* param);
    void LRL(const double x, const double y, const double phi, RSPParam* param);
    void SLS(const double x, const double y, const double phi, RSPParam* param);
    void LRLRn(const double x, const double y, const double phi, RSPParam* param);
    void LRLRp(const double x, const double y, const double phi, RSPParam* param);
    void LRSR(const double x, const double y, const double phi, RSPParam* param);
    void LRSL(const double x, const double y, const double phi, RSPParam* param);
    void LRSLR(const double x, const double y, const double phi, RSPParam* param);
    void LS(const double x, const double y, const double phi, RSPParam* param);
    std::pair<double, double> calc_tau_omega(
            const double u,
            const double v,
            const double xi,
            const double eta,
            const double phi);
 
protected:
    common::VehicleParam vehicle_param_;
    PlannerOpenSpaceConfig planner_open_space_config_;
    double max_kappa_;
    double traj_forward_penalty_;
    double traj_back_penalty_;
    double traj_gear_switch_penalty_;
    double traj_steer_penalty_;
    double traj_steer_change_penalty_;
    double traj_short_length_penalty_;
    double traj_expected_shortest_length_;
 
    // park generic
public:
    ReedShepp(const common::VehicleParam& vehicle_param, const WarmStartConfig& warm_start_conf);
 
    bool reeds_sheep_last_straight_ = false;
 
protected:
    WarmStartConfig planner_warm_start_config_;
};
}  // namespace planning
}  // namespace apollo