agv_ros_ws/my_planner/include/teb/path_smoother.h

#ifndef PATH_SMOOTHER_HPP
#define PATH_SMOOTHER_HPP

#include "costmap_2d/costmap_2d.h"
#include "teb/lbfgs.h"
#include "teb/BandedSystem.h"
#include <Eigen/Eigen>
#include <cmath>
#include <cfloat>
#include <iostream>
#include <vector>
#include <geometry_msgs/PoseStamped.h>
#include <algorithm>
#include <costmap_2d/costmap_2d_ros.h>
#include <algorithm>

namespace path_smoother
{

    class PathSmoother
    {
    private:
        int pieceNum;   // 三次样条曲线数量
        int nodeNum;    // 决策变量点数量
        double goalOri; // 轨迹目标点朝向
        bool clamp_spline_flag_;

        std::vector<Eigen::Matrix<double, 2, 4>> mycMatVec; // 分段三次样条曲线参数
        Eigen::Vector2d startPoint;                         // 起点坐标
        Eigen::Vector2d goalPoint;                          // 终点坐标
        Eigen::MatrixX2d points;                            // 路径的所有点          这个一会儿该成initPoints
        Eigen::VectorXd decisionVariables;                  // 一维的决策变量 包含起点和终点,我怎么感觉不应该包含啊 ??  包含了的话起点和终点就会动了
        BandedSystem A;                                     // 三次样条曲线参数求解矩阵  Ax = b
        Eigen::MatrixX2d b;                                 // 三次样条曲线参数求解矩阵  Ax = b
        lbfgs::lbfgs_parameter_t lbfgs_params;              // lbfgs 求解的参数
        costmap_2d::Costmap2D *costMap_;                    // 代价地图
        Eigen::Matrix2d robot_orientation;                    // 小车当前朝向
        // std::vector<double> duration_;                      // 每段轨迹的时间

        Eigen::MatrixX2d Points;            // 决策变量点
        Eigen::Matrix2Xd costGradient;      // 能耗梯度
        Eigen::Matrix2Xd penaltyGradient;   // 障碍物避让梯度
        Eigen::Matrix2Xd kinematicGradient; // 运动学约束梯度

        // getKinematicCost()的一些参数
        double kCost = 0;
        double D = 0;
        double maxTurnAngle;
        double turningAngle;
        std::vector<double> pointAngle;
        Eigen::Matrix<double, 2, 1> tangentVec;
        std::vector<Eigen::Matrix<double, 2, 1>> pointAngleVec; // (b_x, b_y)^T

        // getPenaltyGradient() 的一些参数
        Eigen::Vector2d current_point;
        Eigen::Vector2d up_point;
        Eigen::Vector2d down_point;
        Eigen::Vector2d left_point;
        Eigen::Vector2d right_point;
        std::vector<double> distance = {0, 0, 0, 0};
        double penaltyCost = 0;
        std::vector<double> L_k;
        std::vector<int> cost_k;
        std::vector<double> L_k_cost_k;
        std::vector<std::vector<int>> p_i;

        // solveCurveParameters() 的一些参数
        Eigen::MatrixX2d Delta;
        Eigen::MatrixX2d ttheta = Eigen::MatrixX2d::Zero(1, 2);
        Eigen::Matrix<double, 2, 4> coeff;
        /* 2 行 4 列矩阵
        d_x  c_x  b_x  a_x
        d_y  c_y  b_y  a_y
        */

        // 权重系数
        double weight_start_direction = 5; // 钳制样条曲线的起点约束权重
        double weight_goal_direction = 3;  // 钳制样条曲线的终点约束权重
        // 默认每段轨迹时间是1秒时, 5 3 是效果还ok
        double weight_kinematic_cost = 20000; // 一次违反运动学约束的代价权重系数

