纯跟踪算法可借鉴开发

					```cpp

#include “pathcontrol.h”

using namespace std;

enum StateMachSts_st
{
SM_OFF,
SM_STANDBY,
SM_HANDSHAKE,
SM_ACTIVE,
SM_BRAKE,
SM_COMPLETE,
} sm_state;

int TargetInd;
float Car_current_velicity;
bool active;
int mini_index;
float Lp = 2.7;
float pursuit_K = 0.1;
float pursuit_Lfc = 2;

PathController::PathController() : ImuGpsSize_(3)
{
}

PathController::~PathController()
{
}

// vector ref_path_points;
// vector clean_path_points;
// vector cartesian_path_points;
// queue car_current_XY;
// vector Indx;

void PathController::ReadRefpathGPS_lon_lat_alat()
{
int index;
ifstream inFile(FILE_PATH, ios::in);
if (!inFile)
{
cout << “打开文件失败” << endl;
exit(1);
}

string line;

while (getline(inFile, line)) //getline(inFile, line)表示按行读取CSV文件中的数据
{
    string field;
    istringstream sin(line); //将整行字符串line读入到字符串流sin中
    ImuGpsData tmp = {0};

    getline(sin, field, ','); //将字符串流sin中的字符读入到field字符串中，以逗号为分隔符
    tmp.ImuGpsLat = atof(field.c_str());
    // refpath_Gps_latitude[index] = temp1; //纬度
    // refpath_lat.push_back(temp1);

    getline(sin, field, ','); //将字符串流sin中的字符读入到field字符串中，以逗号为分隔符
    tmp.ImuGpsLon = atof(field.c_str());
    // refpath_Gps_longitude[index] = temp2; //经度
    // refpath_lon.push_back(temp2);

    getline(sin, field, ','); //将字符串流sin中的字符读入到field字符串中，以逗号为分隔符
    tmp.ImuGpsAlat = atof(field.c_str());
    // refpath_Gps_altitude[index] = temp3; //高度
    // refpath_alat.push_back(temp3);
    ref_path_points.push_back(tmp);

    index++;
}

inFile.close();

}

void PathController::CleanrefpathGpsPoint_lon_lat_alat()
{
ImuGpsData clean_temp;
bool flag(false);
Indx.push_back(0);
for (int i = 0; i < ref_path_points.size() && !flag;)
{
for (int j = i + 1; j < ref_path_points.size(); ++j)
{
if (j == ref_path_points.size() - 1)
flag = true;
float dist = abs(sqrt(pow(10000000 * ref_path_points[i].ImuGpsLon - 10000000 * ref_path_points[j].ImuGpsLon, 2) + pow(10000000 * ref_path_points[i].ImuGpsLat - 10000000 * ref_path_points[j].ImuGpsLat, 2)));
if (dist >= 0.000000000001)
{
i = j;
Indx.push_back(i);
}
}
}

for (int k = 0; k < Indx.size() - 1; ++k)
{
    clean_temp.ImuGpsLon = (ref_path_points[Indx[k]].ImuGpsLon);
    clean_temp.ImuGpsLat = (ref_path_points[Indx[k]].ImuGpsLat);
    clean_temp.ImuGpsAlat = (ref_path_points[Indx[k]].ImuGpsAlat);
    clean_path_points.push_back(clean_temp);
}

}

std::vector PathController::RefpathGps2Cal_RefpathXY()
{
CartesianXYPoint cal_tempxy;
float wgs84a = 6378.137;
float wgs84f = 1.0 / 298.257223563;
float wgs84b = wgs84a * (1.0 - wgs84f);
float eccsq = 1 - (wgs84b * wgs84b) / (wgs84a * wgs84a);
float dtr = M_PI / 180.0;
for (int IND = 0; IND < clean_path_points.size() - 1; ++IND)
{
float clat = cos(dtr * clean_path_points[IND].ImuGpsLat);
float clon = cos(dtr * clean_path_points[IND].ImuGpsLon);
float slat = sin(dtr * clean_path_points[IND].ImuGpsLat);
float slon = sin(dtr * clean_path_points[IND].ImuGpsLon);
float dsq = 1.0 - eccsq * slat * slat;
float d = sqrt(dsq);
float rn = wgs84a / d;
float Cal_RefpathX = (rn + clean_path_points[IND].ImuGpsAlat) * clat * clon;
float Cal_RefpathY = (rn + clean_path_points[IND].ImuGpsAlat) * clat * slon;
cal_tempxy.cartesian_X = (Cal_RefpathX);
cal_tempxy.cartesian_Y = (Cal_RefpathY);
cartesian_path_points.push_back(cal_tempxy);
}
return cartesian_path_points;
}

CartesianXYPoint PathController::getCarCurPt()
{
float wgs84a = 6378.137;
float wgs84f = 1.0 / 298.257223563;
float wgs84b = wgs84a * (1.0 - wgs84f);
float eccsq = 1 - (wgs84b * wgs84b) / (wgs84a * wgs84a);
float dtr = M_PI / 180.0;

float clat1 = cos(dtr * car_current_imu_gps_data.ImuGpsLat);
float clon1 = cos(dtr * car_current_imu_gps_data.ImuGpsLon);
float slat1 = sin(dtr * car_current_imu_gps_data.ImuGpsLat);
float slon1 = sin(dtr * car_current_imu_gps_data.ImuGpsLon);
float dsq1 = 1.0 - eccsq * slat1 * slat1;
float d1 = sqrt(dsq1);
float rn1 = wgs84a / d1;
float Car_X1 = (rn1 + car_current_imu_gps_data.ImuGpsAlat) * clat1 * clon1;
float Car_Y1 = (rn1 + car_current_imu_gps_data.ImuGpsAlat) * clat1 * slon1;
return CartesianXYPoint(Car_X1, Car_Y1);

}

std::vector PathController::CarGps2Cal_CarXY()
{
if (car_current_XY.size() == ImuGpsSize_)
{
car_current_XY.pop();
}

while (car_current_XY.size() < ImuGpsSize_) // ImuGpsSize_ == 2
{
    CartesianXYPoint car_current_point = getCarCurPt();
    car_current_XY.push(car_current_point);
}

std::vector<CartesianXYPoint> pts = {car_current_XY.front(), car_current_XY.back()};
//pts[0].cartesian_X;
return pts;
cout << "pts[0].cartesian_X" << pts[0].cartesian_X << "---"
     << "pts[0].cartesian_X" << pts[0].cartesian_Y << endl;
cout << "pts[1].cartesian_X" << pts[1].cartesian_X << "---"
     << "pts[1].cartesian_X" << pts[1].cartesian_Y << endl;

}

int PathController::calc_pursuit_index(CartesianXYPoint car_current_cartisian_point, std::vector &cartesian_path_points) //计算最短路径。
{

int pursuit_index;
float min = abs(sqrt(pow(car_current_cartisian_point.cartesian_X - cartesian_path_points[0].cartesian_X, 2) + pow(car_current_cartisian_point.cartesian_Y - cartesian_path_points[0].cartesian_Y, 2)));
for (int j = 0; j < cartesian_path_points.size(); j++)
{
    float d = abs(sqrt(pow(car_current_cartisian_point.cartesian_X - cartesian_path_points[j].cartesian_X, 2) + pow(car_current_cartisian_point.cartesian_Y - cartesian_path_points[j].cartesian_Y, 2)));
    if (d < min)
    {
        min = d;
        pursuit_index = j;
    }
}
float L = 0.0;
float Lf = pursuit_K * Car_current_velicity + 1.0;
while (Lf > L && (pursuit_index + 1) < cartesian_path_points.size())
{
    float dx = cartesian_path_points[pursuit_index + 1].cartesian_X - cartesian_path_points[pursuit_index].cartesian_X;
    float dy = cartesian_path_points[pursuit_index + 1].cartesian_Y - cartesian_path_points[pursuit_index].cartesian_Y;
    L += sqrt(pow(dx, 2) + pow(dy, 2));
    pursuit_index += 1;
}
return pursuit_index;
cout << "pursuit_index" << pursuit_index << endl;

}

Result PathController::pure_pursuit_control(std::vector &pts, std::vector &cartesian_path_points, int pind)
{
Result ret;
float tx;
float ty;

if (pind < cartesian_path_points.size())
{
    tx = cartesian_path_points[pind].cartesian_X;
    ty = cartesian_path_points[pind].cartesian_Y;
}
else
{
    tx = cartesian_path_points[cartesian_path_points.size() - 1].cartesian_X; //倒数第一个点
    ty = cartesian_path_points[cartesian_path_points.size() - 1].cartesian_Y;
    pind = cartesian_path_points.size() - 1; //最后一个点的索引
}
float pi = 3.14;

float refx_car0x = (cartesian_path_points[pind].cartesian_X-pts[0].cartesian_X );
float refy_car0y = (cartesian_path_points[pind].cartesian_Y-pts[0].cartesian_Y);
float car1x_car0x = (pts[1].cartesian_X - pts[0].cartesian_X) + 0.01;
float car1y_car0y = (pts[1].cartesian_Y - pts[0].cartesian_Y) + 0.1;
//float alpha1 = abs(refx_car0x * car1y_car0y) + abs(refy_car0y * car1x_car0x);
float alpha1 = (refx_car0x * car1x_car0x) + (refy_car0y * car1y_car0y);

float alpha2 = sqrt(pow(refx_car0x, 2) + pow(refy_car0y, 2)) * sqrt(pow(car1x_car0x, 2) + pow(car1y_car0y, 2));
float salpha = alpha1 / alpha2;
if (salpha < -1 && salpha > -2)
{
    salpha = -1;
}
else if (salpha > 1 && salpha < 2)
{
    salpha = 1;
}
float angle = acos(salpha) * 180 / M_PI;

cout << "refx_car0x" << refx_car0x << "refy_car0y" << refy_car0y << "car1x_car0x " << car1x_car0x << "car1y_car0y" << car1y_car0y << endl;
cout << "pts[0].cartesian_X" << pts[0].cartesian_X << "pts[0].cartesian_Y" << pts[0].cartesian_Y << "pts[1].cartesian_X" << pts[1].cartesian_X << "pts[1].cartesian_Y" << pts[1].cartesian_Y << endl;
cout << "cartesian_path_points[pind].cartesian_X" << cartesian_path_points[pind].cartesian_X << "cartesian_path_points[pind].cartesian_Y" << cartesian_path_points[pind].cartesian_Y << endl;
float Lf = pursuit_K * Car_current_velicity + pursuit_Lfc;
//float pursuit_alpha = atan2(cartesian_path_points[pursuit_ind].cartesian_Y - car_current_cartisian_point.cartesian_Y, cartesian_path_points[pursuit_ind].cartesian_X - car_current_cartisian_point.cartesian_X) - CarCurrentSteerAngle - 90;
//float pursuit_delta = atan2(2.0 * Lp * sin(pursuit_alpha) / Lf, 1.0);
float delta = atan(2.0 * Lp * sin(angle) / Lf)* 180 / M_PI;

// float delta = (atan2(2.0 * Lp * sin(alpha) / Lf, 1.0)) * 16;

ret.delta = delta;
ret.ind = pind;
return ret; //返回角度和最近点的索引。
cout << "ret" << ret.delta << "###" << ret.ind << endl;

}

float PathController::Velicity_control()
{

if (Car_current_velicity < 2)
{
    target_acc = 0.25;
}
else if (Car_current_velicity > 5)
{
    target_acc = -0.25;
}
else
{
    target_acc = 0;
}

return target_acc;

}

//激活条件
// bool PathController::Automobile_Active()
// {
// int active;

// int pursuit_MinDistance_ind = 0;
// float min = abs(sqrt(pow(car_current_cartisian_point.cartesian_X - cartesian_path_points[0].cartesian_X, 2) + pow(car_current_cartisian_point.cartesian_Y - cartesian_path_points[0].cartesian_Y, 2)));
// for (int j = 0; j < cartesian_path_points.size(); j++)
// {
// float d = abs(sqrt(pow(car_current_cartisian_point.cartesian_X - cartesian_path_points[j].cartesian_X, 2) + pow(car_current_cartisian_point.cartesian_Y - cartesian_path_points[j].cartesian_Y, 2)));
// if (d < min)
// {
// min = d;
// pursuit_MinDistance_ind = j;
// }
// }

// float Min_X_Distance = (cartesian_path_points[pursuit_MinDistance_ind].cartesian_X - car_current_cartisian_point.cartesian_X); //x方向的距离 - 车辆x方向的距离
// float Min_Y_Distance = (cartesian_path_points[pursuit_MinDistance_ind].cartesian_Y - car_current_cartisian_point.cartesian_Y);
// float Min_D_Distance = abs(sqrt(Min_X_Distance * Min_X_Distance + Min_Y_Distance * Min_Y_Distance));
// //
// float pursuit_Lf = (pursuit_K * Car_current_velicity + pursuit_Lfc);

// if (pursuit_Lf > Min_D_Distance)
// {
// active = 1;
// }
// else
// {
// active = 0;
// }
// return active;
// }

void PathController::statemachine_task()
{
// TODO: convert current car gps latitude longitude to S-L points.
// CartesianXYPoint car_current_cartisian_point;
// vector pts;

car_current_cartisian_point = PathController::getCarCurPt();

pts = PathController::CarGps2Cal_CarXY();

switch (sm_state)
{
case SM_OFF:
    PathController::ReadRefpathGPS_lon_lat_alat();
    PathController::CleanrefpathGpsPoint_lon_lat_alat();
    cartesian_path_points = PathController::RefpathGps2Cal_RefpathXY();

    if (cartesian_path_points.size() != 0) // TODO: 1. read reference path and convert to s-l , return finishend.
    {
        sm_state = SM_STANDBY;
    }
    chassis_ctr_handshake_req = false;
    break;

case SM_STANDBY:

    // TODO: 2 . if funtion on condition is reached.

    // active = Automobile_Active();
    active = 1;

    if (active == true && current_gear == GEAR_P)
    {
        sm_state = SM_HANDSHAKE;
    }

    chassis_ctr_handshake_req = false;
    break;
case SM_HANDSHAKE:
    // TODO: 3. if eps controlled is OK
    if (chassis_be_controlled == true)
    {
        sm_state = SM_ACTIVE;
    }
    chassis_ctr_handshake_req = true;
    break;
case SM_ACTIVE:
    TargetInd = PathController::calc_pursuit_index(car_current_cartisian_point, cartesian_path_points);
    // TODO:  4 mini obstacle length < 10 meter
    if (dist_mini_obstacle < 4)
    {
        sm_state = SM_BRAKE;
    }

    else if (TargetInd == cartesian_path_points.size() - 1) // TODO 5 reach the reference path end
    {
        sm_state = SM_COMPLETE;
    }
    else
    {
        //TODO : SM active actions: calculate the output parameters
        Result ret;
        ret = PathController::pure_pursuit_control(pts, cartesian_path_points, TargetInd);
        target_steer_ang = ret.delta;
        int p_ind = ret.ind;
        target_acc = PathController::Velicity_control();
        // then output
        chassis_ctr_handshake_req = true;
        target_gear = GEAR_D;
        cout << " p_ind" << p_ind << endl;
    }
    break;
case SM_BRAKE:
    if (dist_mini_obstacle > 6)
    {
        sm_state = SM_ACTIVE;
    }
    else
    {
        chassis_ctr_handshake_req = true;
        target_acc = -0.5;
    }
    break;
case SM_COMPLETE:
    if (current_gear == GEAR_P)
    {
        // jump to off, wait for next function on .
        sm_state = SM_OFF;
    }
    else
    {
        chassis_ctr_handshake_req = true;
        target_acc = -0.5;
        if (Car_current_velicity <= 0.2)
        {
            target_gear = GEAR_P;
        }
    }
    break;
default:
    break;
}
//printf("SM state: %d, steer angle: %f \n", sm_state, target_steer_ang);
printf("SM state: %d, arget_steer_ang: %f, cam_distance: %f \n", sm_state, target_steer_ang, dist_mini_obstacle);

}

```cpp
#ifndef PLANING_CONTROL_H
#define PLANING_CONTROL_H

