modularization/src/decition/mpc-linear/dps_routeplan.cpp

#include "dps/inc/dps.h"
#include "qmath.h"
#include "bus/inc/bus_manager.h"
#include <vector>







void Dps::DistanceCalculate(double currentLongitude, double currentLatitude, double targetLongitude, double targetLatitude,DistanceInfo &returnDis)
{
    DistanceInfo distance;

    distance.disx=qAbs(currentLongitude-targetLongitude)*qCos(targetLatitude/180*M_PI)*111700;
    distance.disy=qAbs(currentLatitude-targetLatitude)*111700;
    distance.dis=qSqrt(distance.disx*distance.disx+distance.disy*distance.disy);//当前点与临时点坐标差
    returnDis = distance;
}



double Dps::GetHead(double aLongitude,double aLatitude,double bLongitude,double bLatitude)
{
    double dx;
    double dy;
    double angle;
    dx = (bLongitude - aLongitude) * 111700 * cos(aLatitude / 180 * M_PI);//当前点与最近点经度距离
    dy = (bLatitude - aLatitude) * 111700;//当前点与最近点纬度距离

    if(dx>-0.0001&&dx<0.0001)
    {
        if(dy>0)
            angle=0;
        else
            angle=180;
    }
    else
    {
        angle = qAtan((double)dy / dx) / M_PI * 180;//当前点与最近点两点连线与正北的夹角
        if (dx > 0) angle = 90 - angle;
        else angle = 270 - angle;
    }
    return angle;
}

void Dps::GetUsualPointKey(const QList<GPSInfo> &gpsList,double currentLongitude,double currentLatitude,double currentHead,double vehicleSpeed,const KeyPointInfo &keyPointLast,KeyPointInfo &retKeyPoint)
{
    KeyPointInfo keyPoint;//关键点信息
    int gpsListsize;//传入路径list大小
    GPSInfo memoryGps;//临时GPS信息
    double memoryLongitude;//临时经度
    double memoryLatitude;//临时纬度
    double memoryHead;//临时航向角
    double disHead;//航向差
    int preNum;//检索路径点个数
    DistanceInfo dis;//两点间距
    double currentGear;//当前档位
    int desiredGear;//期望档位
    double minDistance = MINDISTANCE;//寻找keyPoint的最小值
    double rtkSpd;
    int begine;
    double preDis;
    keyPoint = keyPointLast;
    begine = keyPointLast.num;

    gpsListsize=gpsList.size();
    DistanceInfo pointdistance;

    DistanceCalculate(gpsList.at(9).iLongitude,gpsList.at(9).iLatitude,gpsList.at(10).iLongitude,gpsList.at(10).iLatitude,pointdistance);
    pointdistance.dis=qAbs(pointdistance.dis);
    //qDebug()<<"间距："<<pointdistance.dis;

    preDis =  vehicleSpeed*2*0.2;

    preNum = qRound(preDis/pointdistance.dis);
    if(preDis < 6)
    {
        preNum = qRound(6/pointdistance.dis);
    }
    if(preDis > 14)
    {
        preNum = qRound(14/pointdistance.dis);
    }
    m_datapool->GetValue(desiredGear,DataPool::DESIRESHIFT);
    m_datapool->GetValue(currentGear,DataPool::VEHSFTPSNG);
    QVariant ina;
    ina = m_datapool->GetValue(DataPool::RTKDATA,0);
    rtkSpd = ina.value<RtkData>().gSpd;

    if((desiredGear==2&&currentGear==4)||(desiredGear==4&&currentGear==2))
    {
        preDis = 20;
        preNum = qRound(preDis/pointdistance.dis);
        begine =keyPointLast.num+qRound(6/pointdistance.dis);

    }
    int jLimit;
    //qDebug()<<"preNum:"<<preNum;
    jLimit=qMin(keyPointLast.num+preNum,gpsListsize);
    bool pointFlag=false;
    for (int j = begine ; j<jLimit; j++)
    {
        memoryGps = gpsList.at(j);
        memoryLongitude = memoryGps.iLongitude;
        memoryLatitude = memoryGps.iLatitude;

        DistanceCalculate(currentLongitude,currentLatitude,memoryLongitude,memoryLatitude,dis);
        if((desiredGear==2&&currentGear==4)||(desiredGear==4&&currentGear==2))
        {
            memoryHead=GetHead(currentLongitude,currentLatitude,memoryLongitude,memoryLatitude);//当前点到计算点连线与正北方向夹角
        }
        else
        {
            memoryHead=memoryGps.iHead;
        }
        disHead=qAbs(currentHead-memoryHead);
        if(disHead>M_PI)
            disHead=disHead-2*M_PI;
        if(disHead<-M_PI)
            disHead=2*M_PI+disHead;

        if((desiredGear==2&&currentGear==4)||(desiredGear==4&&currentGear==2))
        {
            if((minDistance > dis.dis)&&((disHead>90)&&(disHead<270)))
            {
                minDistance = dis.dis;
                keyPoint.num=memoryGps.num;
                keyPoint.id=memoryGps.id;
                keyPoint.type=memoryGps.type;
                pointFlag = true;
                timeCount = 0;
                historygear=currentGear;
            }
        }
        else
        {
            if((minDistance > dis.dis)&&((disHead<90)||(disHead>270)))
            {
                minDistance = dis.dis;
                keyPoint.num=memoryGps.num;
                keyPoint.id=memoryGps.id;
                keyPoint.type=memoryGps.type;
                pointFlag = true;
                timeCount = 0;
            }
        }
    }
    if(!pointFlag)
    {
        if(timeCount == 0)
        {
            newPointtime.restart();
        }
        timeCount++;
        if((vehicleSpeed > 1||rtkSpd >1) && newPointtime.elapsed() > 1000)
        {
            GetGlobalPointKey(gpsList,currentLongitude,currentLatitude,currentHead,keyPointLast.num,keyPoint);
            timeCount = 0;
        }
        else
        {
            keyPoint = keyPointLast;
        }
    }
    retKeyPoint = keyPoint;
}