        static inline double costFunction(void *ptr,                // 当前类的指针
                                          const Eigen::VectorXd &x, // 决策变量，x
                                          Eigen::VectorXd &g)       // 梯度
        {                                                           // 根据坐标，计算 代价函数值 和 梯度值
            // 权重系数
            double weight_cost_energy = 10;    // 代价函数  能耗权重系数
            double weight_cost_obstacle = 80;  // 代价函数  障碍物避让权重系数
            double weight_cost_kinematic = 10; // 代价函数  运动学约束权重系数

            double weight_gradient_energy = 100;    // 梯度函数  能耗权重系数
            double weight_gradient_obstacle = 6;    // 梯度函数  障碍物避让权重系数
            double weight_gradient_kinematic = 0.4; // 梯度函数  运动学约束权重系数

            PathSmoother &obj = *(PathSmoother *)ptr; // 上面定义了 ptr 的类型是void, 这里要转换成 PathSmoother 类型的指针
            // lbfgs::lbfgs_optimize 中没有改动points, 只改动decisionVariables,为了方便计算做个转换
            Eigen::MatrixX2d Points;            // 决策变量点
            Eigen::Matrix2Xd costGradient;      // 能耗梯度
            Eigen::Matrix2Xd penaltyGradient;   // 障碍物避让梯度
            Eigen::Matrix2Xd kinematicGradient; // 运动学约束梯度

            costGradient.resize(2, obj.pieceNum - 1);
            penaltyGradient.resize(2, obj.pieceNum - 1);
            kinematicGradient.resize(2, obj.pieceNum - 1);

            costGradient.setZero();
            penaltyGradient.setZero();
            kinematicGradient.setZero();

            Points.resize(obj.nodeNum, 2);
            Points.row(0) = obj.startPoint;
            Points.row(Points.rows() - 1) = obj.goalPoint;

            for (int i = 1; i < (x.size() / 2) + 1; i++)
            {
                Points(i, 0) = x(2 * i - 2); // 决策变量不包含起点和终点
                Points(i, 1) = x(2 * i - 1);
            }

            double costEnergy = 0;
            double costPenalty = 0;
            double kinematicCost = 0;

            // 行驶能耗
            obj.solveCurveParameters(Points);
            obj.getCostEnergy(costEnergy);

            // 障碍物避让
            obj.getCostGradient(costGradient);
            costPenalty = obj.getPenaltyGradient(penaltyGradient, Points);

            // 运动学约束
            obj.getKinematicCost(kinematicCost, kinematicGradient);

            double cost = weight_cost_energy * costEnergy + weight_cost_obstacle * costPenalty + weight_cost_kinematic * kinematicCost;
            for (int i = 0; i < obj.pieceNum - 1; i++)
            {
                g(2 * i) = weight_gradient_energy * costGradient(0, i) + weight_gradient_obstacle * penaltyGradient(0, i) - weight_gradient_kinematic * kinematicGradient(0, i);
                g(2 * i + 1) = weight_gradient_energy * costGradient(1, i) + weight_gradient_obstacle * penaltyGradient(1, i) - weight_gradient_kinematic * kinematicGradient(1, i);
            }
            return cost;
        }