#include <cstdio>
#include <cmath>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <string>
#include <vector>
#include <queue>

#define FILE_PATH "/home/titan/Bo.csv"

struct CartesianXYPoint
{
    float cartesian_X;
    float cartesian_Y;
    CartesianXYPoint() : cartesian_X(0.0f), cartesian_Y(0.0f)
    {
    }

    CartesianXYPoint(const float _x, const float _y)
        : cartesian_X(_x), cartesian_Y(_y)
    {
    }

    CartesianXYPoint &operator=(const CartesianXYPoint &obj)
    {
        this->cartesian_X = obj.cartesian_X;
        this->cartesian_Y = obj.cartesian_Y;
        return *this;
    }
};

struct ImuGpsData
{
    float ImuGpsLon;
    float ImuGpsLat;
    float ImuGpsAlat;
};

enum Gear
{
    GEAR_NONE = 0,
    GEAR_P = 1,
    GEAR_R = 2,
    GEAR_N = 3,
    GEAR_D = 4,

};

struct Result
{
    double delta;
    int ind;
};

class PathController
{

public:
    PathController();
    ~PathController();

public:
    void statemachine_task();

public:
    // input parameters:--------------------------------------------------如何给直接给
    float Car_current_velicity; // uinit: m/s^2
    float dist_mini_obstacle;   // uinit: m
    ImuGpsData car_current_imu_gps_data;
    float CarCurrentSteerAngle;
    bool chassis_be_controlled;
    enum Gear current_gear;

