AGV/MPC/loaded_mpc.h

//
// Created by zx on 22-12-1.
//

#ifndef LIO_LIVOX_CPP_MPC_LOADED_MPC_H_
#define LIO_LIVOX_CPP_MPC_LOADED_MPC_H_

#include <Eigen/Core>
#include "trajectory.h"

class LoadedMPC
{
 public:
    LoadedMPC(double wheelbase);
    ~LoadedMPC();
    virtual bool solve(const Trajectory& trajectory, const Pose2d& target,Eigen::VectorXd statu,
                       std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory);

 protected:
    /*
     * 根据当前点和轨迹，选择前方 point_num
     * point:当前点，target：目标点
     */
    static bool filte_Path(const Pose2d& point,const Pose2d& target,
                           const Trajectory& trajectory,std::vector<Pose2d>& poses,int point_num=10);
    /*
     * 根据路径点，拟合曲线
     */
    static Eigen::VectorXd fit_path(const std::vector<Pose2d>& trajectory);
    double wheelbase_;

};


#endif //LIO_LIVOX_CPP_MPC_LOADED_MPC_H_