        inline void getKinematicCost(double &kinematicCost, Eigen::Matrix2Xd &kinematicGradient)
        {
            // 计算每个决策变量点的朝向，不能用Hybrid A* 的朝向信息，因为优化过程中点的位置会变化
            for (int i = 0; i < nodeNum - 1; i++)
            {
                tangentVec = mycMatVec[i].col(2) * 0.1 + mycMatVec[i].col(1) * 0.01 + mycMatVec[i].col(0) * 0.001; // 差分法计算每个点的方向向量
                tangentVec.normalize();
                pointAngleVec[i] = tangentVec;
                pointAngle[i] = std::atan(tangentVec(1) / tangentVec(0));
                if (tangentVec(1) < 0)
                    pointAngle[i] += 3.141592;
            }

            for (int i = 0; i < nodeNum - 3; i++) // 这里只能给第2个到倒数第2个的梯度
            {
                D = std::sqrt(std::pow((points(i, 0) - points(i + 1, 0)), 2) + std::pow((points(i, 1) - points(i + 1, 1)), 2));
                // maxTurnAngle = 3.141592 * 2 * std::asin(D / (2 * 0.77)) / 0.77; // 这段轨迹的最大允许转向角
                maxTurnAngle = 2 * std::asin(D / (2 * 0.77)); // 这段轨迹的最大允许转向角

                if (i == 0)
                    turningAngle = std::acos(robot_orientation(0) * pointAngleVec[0](0) + robot_orientation(1) * pointAngleVec[0](1));
                else
                    turningAngle = std::acos(pointAngleVec[i - 1](0) * pointAngleVec[i](0) + pointAngleVec[i - 1](1) * pointAngleVec[i](1));

                if (std::abs(turningAngle) > maxTurnAngle)
                {
                    kCost = weight_kinematic_cost * std::pow((turningAngle - maxTurnAngle), 2);
                    kinematicCost += kCost;
                    double cross_product = pointAngleVec[i - 1](0) * pointAngleVec[i](1) - pointAngleVec[i - 1](1) * pointAngleVec[i](0);
                    if (cross_product < 0)
                    {
                        kinematicGradient(0, i + 1) = -pointAngleVec[i](1) * kCost;
                        kinematicGradient(1, i + 1) = pointAngleVec[i](0) * kCost;
                    }
                    else
                    {
                        kinematicGradient(0, i + 1) = pointAngleVec[i](1) * kCost;
                        kinematicGradient(1, i + 1) = -pointAngleVec[i](0) * kCost;
                    }
                }
            }
        }

        inline double getPenaltyGradient(Eigen::Matrix2Xd &penaltyGradient, Eigen::MatrixX2d &Points)
        {
            penaltyGradient.setZero();
            Eigen::Matrix2Xd aasdfsd;
            aasdfsd.resize(2, pieceNum - 1);
            double cost_all = 0;
            penaltyCost = 0;

            for (int i = 0; i < penaltyGradient.cols(); i++) // 障碍物避让项 求 代价 和 梯度
            {
                if (static_cast<int>(costMap_->getCost(round(Points(i, 0)), round(Points(i, 1)))) > 20)
                {
                    current_point(0) = round(Points(i, 0));
                    current_point(1) = round(Points(i, 1));
                    std::vector<std::pair<int, int>> offsets = {{-1, -1}, {0, -1}, {1, -1}, {1, 0}, {1, 1}, {0, 1}, {-1, 1}, {-1, 0}};
                    for (int i = 0; i < 8; i++) // 找到8个邻居节点
                    {
                        p_i[i][0] = current_point(0) + offsets[i].first;
                        p_i[i][1] = current_point(1) + offsets[i].second;
                    }
                    for (int ii = 0; ii < 8; ii++)
                    {
                        L_k[ii] = std::sqrt(std::pow(p_i[ii][0] - Points(i, 0), 2) + std::pow(p_i[ii][1] - Points(i, 1), 2));
                        cost_k[ii] = costMap_->getCost(round(p_i[ii][0]), round(p_i[ii][1]));
                        L_k_cost_k[ii] = std::max(L_k[ii] * cost_k[ii], L_k[ii]);
                    }

                    int L_m_index = std::min_element(L_k_cost_k.begin(), L_k_cost_k.end()) - L_k_cost_k.begin();
                    penaltyCost = std::max(L_k[L_m_index] * costMap_->getCost(current_point(0), current_point(1)), penaltyCost);
                    cost_all += L_k[L_m_index] * costMap_->getCost(current_point(0), current_point(1));
                    double alpha = std::atan2(Points(i, 1) - p_i[L_m_index][1], Points(i, 0) - p_i[L_m_index][0]);
                    if (alpha < 0)
                    {
                        alpha += 2 * M_PI;
                    }
                    penaltyGradient(0, i) = costMap_->getCost(current_point(0), current_point(1)) * (L_k[L_m_index] - 0.5) * std::cos(alpha);
                    penaltyGradient(1, i) = costMap_->getCost(current_point(0), current_point(1)) * (L_k[L_m_index] - 0.5) * std::sin(alpha);
                }
                else // 如果不是障碍物，则没有梯度
                {
                    penaltyGradient(0, i) = 0;
                    penaltyGradient(1, i) = 0;
                }
            }
            return penaltyCost;
        }

