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(const Pose2d& obs,double min_velocity,double max_velocity);
    virtual ~LoadedMPC();
    virtual bool solve(Trajectory trajectory, 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,Pose2d target,
                           Trajectory trajectory,std::vector<Pose2d>& poses,int point_num=10);
    /*
     * 根据路径点，拟合曲线
     */
    static Eigen::VectorXd fit_path(const std::vector<Pose2d>& trajectory);
    Pose2d obs_relative_pose_;    //障碍物相对小车的位姿
    double min_velocity_;
    double max_velocity_;

};

#endif //LIO_LIVOX_CPP_MPC_LOADED_MPC_H_