agv_ros_ws/my_planner/include/hybrid_A_star/rs_path.h

/*******************************************************************************
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2022 Zhang Zhimeng
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
 * SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
 * WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/

#ifndef HYBRID_A_STAR_RS_PATH_H     
#define HYBRID_A_STAR_RS_PATH_H

#include "hybrid_A_star/type.h"

#include <cmath>
#include <limits>
#include <vector>
#include <Eigen/Core>


/*!
 * Refer to this paper:
 *   1990 Optimal paths for a car that goes both forwards and backwards. J. A. Reeds, L. A. Shepp. Pacific J.
 *
 * Notes: There are many notation errors in this paper, and there are many formula derivation errors.
 *        When reading the paper, please note that the errors! Errors mainly concentrated in Section 8.
 */
class RSPath {
public:
    // double turning_radius_ = 1.0;

    // RSPath() = delete;

    // explicit RSPath(double turning_radius = 1.0);

    enum RSPathSegmentType {
        N = 0, L = 1, S = 2, R = 3
    };
    static const RSPathSegmentType RS_path_segment_type[18][5];

    struct RSPathData {
    public:
        explicit RSPathData(const RSPathSegmentType *type = RS_path_segment_type[0],
                            double t = std::numeric_limits<double>::max(),
                            double u = 0.0, double v = 0.0, double w = 0.0, double x = 0.0) : type_(type) {
            length_[0] = t;
            length_[1] = u;
            length_[2] = v;
            length_[3] = w;
            length_[4] = x;
            total_length_ = std::fabs(length_[0]) + std::fabs(length_[1]) + std::fabs(length_[2])
                            + std::fabs(length_[3]) + std::fabs(length_[4]);
        }

        double Length() const {
            return total_length_;
        }

    public:
        double length_[5]{};
        const RSPathSegmentType *type_;

    private:
        double total_length_;
    };

    /*!
     * Calculate the actual distance from (x_0, y_0, yaw_0) to (x_1, y_1, yaw_1)
     * @param x_0
     * @param y_0
     * @param yaw_0
     * @param x_1
     * @param y_1
     * @param yaw_1
     * @return
     */
    double Distance(double x_0, double y_0, double yaw_0,
                    double x_1, double y_1, double yaw_1);

    /*!
     * Get a reed shepp path
     * @param start_state start state including position and yaw
     * @param goal_state goal state including position and yaw
     * @param step_size actual step size for collision detection
     * @return Discrete points of reed shepp path, including position and yaw
     */
    TypeVectorVecd<3> GetRSPath(const Eigen::Vector3d &start_state, const Eigen::Vector3d &goal_state, double step_size, double &length);

    RSPathData GetRSPath(double x_0, double y_0, double yaw_0,
                         double x_1, double y_1, double yaw_1);

    RSPathData GetRSPath(double x, double y, double phi);

private:

    static void CSC(double x, double y, double phi, RSPathData &path);

    void CCC(double x, double y, double phi, RSPathData &path);

    static void CCCC(double x, double y, double phi, RSPathData &path);

    static void CCSC(double x, double y, double phi, RSPathData &path);

    static void CCSCC(double x, double y, double phi, RSPathData &path);

    static inline double Mod2Pi(double x);

    static inline void Polar(double x, double y, double &r, double &theta);

    static inline void TauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega);

    // Paper P390, formula 8.1
    static inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v);

    // Paper P390, formula 8.2
    static inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v);

    // Paper P390 formula 8.3 and 8.4.
    // There is an error in the deduction of the formula in the paper.
    // It can be deduced by itself according to the inscribed circle.
    static inline bool LpRmL(double x, double y, double phi, double &t, double &u, double &v);

    // Paper P391 formula 8.7
    static inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v);

    // Paper P391 formula 8.8
    static inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v);

    // Paper P391 formula 8.9
    static inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v);

    // Paper P391 formula 8.10
    static inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v);

    // Paper P391 formula 8.11
    // There is an error in the deduction of the formula in the paper.
    // It can be deduced by itself according to the inscribed circle.
    static inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v);

// private:
    // double turning_radius_ = 1.0;
};

#endif //HYBRID_A_STAR_RS_PATH_H