        inline void getCostGradient(Eigen::Matrix2Xd &costGradient)
        {
            // double dc0, dc1, dd0, dd1;
            for (int i = 0; i < mycMatVec.size() - 1; i++)
            {
                for (int j = 0; j < 2; j++)
                {
                    // dc0 = 8 * mycMatVec[i](j, 1) + 12 * mycMatVec[i](j, 0);
                    // dc1 = 8 * mycMatVec[i + 1](j, 1) + 12 * mycMatVec[i + 1](j, 0);
                    // dd0 = 12 * mycMatVec[i](j, 1) + 24 * mycMatVec[i](j, 0);
                    // dd1 = 12 * mycMatVec[i + 1](j, 1) + 24 * mycMatVec[i + 1](j, 0);
                    // costGradient(j, i) = 3 * (dc0 - dc1) + 2 * (dd1 - dd0);
                    costGradient(j, i) = 12 * mycMatVec[i + 1](j, 0) - 12 * mycMatVec[i](j, 0); // 上面的式子化简下来就是这个了

                    // coeff.col(0) = ((b.row(i + 1) - b.row(i)) / 3).transpose();                    // d
                    // coeff.col(1) = b.row(i).transpose();                                           // c
                    // coeff.col(2) = (Delta.row(i) - (2 * b.row(i) + b.row(i + 1)) / 3).transpose(); // b
                    // coeff.col(3) = points.row(i).transpose();                                      // a
                    // mycMatVec[i] = (coeff);
                }
            }
        }

        inline void getCostEnergy(double &costEnergy)
        {
            for (int i = 0; i < pieceNum; i++)
            {
                for (int j = 0; j < 2; j++) // 代价函数（这里没有乘权重系数）
                    // costEnergy += 4 * std::pow(mycMatVec[i](j, 1), 2) + 12 * mycMatVec[i](j, 1) * mycMatVec[i](j, 0) * duration_[i] * duration_[i] + 12 * std::pow(mycMatVec[i](j, 0), 2) * duration_[i] * duration_[i] * duration_[i];
                    costEnergy += 4 * std::pow(mycMatVec[i](j, 1), 2) + 12 * mycMatVec[i](j, 1) * mycMatVec[i](j, 0) + 12 * std::pow(mycMatVec[i](j, 0), 2);
            }
            return;
        }