void Dps::LateralPlan()//横向规划：输出关键点、横向偏差、航向偏差
{
    int ij;
    double tLng;//实时经度
    double tLat;//实时纬度
    double tHead;//实时航向角
    double currentHead;//实时航向角
    SpecialCurve specialCurve;
    double mCur;//临时曲率
    double mLngs;
    double mLats;
    PrePoint prePoint;
    int prepointcount;
    int prepointcount2;
    double prepointdis;
    double prepointdis2;
    double m_angle = 0;//航向偏差
    double m_anglex[5];
    double pointangle;//当前点与预瞄点连线夹角
    double pointangle2;//当前点与关键点连线夹角
    double near_angle;
    double vehicleSpeed;//当前车速
    double strAng;//当前方向盘转角-0811
    KeyPointInfo keyPointLast;//上次计算的关键点
    KeyPointInfo keyPointLastRef;//上次计算的关键点
    QList<GPSInfo> pGpsList;
    double curveatKeypoint;   //关键点附近的曲率
    double curveatPreview;    //预瞄点1附近的曲率
    double curveatPreview2;   //预瞄点2附近的曲率
    double pointDisx = 0;  //横向偏差

    QVariant ina;
    ina = m_datapool->GetValue(DataPool::RTKDATA,0);
    tLng = ina.value<RtkData>().gLng;
    tLat = ina.value<RtkData>().gLat;
    tHead = ina.value<RtkData>().gHead;
    currentHead = ina.value<RtkData>().gHead;

    QVariant inb;
    inb = m_datapool->GetValue(DataPool::VEHSPD,0);
    vehicleSpeed = inb.value<double>();

    QVariant inc;
    inc = m_datapool->GetValue(DataPool::KEYPOINTINFO,0);
    keyPointLast = inc.value<KeyPointInfo>();

    auto ind = m_datapool->GetValue(DataPool::TMS_GPS_PATH, 0);
    auto *gpsinfo = ind.value<void*>();
    QList<GPSInfo> *normPath = (QList<GPSInfo>*)gpsinfo;
    if(normPath)
    {
        pGpsList=*normPath;
    }

    QVariant ine;//取方向盘转角值
    ine = m_datapool->GetValue(DataPool::VEHSTRANG,0);
    strAng = ine.value<double>();




    GetUsualPointKey( pGpsList,tLng,tLat,tHead,vehicleSpeed,keyPointLast, dpsKeyPoint);//计算车辆位置的关键点

    QVariant outa;
    outa = QVariant::fromValue(dpsKeyPoint);
    m_datapool->SetValue(DataPool::KEYPOINTINFO,outa);


    pointangle2=GetHead(tLng,tLat, pGpsList.at(dpsKeyPoint.num).iLongitude, pGpsList.at(dpsKeyPoint.num).iLatitude);


    /*计算预瞄轨迹*/
    QList<UTMInfo> globalUTMList = m_datapool->GetValue(DataPool::UTMROUT, 0).value<QList<UTMInfo>>();
    double cur_X,cur_Y,cur_heading;
     m_gpsCoorTrans.LatLonToUTMXY(tLat, tLng, 50, cur_X, cur_Y);
     cur_heading=currentHead/180*M_PI;
     float path_length = 0.0,dx=0.0,dy=0.0;
     std::vector<UTMInfo> ref_traj;
    for (int i =  dpsKeyPoint.num; i < globalUTMList.size()-1; i++) {  //车辆坐标系下参考轨迹
                UTMInfo traj;
                double x_t,y_t,heading_t;
                x_t = globalUTMList.at(i).x - cur_X;
                y_t = globalUTMList.at(i).y - cur_Y;
                traj.y= x_t * sin(M_PI*3.0/2.0+cur_heading) + y_t * cos(M_PI*3.0/2.0+cur_heading);
                traj.x = x_t * cos(M_PI*3.0/2.0+cur_heading) - y_t * sin(M_PI*3.0/2.0+cur_heading);//rtk to rear  para广德的车要减0.9
                traj.yaw = -(dpsGlobalPathList.at(i).iHead/180*M_PI - cur_heading);
                if(traj.yaw < -M_PI){
                    traj.yaw = traj.yaw + 2*M_PI;
                }
                if(traj.yaw > M_PI){
                    traj.yaw = traj.yaw - 2*M_PI;
                }
                traj.k=dpsGlobalPathList.at(i).iCurvature;

                ref_traj.push_back(traj);

                if(i > 0){
                    dx = globalUTMList.at(i).x  - globalUTMList.at(i-1).x ;
                    dy = globalUTMList.at(i).y  - globalUTMList.at(i-1).y ;
                    path_length += sqrt(dx*dx+dy*dy);
                }
                if((path_length > vehicleSpeed/3.6*2)&&(path_length >5)){
                    break;
                }
    }

    double dist = 0;
    std::vector<UTMInfo> mpc_ref_traj;
    if(mpc_ref_traj.size()>0)
    {
        mpc_ref_traj.clear();
    }
    mpc_ref_traj.push_back(ref_traj.at(0));
    for(int i = 1;i < ref_traj.size();i++){
                dx = ref_traj.at(i).x - ref_traj.at(i-1).x;
                dy = ref_traj.at(i).y - ref_traj.at(i-1).y;
                dist += sqrt(dx*dx + dy*dy);
                if(dist > (vehicleSpeed/3.6*0.02)){
                    mpc_ref_traj.push_back(ref_traj.at(i-1));
                    dist =  dist - vehicleSpeed/3.6*0.02;
                }
                if(mpc_ref_traj.size() > 30){
                    break;
                }
    }

    /*将横向偏差、航向偏差、关键点、预瞄点、关注点曲率信息放入datapool*/
    angleExpect.biaDistance=pointDisx;


    angleExpect.biaHead=m_angle;

    QVariant outb;
    outb = QVariant::fromValue(angleExpect);
    m_datapool->SetValue(DataPool::HORIZONTALDISIRED,outb);

    prePoint.prePointCount=prepointcount;
    prePoint.prePointDistance=prepointdis;
    QVariant outc;
    outc = QVariant::fromValue(prePoint);
    m_datapool->SetValue(DataPool::PREPOINT,outc);



    QVariant outg;
    outg = QVariant::fromValue(mpc_ref_traj);
    m_datapool->SetValue(DataPool::REFTRAIL,outg);


}