    // output
    enum Gear target_gear;
    float target_acc;       //uinit: m/s^2
    float target_steer_ang; // unit: degree -540 ~ +540
    bool chassis_ctr_handshake_req;

private:
    void ReadRefpathGPS_lon_lat_alat();
    void CleanrefpathGpsPoint_lon_lat_alat();
    std::vector<CartesianXYPoint> RefpathGps2Cal_RefpathXY();
    std::vector<CartesianXYPoint> CarGps2Cal_CarXY();
    CartesianXYPoint getCarCurPt();
    int calc_pursuit_index(CartesianXYPoint car_current_cartisian_point, std::vector<CartesianXYPoint> &cartesian_path_points); //计算最短路径。
    Result pure_pursuit_control(std::vector<CartesianXYPoint> &pts, std::vector<CartesianXYPoint> &cartesian_path_points, int pind);
    // bool Automobile_Active();
    float Velicity_control();

private:
    int ImuGpsSize_;
    std::vector<ImuGpsData> ref_path_points;
    std::vector<ImuGpsData> clean_path_points;
    std::vector<int> Indx;
    std::vector<CartesianXYPoint> cartesian_path_points;
    std::queue<CartesianXYPoint> car_current_XY;
    std::vector<CartesianXYPoint> pts;

    CartesianXYPoint car_current_cartisian_point;
    CartesianXYPoint last_car_current_cartisian_point;
};