        inline void solveCurveParameters(const Eigen::MatrixX2d &points) // 这里的points包含起点和终点，即N个点
        {

            Delta.resize(nodeNum - 1, 2);
            for (int i = 0; i < Delta.rows(); i++)
                Delta.row(i) = points.row(i + 1) - points.row(i);
            for (int i = 1; i < b.rows() - 1; i++)
                // b.row(i) = 3 * (Delta.row(i) / duration_[i] - Delta.row(i - 1)) / duration_[i - 1];
                b.row(i) = 3 * (Delta.row(i) - Delta.row(i - 1));

            // 自由边界样条曲线
            // b.row(0) = Eigen::MatrixX2d::Zero(1, 2);
            // b.row(b.rows() - 1) = Eigen::MatrixX2d::Zero(1, 2);

            if (clamp_spline_flag_ == true)
            {
                // 钳制样条曲线边界设置
                ttheta << robot_orientation(0), robot_orientation(1);
                // b.row(0) = 3 * (Delta.row(0) / duration_[0] - ttheta * weight_start_direction);
                b.row(0) = 3 * (Delta.row(0) - ttheta * weight_start_direction);

                ttheta << std::cos(goalOri), std::sin(goalOri);
                // b.row(b.rows() - 1) = 3 * (ttheta * weight_goal_direction - Delta.row(Delta.rows() - 1) / duration_[duration_.size() - 1]);
                b.row(b.rows() - 1) = 3 * (ttheta * weight_goal_direction - Delta.row(Delta.rows() - 1));
            }
            else
            {
                // 自由边界样条曲线
                b.row(0) = Eigen::MatrixX2d::Zero(1, 2);
                b.row(b.rows() - 1) = Eigen::MatrixX2d::Zero(1, 2);
                A(0, 0) = 1;
                A(nodeNum - 1, nodeNum - 1) = 1;
                for (int i = 1; i < nodeNum - 1; i++)
                {
                    A(i, i - 1) = 1;
                    A(i, i) = 4;
                    A(i, i + 1) = 1;
                }
                A.factorizeLU(); // 对矩阵A做LU分解
            }

            A.solve(b);

            for (int i = 0; i < nodeNum - 1; i++)
            {
                coeff.col(0) = ((b.row(i + 1) - b.row(i)) / 3).transpose();                    // d
                coeff.col(1) = b.row(i).transpose();                                           // c
                coeff.col(2) = (Delta.row(i) - (2 * b.row(i) + b.row(i + 1)) / 3).transpose(); // b
                coeff.col(3) = points.row(i).transpose();                                      // a
                mycMatVec[i] = (coeff);

                // coeff.col(0) = ((b.row(i + 1) - b.row(i)) / (3 * duration_[i])).transpose();                                   // d
                // coeff.col(1) = b.row(i).transpose();                                                                           // c
                // coeff.col(2) = (Delta.row(i) / duration_[i] - (2 * b.row(i) + b.row(i + 1)) / (3 * duration_[i])).transpose(); // b
                // coeff.col(3) = points.row(i).transpose();                                                                      // a
                // mycMatVec[i] = (coeff);
            }
        }

    public:
        inline bool setup(std::vector<geometry_msgs::PoseStamped> &plan,                                               // 这个setup 是在global_planner里面调用的，感觉有些数据可以在小车启动的时候就可以先算好了！！！
                          costmap_2d::Costmap2DROS *costmap_ros, std::vector<double> duration, bool clamp_spline_flag) // 传递指针

