agv_ros_ws/my_planner/include/combine_planning/rs_path.hpp

/*******************************************************************************
 * 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_HPP_COMBINE
#define HYBRID_A_STAR_RS_PATH_HPP_COMBINE


#include "combine_planning/rs_path.h"
#include <vector>
#include <Eigen/Core>
#include <glog/logging.h>

namespace combine_planning_ns
{

    double turning_radius_;

    void setTurningRadius(double R)
    {
        turning_radius_ = R;
    }

    // P 371: TABLE 1
    const RSPath::RSPathSegmentType RSPath::RS_path_segment_type[18][5] = {
        {L, R, L, N, N},
        {R, L, R, N, N},
        {L, R, L, R, N},
        {R, L, R, L, N},
        {L, R, S, L, N},
        {R, L, S, R, N},
        {L, S, R, L, N},
        {R, S, L, R, N},
        {L, R, S, R, N},
        {R, L, S, L, N},
        {R, S, R, L, N},
        {L, S, L, R, N},
        {L, S, R, N, N},
        {R, S, L, N, N},
        {L, S, L, N, N},
        {R, S, R, N, N},
        {L, R, S, L, R},
        {R, L, S, R, L}};

    // RSPath::RSPath(double turning_radius) : turning_radius_(turning_radius) {}

    double RSPath::Mod2Pi(double x)
    {
        double v = fmod(x, 2 * M_PI);

        if (v < -M_PI)
        {
            v += 2.0 * M_PI;
        }
        else if (v > M_PI)
        {
            v -= 2.0 * M_PI;
        }

        return v;
    }

    void RSPath::Polar(double x, double y, double &r, double &theta)
    {
        r = std::sqrt(x * x + y * y);
        theta = std::atan2(y, x);
    }

    void RSPath::TauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
    {
        double delta = Mod2Pi(u - v);
        double A = std::sin(u) - std::sin(delta);
        double B = std::cos(u) - std::cos(delta) - 1.0;
        double t1 = std::atan2(eta * A - xi * B, xi * A + eta * B);
        double t2 = 2.0 * (std::cos(delta) - std::cos(v) - std::cos(u)) + 3.0;
        tau = (t2 < 0.0) ? Mod2Pi(t1 + M_PI) : Mod2Pi(t1);
        omega = Mod2Pi(tau - u + v - phi);
    }

    bool RSPath::LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
    {
        Polar(x - std::sin(phi), y - 1.0 + std::cos(phi), u, t);

        if (t >= 0.0)
        {
            v = Mod2Pi(phi - t);
            if (v >= 0.0)
            {
                return true;
            }
        }

        return false;
    }

    bool RSPath::LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
    {
        double t1, u1;
        Polar(x + std::sin(phi), y - 1 - std::cos(phi), u1, t1);
        u1 = std::pow(u1, 2);

        if (u1 < 4.0)
        {
            return false;
        }

        double theta;
        u = std::sqrt(u1 - 4.0);
        theta = std::atan2(2.0, u);
        t = Mod2Pi(t1 + theta);
        v = Mod2Pi(t - phi);

        return true;
    }

    RSPath::RSPathData RSPath::GetRSPath(const double x_0, const double y_0, const double yaw_0,
                                         const double x_1, const double y_1, const double yaw_1)
    {
        // translation
        double dx = x_1 - x_0;
        double dy = y_1 - y_0;

        // rotate
        double c = std::cos(yaw_0); // 2d rotation matrix
        double s = std::sin(yaw_0);
        double x = c * dx + s * dy;
        double y = -s * dx + c * dy;
        double phi = yaw_1 - yaw_0;
        return GetRSPath(x / turning_radius_, y / turning_radius_, phi);
    }

    RSPath::RSPathData RSPath::GetRSPath(const double x, const double y, const double phi)
    {
        RSPathData path;
        CSC(x, y, phi, path);
        CCC(x, y, phi, path);
        CCCC(x, y, phi, path);
        CCSC(x, y, phi, path);
        CCSCC(x, y, phi, path);

        return path;
    }

    double RSPath::Distance(const double x_0, const double y_0, const double yaw_0,
                            const double x_1, const double y_1, const double yaw_1)
    {
        return turning_radius_ * GetRSPath(x_0, y_0, yaw_0, x_1, y_1, yaw_1).Length();
    }

    void RSPath::CSC(double x, double y, double phi, RSPathData &path)
    {
        double t, u, v, length_min = path.Length(), L;

        if (LpSpLp(x, y, phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[14], t, u, v);
            length_min = L;
        }

        if (LpSpLp(-x, y, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[14], -t, -u, -v);
            length_min = L;
        }

        if (LpSpLp(x, -y, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[15], t, u, v);
            length_min = L;
        }

        if (LpSpLp(-x, -y, phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[15], -t, -u, -v);
            length_min = L;
        }

        if (LpSpRp(x, y, phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[12], t, u, v);
            length_min = L;
        }

        if (LpSpRp(-x, y, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[12], -t, -u, -v);
            length_min = L;
        }

        if (LpSpRp(x, -y, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[13], t, u, v);
            length_min = L;
        }

        if (LpSpRp(-x, -y, phi, t, u, v) && length_min > std::fabs(t) + std::fabs(u) + std::fabs(v))
        {
            path = RSPathData(RS_path_segment_type[13], -t, -u, -v);
        }
    }

    bool RSPath::LpRmL(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x - std::sin(phi);
        double eta = y - 1.0 + std::cos(phi);
        double u1, theta;
        Polar(xi, eta, u1, theta);

        if (u1 > 4.0)
        {
            return false;
        }

        u = -2.0 * std::asin(0.25 * u1);
        t = Mod2Pi(theta + 0.5 * u + M_PI);
        v = Mod2Pi(phi - t + u);

        return true;
    }

    void RSPath::CCC(double x, double y, double phi, RSPathData &path)
    {
        double t, u, v, L;
        double length_min = path.Length();

        if (LpRmL(x, y, phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[0], t, u, v);
            length_min = L;
        }

        if (LpRmL(-x, y, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[0], -t, -u, -v);
            length_min = L;
        }

        if (LpRmL(x, -y, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[1], t, u, v);
            length_min = L;
        }

        if (LpRmL(-x, -y, phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[1], -t, -u, -v);
            length_min = L;
        }

        double xb = x * std::cos(phi) + y * std::sin(phi);
        double yb = x * std::sin(phi) - y * std::cos(phi);
        if (LpRmL(xb, yb, phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[0], v, u, t);
            length_min = L;
        }

        if (LpRmL(-xb, yb, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[0], -v, -u, -t);
            length_min = L;
        }

        if (LpRmL(xb, -yb, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[1], v, u, t);
            length_min = L;
        }

        if (LpRmL(-xb, -yb, phi, t, u, v) && length_min > std::fabs(t) + std::fabs(u) + std::fabs(v))
        {
            path = RSPathData(RS_path_segment_type[1], -v, -u, -t);
        }
    }

    bool RSPath::LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x + std::sin(phi);
        double eta = y - 1.0 - std::cos(phi);
        double rho = 0.25 * (2.0 + std::sqrt(xi * xi + eta * eta));

        if (rho > 1.0)
        {
            return false;
        }

        u = std::acos(rho);
        TauOmega(u, -u, xi, eta, phi, t, v);

        return true;
    }

    bool RSPath::LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x + std::sin(phi);
        double eta = y - 1.0 - std::cos(phi);
        double rho = (20.0 - xi * xi - eta * eta) / 16.0;

        if (rho >= 0.0 && rho <= 1.0)
        {
            u = -std::acos(rho);
            if (u >= -M_PI_2)
            {
                TauOmega(u, u, xi, eta, phi, t, v);
                return true;
            }
        }

        return false;
    }

    void RSPath::CCCC(double x, double y, double phi, RSPathData &path)
    {
        double t, u, v, L;
        double length_min = path.Length();

        if (LpRupLumRm(x, y, phi, t, u, v) && length_min > (L = std::fabs(t) + 2.0 * std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[2], t, u, -u, v);
            length_min = L;
        }

        if (LpRupLumRm(-x, y, -phi, t, u, v) &&
            length_min > (L = std::fabs(t) + 2.0 * std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[2], -t, -u, u, -v);
            length_min = L;
        }

        if (LpRupLumRm(x, -y, -phi, t, u, v) &&
            length_min > (L = std::fabs(t) + 2.0 * std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[3], t, u, -u, v);
            length_min = L;
        }

        if (LpRupLumRm(-x, -y, phi, t, u, v) &&
            length_min > (L = std::fabs(t) + 2.0 * std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[3], -t, -u, u, -v);
            length_min = L;
        }

        if (LpRumLumRp(x, y, phi, t, u, v) &&
            length_min > (L = std::fabs(t) + 2.0 * std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[2], t, u, u, v);
            length_min = L;
        }

        if (LpRumLumRp(-x, y, -phi, t, u, v) &&
            length_min > (L = std::fabs(t) + 2.0 * std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[2], -t, -u, -u, -v);
            length_min = L;
        }

        if (LpRumLumRp(x, -y, -phi, t, u, v) &&
            length_min > (L = std::fabs(t) + 2.0 * std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[3], t, u, u, v);
            length_min = L;
        }

        if (LpRumLumRp(-x, -y, phi, t, u, v) && length_min > std::fabs(t) + 2.0 * std::fabs(u) + std::fabs(v))
        {
            path = RSPathData(RS_path_segment_type[3], -t, -u, -u, -v);
        }
    }

    bool RSPath::LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x - std::sin(phi);
        double eta = y - 1.0 + std::cos(phi);
        double rho, theta;

        Polar(xi, eta, rho, theta);

        if (rho < 2.0)
        {
            return false;
        }

        double r = std::sqrt(rho * rho - 4.0);
        u = 2.0 - r;
        t = Mod2Pi(theta + std::atan2(r, -2.0));
        v = Mod2Pi(phi - M_PI_2 - t);

        return true;
    }

    bool RSPath::LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x + std::sin(phi);
        double eta = y - 1.0 - std::cos(phi);
        double rho, theta;

        Polar(-eta, xi, rho, theta);

        if (rho < 2.0)
        {
            return false;
        }

        t = theta;
        u = 2.0 - rho;
        v = Mod2Pi(t + M_PI_2 - phi);

        return true;
    }

    void RSPath::CCSC(double x, double y, double phi, RSPathData &path)
    {
        double t, u, v, L;
        double length_min = path.Length() - M_PI_2;

        if (LpRmSmLm(x, y, phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[4], t, -M_PI_2, u, v);
            length_min = L;
        }

        if (LpRmSmLm(-x, y, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[4], -t, M_PI_2, -u, -v);
            length_min = L;
        }

        if (LpRmSmLm(x, -y, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[5], t, -M_PI_2, u, v);
            length_min = L;
        }

        if (LpRmSmLm(-x, -y, phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[5], -t, M_PI_2, -u, -v);
            length_min = L;
        }

        if (LpRmSmRm(x, y, phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[8], t, -M_PI_2, u, v);
            length_min = L;
        }

        if (LpRmSmRm(-x, y, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[8], -t, M_PI_2, -u, -v);
            length_min = L;
        }

        if (LpRmSmRm(x, -y, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[9], t, -M_PI_2, u, v);
            length_min = L;
        }

        if (LpRmSmRm(-x, -y, phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[9], -t, M_PI_2, -u, -v);
            length_min = L;
        }

        double xb = x * std::cos(phi) + y * std::sin(phi);
        double yb = x * std::sin(phi) - y * std::cos(phi);
        if (LpRmSmLm(xb, yb, phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[6], v, u, -M_PI_2, t);
            length_min = L;
        }

        if (LpRmSmLm(-xb, yb, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[6], -v, -u, M_PI_2, -t);
            length_min = L;
        }

        if (LpRmSmLm(xb, -yb, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[7], v, u, -M_PI_2, t);
            length_min = L;
        }

        if (LpRmSmLm(-xb, -yb, phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[7], -v, -u, M_PI_2, -t);
            length_min = L;
        }

        if (LpRmSmRm(xb, yb, phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[10], v, u, -M_PI_2, t);
            length_min = L;
        }

        if (LpRmSmRm(-xb, yb, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[10], -v, -u, M_PI_2, -t);
            length_min = L;
        }

        if (LpRmSmRm(xb, -yb, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[11], v, u, -M_PI_2, t);
            length_min = L;
        }

        if (LpRmSmRm(-xb, -yb, phi, t, u, v) && length_min > std::fabs(t) + std::fabs(u) + std::fabs(v))
        {
            path = RSPathData(RS_path_segment_type[11], -v, -u, M_PI_2, -t);
        }
    }

    bool RSPath::LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x + std::sin(phi);
        double eta = y - 1.0 - std::cos(phi);
        double rho, theta;

        Polar(xi, eta, rho, theta);

        if (rho >= 2.0)
        {
            u = 4.0 - std::sqrt(rho * rho - 4.0);

            if (u <= 0.0)
            {
                t = Mod2Pi(std::atan2((4.0 - u) * xi - 2.0 * eta, -2.0 * xi + (u - 4.0) * eta));
                v = Mod2Pi(t - phi);

                return true;
            }
        }

        return false;
    }

    void RSPath::CCSCC(double x, double y, double phi, RSPathData &path)
    {
        double t, u, v, L;
        double length_min = path.Length() - M_PI;
        if (LpRmSLmRp(x, y, phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[16], t, -M_PI_2, u, -M_PI_2, v);
            length_min = L;
        }

        if (LpRmSLmRp(-x, y, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[16], -t, M_PI_2, -u, M_PI_2, -v);
            length_min = L;
        }

        if (LpRmSLmRp(x, -y, -phi, t, u, v) && length_min > (L = std::fabs(t) + std::fabs(u) + std::fabs(v)))
        {
            path = RSPathData(RS_path_segment_type[17], t, -M_PI_2, u, -M_PI_2, v);
            length_min = L;
        }

        if (LpRmSLmRp(-x, -y, phi, t, u, v) && length_min > std::fabs(t) + std::fabs(u) + std::fabs(v))
        {
            path = RSPathData(RS_path_segment_type[17], -t, M_PI_2, -u, M_PI_2, -v);
        }
    }

    TypeVectorVecd<3> RSPath::GetRSPath(const Eigen::Vector3d &start_state, const Eigen::Vector3d &goal_state,
                                        const double step_size, double &length)
    {
        RSPathData rs_path = GetRSPath(start_state.x(), start_state.y(), start_state.z(),
                                       goal_state.x(), goal_state.y(), goal_state.z());

        length = rs_path.Length() * turning_radius_;
        // Debug info
        //    std::cout << "rs length: " << rs_path.Length() << "  | "
        //              << rs_path.length_[0] << " " << rs_path.length_[1] << " "
        //              << rs_path.length_[2] << " " << rs_path.length_[3] << " "
        //              << rs_path.length_[4] << std::endl;
        //    std::cout << "type: "
        //              << rs_path.type_[0] << " " << rs_path.type_[1] << " "
        //              << rs_path.type_[2] << " " << rs_path.type_[3] << " "
        //              << rs_path.type_[4] << std::endl;

        const double path_length = rs_path.Length() * turning_radius_;
        const auto interpolation_number = static_cast<unsigned int>(path_length / step_size);

        double phi;

        TypeVectorVecd<3> path_poses;

        for (unsigned int i = 0; i <= interpolation_number; ++i)
        {
            double v;
            double t = i * 1.0 / interpolation_number;
            double seg = t * rs_path.Length();

            Eigen::Vector3d temp_pose(0.0, 0.0, start_state.z());
            for (unsigned int j = 0; j < 5u && seg > 0; ++j)
            {
                if (rs_path.length_[j] < 0.0)
                {
                    v = std::max(-seg, rs_path.length_[j]);
                    seg += v;
                }
                else
                {
                    v = std::min(seg, rs_path.length_[j]);
                    seg -= v;
                }

                phi = temp_pose.z();
                switch (rs_path.type_[j])
                {
                case L:
                    temp_pose.x() = std::sin(phi + v) - std::sin(phi) + temp_pose.x();
                    temp_pose.y() = -std::cos(phi + v) + std::cos(phi) + temp_pose.y();
                    temp_pose.z() = phi + v;
                    break;
                case R:
                    temp_pose.x() = -std::sin(phi - v) + std::sin(phi) + temp_pose.x();
                    temp_pose.y() = std::cos(phi - v) - std::cos(phi) + temp_pose.y();
                    temp_pose.z() = phi - v;
                    break;
                case S:
                    temp_pose.x() = v * std::cos(phi) + temp_pose.x();
                    temp_pose.y() = v * std::sin(phi) + temp_pose.y();
                    temp_pose.z() = phi;
                    break;
                case N:
                    break;
                }
            }

            Eigen::Vector3d pose;
            pose.block<2, 1>(0, 0) = temp_pose.block<2, 1>(0, 0) * turning_radius_ + start_state.block<2, 1>(0, 0);
            pose.z() = temp_pose.z();

            path_poses.emplace_back(pose);
        }

        return path_poses;
    }
};
#endif // HYBRID_A_STAR_RS_PATH_H