#endif

#include "pathcontrol.h"

using namespace std;

int TargetInd;
float Car_current_velicity;
bool active;
int mini_index;

PathController::PathController() : ImuGpsSize_(2)
{
    // CartesianXYPoint p0(0.1, 0.2);
    // CartesianXYPoint p1(0.19, 0.29);
    // p0 = p1;
    // CartesianXYPoint p2 = p0 - p1;

    // ImuGpsData imu1 = {0.1, 0.2, 0.3};
    // ImuGpsData imu2 = imu1;
    // imu1.ImuGpsAlat = 0;
    // ImuGpsData imu3 = imu2 - imu1;

    read_ref_path_gps();
    cleanRefPathGpsData();
    refPathGps2XY();
}

PathController::~PathController()
{
}

void PathController::read_ref_path_gps()
{
    ifstream inFile(FILE_PATH, ios::in);
    if (!inFile)
    {
        cout << "打开文件失败" << endl;
        exit(1);
    }

    string line;
    ref_path_points.clear();
    while (getline(inFile, line)) //getline(inFile, line)表示按行读取CSV文件中的数据
    {
        string field;
        istringstream sin(line); //将整行字符串line读入到字符串流sin中
        ImuGpsData tmp = {0};

        getline(sin, field, ','); //将字符串流sin中的字符读入到field字符串中，以逗号为分隔符
        tmp.ImuGpsLat = atof(field.c_str());

        getline(sin, field, ','); //将字符串流sin中的字符读入到field字符串中，以逗号为分隔符
        tmp.ImuGpsLon = atof(field.c_str());

        getline(sin, field, ','); //将字符串流sin中的字符读入到field字符串中，以逗号为分隔符
        tmp.ImuGpsAlat = atof(field.c_str());

        ref_path_points.push_back(tmp);
    }

    inFile.close();
}