        {
            clamp_spline_flag_ = clamp_spline_flag;
            costMap_ = costmap_ros->getCostmap(); // 代价地图
            pieceNum = plan.size() - 1;           // 分段数量
            nodeNum = plan.size();
            startPoint(0) = plan.front().pose.position.x; // 起点坐标
            startPoint(1) = plan.front().pose.position.y;
            goalPoint(0) = plan.back().pose.position.x; // 终点坐标
            goalPoint(1) = plan.back().pose.position.y;
            points.resize(plan.size(), 2);
            decisionVariables.resize(plan.size() * 2 - 4);
            // goalOri = plan[plan.size() - 1].pose.orientation.z; // 这里要改！！
            Eigen::Quaterniond q;
            Eigen::Vector3d euler;
            q.x() = plan[plan.size() - 1].pose.orientation.x;
            q.y() = plan[plan.size() - 1].pose.orientation.y;
            q.z() = plan[plan.size() - 1].pose.orientation.z;
            q.w() = plan[plan.size() - 1].pose.orientation.w;
            euler = q.toRotationMatrix().eulerAngles(2, 1, 0); // 用Eigen将四元数转换成直观的欧拉角
            goalOri = std::abs(euler[1]) > 1.57 ? euler[0] + 3.141592 : euler[0];

            for (int i = 0; i < plan.size(); i++)
            {
                points(i, 0) = plan[i].pose.position.x;
                points(i, 1) = plan[i].pose.position.y;
            }
            for (int i = 1; i < plan.size() - 1; i++)
            {
                decisionVariables(2 * i - 2) = plan[i].pose.position.x;
                decisionVariables(2 * i - 1) = plan[i].pose.position.y;
            }

            A.create(nodeNum, 1, 1); // A是带状矩阵，后面两个1表示对角线及上下各1行不是0,其他都是0
            b.resize(nodeNum, 2);

            // duration_ = duration;

            // duration_.resize(nodeNum - 1);
            // std::fill(duration_.begin(), duration_.end(), 1);

            A.reset();

            // 钳制样条曲线边界设置(默认开始的时候是钳制样条曲线)
            A(0, 0) = 2;
            A(0, 1) = 1;
            A(nodeNum - 1, nodeNum - 2) = 1;
            A(nodeNum - 1, nodeNum - 1) = 2 * 2;

            for (int i = 1; i < nodeNum - 1; i++)
            {
                A(i, i - 1) = 1;
                A(i, i) = 4;
                A(i, i + 1) = 1;
            }

            // A(0, 0) = 2 * duration_[0];
            // A(0, 1) = duration_[0];
            // A(nodeNum - 1, nodeNum - 2) = duration_[duration_.size() - 1];
            // A(nodeNum - 1, nodeNum - 1) = 2 * duration_[duration_.size() - 1];
            // for (int i = 1; i < nodeNum - 1; i++)
            // {
            //     A(i, i - 1) = duration_[i - 1];
            //     A(i, i) = 2 * duration_[i - 1] + 2 * duration_[i];
            //     A(i, i + 1) = duration_[i];
            // }

            A.factorizeLU(); // 对矩阵A做LU分解

            mycMatVec.resize(pieceNum);

            // 运动学约束函数的变量初始化
            pointAngleVec.resize(nodeNum - 1);
            pointAngle.resize(nodeNum - 1);

            L_k.resize(8);
            cost_k.resize(8);
            L_k_cost_k.resize(8);
            p_i.resize(8);
            for (int i = 0; i < 8; i++)
            {
                p_i[i].resize(2);
            }

            return true;
        }

        Eigen::VectorXd optimize(int &Ret)
        {
            std::vector<geometry_msgs::PoseStamped> plan;
            double minCost = 100;
            lbfgs_params.mem_size = 8;   // lbfgs 的一些求解参数, 都有默认值, 按需设置
            lbfgs_params.delta = 1.0e-4; // e-4效果还可以，e-03效果就比较一般，时间上e-3差不多要比e-4快一倍
            int result = lbfgs::lbfgs_optimize(decisionVariables, minCost, &PathSmoother::costFunction, nullptr, this, lbfgs_params);
            // this 应该是传递 A_star_global_planner 中 path_smoother::PathSmoother path_smoother_; 实例化的 path_smoother_ 对象
            if (result >= 0)
            {
                plan.clear();
                plan.reserve(decisionVariables.size() / 2);
                for (int i = 0; i < decisionVariables.size() / 2; i++)
                {
                    plan[i].pose.position.x = decisionVariables(2 * i + 0);
                    plan[i].pose.position.y = decisionVariables(2 * i + 1);
                }
            }
            else
            {
                std::cout << "优化失败，`result = " << result << std::endl;
                decisionVariables.resize(0);
            }
            return decisionVariables;
        }

        void setRobotPose(const geometry_msgs::PoseStamped &robot_pose)
        {
            Eigen::Quaterniond q;
            q.x() = robot_pose.pose.orientation.x;
            q.y() = robot_pose.pose.orientation.y;
            q.z() = robot_pose.pose.orientation.z;
            q.w() = robot_pose.pose.orientation.w;
            Eigen::Vector3d euler = q.toRotationMatrix().eulerAngles(2, 1, 0); // 用Eigen将四元数转换成直观的欧拉角
            double robot_theta = std::abs(euler[1]) > 1.57 ? euler[0] + 3.141592 : euler[0];

            robot_orientation(0) = std::cos(robot_theta); // x
            robot_orientation(1) = std::sin(robot_theta); // y
        }
    };
}

#endif