#include <iostream>
#include <ros/ros.h>
// #include <vector.h>
#include <costmap_2d/costmap_2d_ros.h>
#include "teb/path_smoother.h"
#include <nav_msgs/Path.h>

namespace teb_planner_ns
{
    class TEB_Planner
    {
    public:
        TEB_Planner();
        ~TEB_Planner();
        void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros);
        bool makePlan(const geometry_msgs::PoseStamped &robot_pose, const std::vector<geometry_msgs::PoseStamped> &global_plan, std::vector<geometry_msgs::PoseStamped> &local_plan, const bool is_forward);

    private:
        std::vector<geometry_msgs::PoseStamped> global_plan_;
        std::vector<geometry_msgs::PoseStamped> local_plan_;
        path_smoother::PathSmoother path_smoother_;
        costmap_2d::Costmap2DROS *costmap_ros_;
        std::vector<double> duration_; // TEB每段轨迹的持续时间
        std::vector<geometry_msgs::PoseStamped> pre_local_plan_;
        geometry_msgs::PoseStamped robot_pose_;
        nav_msgs::Path pub_local_plan_;
        ros::Publisher plan_pub_;

        bool use_clamp_spline_flag;

        bool choiceLocalPlanPoint(std::vector<geometry_msgs::PoseStamped> &local_plan);
        void trajectoryOpt(const geometry_msgs::PoseStamped &robot_pose, std::vector<geometry_msgs::PoseStamped> &plan);
        void copyVectorPoseStamped(const std::vector<geometry_msgs::PoseStamped> &copied_vec, std::vector<geometry_msgs::PoseStamped> &vec);
        void myWorldToMap(std::vector<geometry_msgs::PoseStamped> &local_plan);
        void myMapToWorld(std::vector<geometry_msgs::PoseStamped> &local_plan);


    };
};