void PathController::cleanRefPathGpsData()
{
    bool flag(false);
    Index.clear();
    Index.push_back(0);
    for (int i = 0; i < ref_path_points.size() && !flag;)
    {
        for (int j = i + 1; j < ref_path_points.size(); ++j)
        {
            if (j == ref_path_points.size() - 1)
                flag = true;
            float dist = abs(sqrt(pow(10000000 * ref_path_points[i].ImuGpsLon - 10000000 * ref_path_points[j].ImuGpsLon, 2) + pow(10000000 * ref_path_points[i].ImuGpsLat - 10000000 * ref_path_points[j].ImuGpsLat, 2)));
            if (dist > 0)
            {
                i = j;
                Index.push_back(i);
            }
        }
    }

    clean_path_points.clear();
    for (int k = 0; k < Index.size(); ++k)
    {
        ImuGpsData clean_temp;
        clean_temp = ref_path_points[Index[k]];
        clean_path_points.push_back(clean_temp);
    }
}

// void PathController::refPathGps2XY()
// {
//     cartesian_path_points.clear();
//     CartesianXYPoint cal_tempxy;
//     double wgs84a = 6378.137;
//     double wgs84f = 1.0 / 298.257223563;
//     double wgs84b = wgs84a * (1.0 - wgs84f);
//     double eccsq = 1 - (wgs84b * wgs84b) / (wgs84a * wgs84a);
//     double dtr = M_PI / 180.0;
//     for (int IND = 0; IND < clean_path_points.size(); ++IND)
//     {
//         double clat = cos(dtr * clean_path_points[IND].ImuGpsLat);
//         double clon = cos(dtr * clean_path_points[IND].ImuGpsLon);
//         double slat = sin(dtr * clean_path_points[IND].ImuGpsLat);
//         double slon = sin(dtr * clean_path_points[IND].ImuGpsLon);
//         double dsq = 1.0 - eccsq * slat * slat;
//         double d = sqrt(dsq);
//         double rn = wgs84a / d;
//         double Cal_RefpathX = (rn + 7.5) * clat * clon;
//         double Cal_RefpathY = (rn + 7.5) * clat * slon;
//         cal_tempxy.cartesian_X = (Cal_RefpathX);
//         cal_tempxy.cartesian_Y = (Cal_RefpathY);
//         cartesian_path_points.push_back(cal_tempxy);
//     }
// }

void PathController::refPathGps2XY()
{
    cartesian_path_points.clear();
    CartesianXYPoint cal_tempxy;

    for (int IND = 0; IND < clean_path_points.size(); ++IND)
    {
        cal_tempxy = ConvertGPS2XY(clean_path_points[IND]);

        cartesian_path_points.push_back(cal_tempxy);
    }
}

void PathController::CalculateCarCurrentCartesianXYPt()
{
    double wgs84a = 6378.137;
    double wgs84f = 1.0 / 298.257223563;
    double wgs84b = wgs84a * (1.0 - wgs84f);
    double eccsq = 1 - (wgs84b * wgs84b) / (wgs84a * wgs84a);
    double dtr = M_PI / 180.0;

    double clat1 = cos(dtr * car_current_imu_gps_data.ImuGpsLat);
    double clon1 = cos(dtr * car_current_imu_gps_data.ImuGpsLon);
    double slat1 = sin(dtr * car_current_imu_gps_data.ImuGpsLat);
    double slon1 = sin(dtr * car_current_imu_gps_data.ImuGpsLon);
    double dsq1 = 1.0 - eccsq * slat1 * slat1;
    double d1 = sqrt(dsq1);
    double rn1 = wgs84a / d1;
    double Car_X1 = (rn1 + 7.5) * clat1 * clon1;
    double Car_Y1 = (rn1 + 7.5) * clat1 * slon1;
    car_current_cartisian_point.cartesian_X = Car_X1;
    car_current_cartisian_point.cartesian_Y = Car_Y1;
}

CartesianXYPoint PathController::ConvertGPS2XY(ImuGpsData gps_data)
{
    CartesianXYPoint res;

    double wgs84a = 6378.137;
    double wgs84f = 1.0 / 298.257223563;
    double wgs84b = wgs84a * (1.0 - wgs84f);
    double eccsq = 1 - (wgs84b * wgs84b) / (wgs84a * wgs84a);
    double dtr = M_PI / 180.0;

    double clat = cos(dtr * gps_data.ImuGpsLat);
    double clon = cos(dtr * gps_data.ImuGpsLon);
    double slat = sin(dtr * gps_data.ImuGpsLat);
    double slon = sin(dtr * gps_data.ImuGpsLon);
    double dsq = 1.0 - eccsq * slat * slat;
    double d = sqrt(dsq);
    double rn = wgs84a / d;
    double Car_X = (rn + 7.5) * clat * clon;
    double Car_Y = (rn + 7.5) * clat * slon;

    // convert the calculated result unit:1km to 1m
    res.cartesian_X = Car_X * 1000;
    res.cartesian_Y = Car_Y * 1000;

    return res;
}

void PathController::CarGps2Cal_CarXY()
{
    if (car_history_XY.size() == 0)
    {
        car_history_XY.push(car_current_cartisian_point);
    }
    else
    {
        float dx = car_current_cartisian_point.cartesian_X - car_history_XY.back().cartesian_X;
        float dy = car_current_cartisian_point.cartesian_Y - car_history_XY.back().cartesian_Y;
        float diff_len = sqrt(pow(dx, 2) + pow(dy, 2));
        if (diff_len > 0.2) // if move more than 20cm
        {
            if (car_history_XY.size() < ImuGpsSize_)
            {
                car_history_XY.push(car_current_cartisian_point);
            }
            else
            {
                car_history_XY.pop();
                car_history_XY.push(car_current_cartisian_point);
            }
        }
    }
}

int PathController::calc_pursuit_index() //计算最短路径。
{

    int pursuit_index = 0;
    float min = abs(sqrt(pow(car_current_cartisian_point.cartesian_X - cartesian_path_points[0].cartesian_X, 2) + pow(car_current_cartisian_point.cartesian_Y - cartesian_path_points[0].cartesian_Y, 2)));
    for (int j = 0; j < cartesian_path_points.size(); j++)
    {
        float d = abs(sqrt(pow(car_current_cartisian_point.cartesian_X - cartesian_path_points[j].cartesian_X, 2) + pow(car_current_cartisian_point.cartesian_Y - cartesian_path_points[j].cartesian_Y, 2)));
        if (d < min)
        {
            min = d;
            pursuit_index = j;
        }
    }

    float L = 0.0;
    float Lf = PURSUIT_K * Car_current_velicity + PURSUIT_LFC;
    while (Lf > L && (pursuit_index + 1) < cartesian_path_points.size())
    {
        float dx = cartesian_path_points[pursuit_index + 1].cartesian_X - cartesian_path_points[pursuit_index].cartesian_X;
        float dy = cartesian_path_points[pursuit_index + 1].cartesian_Y - cartesian_path_points[pursuit_index].cartesian_Y;
        L += sqrt(pow(dx, 2) + pow(dy, 2));
        pursuit_index += 1;
    }

    cout << "pursuit_index " << pursuit_index << endl;
    return pursuit_index;
}

Result PathController::pure_pursuit_control(const int pind)
{
    float tx;
    float ty;
    int _pind = pind;
    if (_pind < cartesian_path_points.size())
    {
        tx = cartesian_path_points[_pind].cartesian_X;
        ty = cartesian_path_points[_pind].cartesian_Y;
    }
    else
    {
        tx = cartesian_path_points[cartesian_path_points.size() - 1].cartesian_X; //倒数第一个点
        ty = cartesian_path_points[cartesian_path_points.size() - 1].cartesian_Y;
        _pind = cartesian_path_points.size() - 1; //最后一个点的索引
    }
    float delta = 0;

    // if(car_history_XY.size() == ImuGpsSize_)
    // {
    //     float refx_car0x = (tx - car_history_XY.front().cartesian_X);
    //     float refy_car0y = (ty - car_history_XY.front().cartesian_Y);
    //     float car1x_car0x = (car_history_XY.back().cartesian_X - car_history_XY.front().cartesian_X);
    //     float car1y_car0y = (car_history_XY.back().cartesian_Y - car_history_XY.front().cartesian_Y);
    //     float alpha1 = (refx_car0x * car1x_car0x) + (refy_car0y * car1y_car0y);
    //     float alpha2 = sqrt(pow(refx_car0x, 2) + pow(refy_car0y, 2)) * sqrt(pow(car1x_car0x, 2) + pow(car1y_car0y, 2));
    //     float salpha = alpha1 / alpha2;

    //     if (salpha < -1)
    //     {
    //         salpha = -1;
    //     }
    //     else if (salpha > 1)
    //     {
    //         salpha = 1;
    //     }

    // double sinQ = sqrt(1-salpha*salpha);

    int delta1 = (360 - CarCurrentSteerAngle);
    int YAW = delta1 % 360;

    float H = (90.0f - YAW)；

    float Lf = PURSUIT_K * Car_current_velicity + PURSUIT_LFC;
    float refx_car0x = (tx - car_current_cartisian_point.cartesian_X);
    float refy_car0y = (ty - car_current_cartisian_point.cartesian_Y);
    float slope = atan2(refy_car0y, refx_car0x)*180.0f/M_PI;
    float alpha = slope - H;
    delta = alpha * 180.0f / M_PI * 16.25f;
    // float Dir = alpha * 180 / M_PI * 16.25;

    //delta = atan((2.0 * VEHICLE_LENGTH * sin(alpha)) / Lf) * 180 / M_PI;

    //     if(delta > 30)
    //     {
    //         delta = 30;
    //     }
    //     else if (delta < -30)
    //     {
    //         delta = -30;
    //     }
    //     else
    //     {

    //     }

    //     delta *= 16;
    // }
    // else
    // {
    //     delta = CarCurrentSteerAngle;
    // }

    Result ret;
    ret.delta = delta;
    ret.ind = _pind;
    cout << "cartesian_path_points[_pind].cartesian_X" << cartesian_path_points[_pind].cartesian_X << " cartesian_path_points[_pind].cartesian_Y" << cartesian_path_points[_pind].cartesian_Y << endl;
    cout << "car_current_cartisian_point.cartesian_X" << car_current_cartisian_point.cartesian_X << " car_current_cartisian_point.cartesian_Y" << car_current_cartisian_point.cartesian_Y << endl;
    cout << "ret:" << ret.delta << " ret.ind" << ret.ind << endl;
    return ret; //返回角度和最近点的索引。
}

void PathController::Velocity_control()
{

    if (Car_current_velicity < 5)
    {
        target_acc = 0.25;
    }
    else if (Car_current_velicity > 7)
    {
        target_acc = -0.25;
    }
    else
    {
        target_acc = 0;
    }
}

void PathController::statemachine_task()
{
    car_current_cartisian_point = ConvertGPS2XY(car_current_imu_gps_data);
    CarGps2Cal_CarXY();
    switch (sm_state)
    {
    case SM_OFF:
        if (cartesian_path_points.size() != 0) // TODO: 1. read reference path and convert to s-l , return finishend.
        {
            sm_state = SM_STANDBY;
        }

        chassis_ctr_handshake_req = false;
        break;

    case SM_STANDBY:
        // TODO: 2 . if funtion on condition is reached.
        // active = Automobile_Active();
        active = true;
        if (active && current_gear == GEAR_P)
        {
            sm_state = SM_HANDSHAKE;
        }

        chassis_ctr_handshake_req = false;
        break;
    case SM_HANDSHAKE:
        // TODO: 3. if eps controlled is OK
        if (chassis_be_controlled == true)
        {
            sm_state = SM_ACTIVE;
        }
        chassis_ctr_handshake_req = true;
        break;
    case SM_ACTIVE:
        TargetInd = calc_pursuit_index();
        // TODO:  4 mini obstacle length < 10 meter
        if (dist_mini_obstacle < 4)
        {
            sm_state = SM_BRAKE;
        }

        else if (TargetInd == cartesian_path_points.size() - 1) // TODO 5 reach the reference path end
        {
            sm_state = SM_COMPLETE;
        }
        else
        {
            //TODO : SM active actions: calculate the output parameters
            Result ret;
            ret = pure_pursuit_control(TargetInd);
            target_steer_ang = ret.delta;
            Velocity_control();
            // then output
            chassis_ctr_handshake_req = true;
            target_gear = GEAR_D;
        }
        break;
    case SM_BRAKE:
        if (dist_mini_obstacle > 6)
        {
            sm_state = SM_ACTIVE;
        }
        else
        {
            chassis_ctr_handshake_req = true;
            target_acc = -0.5;
        }
        break;
    case SM_COMPLETE:
        if (current_gear == GEAR_P)
        {
            // jump to off, wait for next function on .
            sm_state = SM_OFF;
        }
        else
        {
            chassis_ctr_handshake_req = true;
            target_acc = -0.5;
            if (Car_current_velicity <= 0.2)
            {
                target_gear = GEAR_P;
            }
        }
        break;
    default:
        break;
    }
    //printf("SM state: %d, steer angle: %f \n", sm_state, target_steer_ang);
    // printf("SM state: %d, arget_steer_ang: %f, cam_distance: %f \n", sm_state, target_steer_ang, dist_mini_obstacle);
}

#ifndef PLANING_CONTROL_H
#define PLANING_CONTROL_H

#include <cstdio>
#include <cmath>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <string>
#include <vector>
#include <queue>


#define  FILE_PATH  "/home/titan/Bo.csv"
const float VEHICLE_LENGTH = 2.7f;
const float PURSUIT_K   = 0.1f;
const float PURSUIT_LFC = 2;
 

struct CartesianXYPoint
{
    float cartesian_X;
    float cartesian_Y;
    CartesianXYPoint() : cartesian_X(0.0f), cartesian_Y(0.0f)
    {
    }

    CartesianXYPoint(const float _x, const float _y)
        : cartesian_X(_x), cartesian_Y(_y)
    {
    }

    CartesianXYPoint &operator=(const CartesianXYPoint &obj)
    {
        this->cartesian_X = obj.cartesian_X;
        this->cartesian_Y = obj.cartesian_Y;
        return *this;
    }
};

struct ImuGpsData
{
    double ImuGpsLon;
    double ImuGpsLat;
    double ImuGpsAlat;
};



enum Gear
{
    GEAR_NONE = 0,
    GEAR_P = 1,
    GEAR_R = 2,
    GEAR_N = 3,
    GEAR_D = 4,
};

struct Result
{
    double delta;
    int ind;
};

enum StateMachSts_st
{
    SM_OFF,
    SM_STANDBY,
    SM_HANDSHAKE,
    SM_ACTIVE,
    SM_BRAKE,
    SM_COMPLETE,
};



/**
 * @brief, As following steps: 
 *         1. Get reference GPS path, by csv files;
 *         2. Clean data;
 *         3. Transform reference path GPS data to XY coordinate;
 *         4. Vehicle GPS pose to XY;
 *         5. Get pursiut index;
 *         6. Get pursuit angle;    
 */
class PathController
{

public:
    PathController();
    ~PathController();

public:

    void statemachine_task();

public:
    // input parameters:--------------------------------------------------如何给直接给
    float Car_current_velicity; // uinit: m/s^2
    float dist_mini_obstacle;   // uinit: m
    ImuGpsData car_current_imu_gps_data;
    float CarCurrentSteerAngle;
    bool chassis_be_controlled;
    enum Gear current_gear;
    StateMachSts_st sm_state;
    // output
    enum Gear target_gear;
    float target_acc;       //uinit: m/s^2
    float target_steer_ang; // unit: degree -540 ~ +540
    bool chassis_ctr_handshake_req;

private:

    void ReadRefpathGPS_lon_lat_alat();

    void cleanRefPathGpsData();

    void refPathGps2XY();
    void CarGps2Cal_CarXY();

    void CalculateCarCurrentCartesianXYPt();
    int calc_pursuit_index(); //计算最短路径。
    Result pure_pursuit_control(int pind);
    // bool Automobile_Active();
    void Velocity_control();
    void read_ref_path_gps();
    CartesianXYPoint ConvertGPS2XY(ImuGpsData gps_data);

private:
    uint8_t ImuGpsSize_;
    std::vector<ImuGpsData> ref_path_points;
    std::vector<ImuGpsData> clean_path_points;
    std::vector<int> Index;

    std::vector<CartesianXYPoint> cartesian_path_points;
    std::queue<CartesianXYPoint> car_history_XY;
    std::vector<CartesianXYPoint> pts;
    CartesianXYPoint car_current_cartisian_point;
    CartesianXYPoint last_car_current_cartisian_point;
};

#endif