modularization/src/decition/decition_brain/decision/decide_gps_00.cpp

#include <decision/decide_gps_00.h>
#include <decision/adc_tools/compute_00.h>
#include <decision/adc_tools/gps_distance.h>
#include <decision/decition_type.h>
#include <decision/adc_tools/transfer.h>
#include <decision/obs_predict.h>
#include <common/constants.h>
#include <common/car_status.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <control/can.h>
#include <time.h>
#include <decision/adc_controller/base_controller.h>
#include <decision/adc_controller/pid_controller.h>
#include <decision/adc_planner/lane_change_planner.h>
#include <decision/adc_planner/frenet_planner.h>
#include <QTime>

using namespace std;

#include "ivlog.h"
extern iv::Ivlog * givlog;

#define PI (3.1415926535897932384626433832795)

iv::decision::DecideGps00::DecideGps00() {

}
iv::decision::DecideGps00::~DecideGps00() {

}

float iv::decision::DecideGps00::xiuzhengCs=0;

int iv::decision::DecideGps00::PathPoint = -1;
double iv::decision::DecideGps00::minDis = iv::MaxValue;
double iv::decision::DecideGps00::maxAngle = iv::MinValue;
//int iv::decition::DecideGps00::lastGpsIndex = iv::MaxValue;
int iv::decision::DecideGps00::lastGpsIndex = 0;
double iv::decision::DecideGps00::controlSpeed = 0;
double iv::decision::DecideGps00::controlBrake = 0;
double iv::decision::DecideGps00::controlAng = 0;
double iv::decision::DecideGps00::Iv = 0;
double iv::decision::DecideGps00::lastU = 0;
double iv::decision::DecideGps00::lastVt = 0;
double iv::decision::DecideGps00::lastEv = 0;

int iv::decision::DecideGps00::gpsLineParkIndex = 0;
int iv::decision::DecideGps00::gpsMapParkIndex = 0;
double iv::decision::DecideGps00::lastDistance = MaxValue;
double iv::decision::DecideGps00::lastPreObsDistance = MaxValue;
double iv::decision::DecideGps00::obsDistance = -1;
double iv::decision::DecideGps00::obsSpeed=0;
double iv::decision::DecideGps00::obsDistanceAvoid = -1;
double iv::decision::DecideGps00::lastDistanceAvoid = -1;

double iv::decision::DecideGps00::lidarDistance = -1;
double iv::decision::DecideGps00::myesrDistance = -1;
double iv::decision::DecideGps00::lastLidarDis = -1;

bool iv::decision::DecideGps00::parkMode = false;
bool iv::decision::DecideGps00::readyParkMode = false;

bool iv::decision::DecideGps00::zhucheMode = false;
bool iv::decision::DecideGps00::readyZhucheMode = false;
bool iv::decision::DecideGps00::hasZhuched = false;

double iv::decision::DecideGps00::lastLidarDisAvoid = -1;
double iv::decision::DecideGps00::lidarDistanceAvoid = -1;

int iv::decision::DecideGps00::finishPointNum = -1;
int iv::decision::DecideGps00::zhuchePointNum = 0;

///kkcai, 2020-01-03
bool iv::decision::DecideGps00::isFirstRun = true;
//////////////////////////////////////////////

float iv::decision::DecideGps00::minDecelerate=0;
//bool iv::decition::DecideGps00::startingFlag = true;//起步标志，在起步时需要进行frenet规划。

double offset_real = 0;
double realspeed;
double dSpeed;
double dSecSpeed;
double secSpeed;
double ac;


iv::Point2D obsPoint(-1, -1);
iv::Point2D obsPointAvoid(-1, -1);


int esrIndex = -1;
int esrIndexAvoid = -1;
double obsSpeedAvoid = 0;

double esrDistance = -1;
double esrDistanceAvoid = -1;
int obsLostTime = 0;
int obsLostTimeAvoid = 0;

// double avoidTime = 0;


double avoidX =0;
float roadWidth = 3.5;
int roadSum =10;
int roadNow = 0;
int roadOri =0;
int roadPre = -1;
int lastRoadOri = 0;

int lightTimes = 0;


int lastRoadNum=1;

int stopCount = 0;

int gpsMissCount = 0;

bool rapidStop = false;

bool hasTrumpeted = false;


bool changeRoad=false;
//实验手刹
bool handFirst = true;
double handTimeSpan = 0;
double handStartTime = 0;
double handLastTime = 0;
bool handPark = false;
long handParkTime=10000;

//喇叭
bool trumpetFirstCount=true;
double trumpetTimeSpan = 0;
double trumpetStartTime = 0;
double trumpetLastTime = 0;

//过渡
bool transferFirstCount=true;
double transferTimeSpan = 0;
double transferStartTime = 0;
double transferLastTime = 0;

bool busMode = false;
bool parkBesideRoad=false;

bool chaocheMode = false;
bool specialSignle = false;

double offsetX = 0;

double steerSpeed=9000;

bool transferPieriod;
bool transferPieriod2;
double traceDevi;

bool startingFlag = true;  //起步标志，在起步时需要进行frenet规划。
bool useFrenet = false;    //标志是否启用frenet算法避障
bool useOldAvoid = true;   //标志是否用老方法避障

enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,
                waitAvoid ,shoushaTest,justRun, changingRoad, chaoche, chaocheBack, reverseCar,reversing, reverseCircle, reverseDirect,reverseArr,
                dRever,dRever0,dRever1,dRever2,dRever3,dRever4} vehState;


std::vector<iv::Point2D>  gpsTraceAim; //记录变道后的目标路径，若车辆经frenet规划行驶至该路径上后，就关闭frenet规划
std::vector<iv::Point2D>  gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri,gpsTraceRear;

std::vector<double> esrDisVector(roadSum, -1);
std::vector<double> lidarDisVector(roadSum, -1);
std::vector<double> obsDisVector(roadSum,-1);
std::vector<double> obsSpeedVector(roadSum, 0);

std::vector<int> obsLostTimeVector(roadSum, 0);

std::vector<double> lastLidarDisVector(roadSum, -1);
std::vector<double> lastDistanceVector(roadSum, -1);

bool qiedianCount = false;
//日常展示

iv::decision::Decition iv::decision::DecideGps00::getDecideFromGPS(GPS_INS now_gps_ins,
                                                                   const std::vector<GPSData> gpsMapLine,
                                                                   iv::LidarGridPtr lidarGridPtr,
                                                                   std::vector<iv::Perception::PerceptionOutput> lidar_per,
                                                                   iv::TrafficLight trafficLight)
{
    //   QTime xTime;
    //    xTime.start();
    //39.14034649083606 117.0863975920476 20507469.630314853  4334165.046101382 353
    Decition gps_decition(new DecitionBasic);
    //    vector<iv::Point2D> fpTraceTmp;


    //如果useFrenet、useOldAvoid两者不互为相异，则采用原来的方法。“^”为异或运算符。
    if(!(useFrenet^useOldAvoid)){
        useFrenet = false;
        useOldAvoid = true;
    }

    //    //如果useFrenet、useOldAvoid两者均为真或均为假，则采用原来的方法
    //    if(useFrenet&&useOldAvoid || !useFrenet&&!useOldAvoid){
    //        useFrenet = false;
    //        useOldAvoid = true;
    //    }

    //  ServiceCarStatus.group_control=false;




    //    GPS_INS gps= Coordinate_UnTransfer(0,1.5,now_gps_ins);
    //    now_gps_ins.gps_x=gps.gps_x;
    //    now_gps_ins.gps_y=gps.gps_y;

    //    GaussProjInvCal(now_gps_ins.gps_x, now_gps_ins.gps_y,&now_gps_ins.gps_lng, &now_gps_ins.gps_lat);


    if (isFirstRun)
    {
        initMethods();
        vehState = normalRun;
        startAvoid_gps_ins = now_gps_ins;
        init();
        PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine,
                                                          DecideGps00::lastGpsIndex,
                                                          DecideGps00::minDis,
                                                          DecideGps00::maxAngle);
        DecideGps00::lastGpsIndex = PathPoint;

        if(ServiceCarStatus.speed_control==true){
            Compute00().getDesiredSpeed(gpsMapLine);   //add by zj
        }

        //	ServiceCarStatus.carState = 1;
        //        roadOri = gpsMapLine[PathPoint]->roadOri;
        //        roadNow = roadOri;
        //        roadSum = gpsMapLine[PathPoint]->roadSum;
        //  busMode = false;
        //  vehState = dRever;

        double dis =  GetDistance(*gpsMapLine[0], *gpsMapLine[gpsMapLine.size()-1]);
        if(dis<15){
            circleMode=true;  //201200102
        }else{
            circleMode=false;
        }
        //     circleMode = true;


        getV2XTrafficPositionVector(gpsMapLine);
        //        group_ori_gps=*gpsMapLine[0];
        ServiceCarStatus.bocheMode=false;
        ServiceCarStatus.daocheMode=false;
        parkMode=false;
        readyParkMode=false;
        finishPointNum=-1;
        isFirstRun = false;
    }


    if(ServiceCarStatus.msysparam.gpsOffset_x!=0 || ServiceCarStatus.msysparam.gpsOffset_y!=0 ){
        GPS_INS gpsOffset = Coordinate_UnTransfer(ServiceCarStatus.msysparam.gpsOffset_x, ServiceCarStatus.msysparam.gpsOffset_y, now_gps_ins);
        now_gps_ins.gps_x=gpsOffset.gps_x;
        now_gps_ins.gps_y=gpsOffset.gps_y;
        GaussProjInvCal(now_gps_ins.gps_x,  now_gps_ins.gps_y, &now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
    }

    //1227
    //  ServiceCarStatus.daocheMode=true;

    //1220
    changingDangWei=false;

    minDecelerate=0;
    if(now_gps_ins.gps_lat<0||now_gps_ins.gps_lat>90){
        //  int a=0;
        gps_decition->wheel_angle = 0;
        gps_decition->speed = dSpeed;

        gps_decition->accelerator = -0.5;
        gps_decition->brake=10;
        return gps_decition;
    }





    //1220


    if(ServiceCarStatus.daocheMode){

        now_gps_ins.ins_heading_angle=now_gps_ins.ins_heading_angle+180;
        if( now_gps_ins.ins_heading_angle>=360)
            now_gps_ins.ins_heading_angle= now_gps_ins.ins_heading_angle-360;
    }
    //1220 end



    //1125 traficc
    traffic_light_gps.gps_lat=ServiceCarStatus.iTrafficeLightLat;
    traffic_light_gps.gps_lng=ServiceCarStatus.iTrafficeLightLon;
    GaussProjCal(traffic_light_gps.gps_lng,traffic_light_gps.gps_lat, &traffic_light_gps.gps_x, &traffic_light_gps.gps_y);


    //xiesi
    ///kkcai, 2020-01-03
    //ServiceCarStatus.busmode=true;
    //ServiceCarStatus.busmode=false;//20200102
    ///////////////////////////////////////////////////


    //busMode = ServiceCarStatus.busmode;
    nearStation=false;

    gps_decition->leftlamp = false;
    gps_decition->rightlamp = false;
    //    qDebug("heading is %g",now_gps_ins.ins_heading_angle);


    aim_gps_ins.gps_lat= ServiceCarStatus.mfParkLat;
    aim_gps_ins.gps_lng= ServiceCarStatus.mfParkLng;

    aim_gps_ins.ins_heading_angle= ServiceCarStatus.mfParkHeading;




    GaussProjCal(aim_gps_ins.gps_lng,aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
    is_arrivaled = false;




    //  xiuzhengCs=-0.8;  //1026

    xiuzhengCs=0;
    //    if (parkMode)
    //    {


    //        handBrakePark(gps_decition,10000,now_gps_ins);
    //        return gps_decition;
    //    }





    realspeed = now_gps_ins.speed;

    secSpeed = realspeed / 3.6;




    //sidePark

    if(ServiceCarStatus.mnParkType==1){

        if( vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 &&  vehState!=dRever2  &&  vehState!=dRever3 && vehState!=dRever4 && vehState!=reverseArr ){
            int bocheEN=Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
            if(bocheEN==1){
                ServiceCarStatus.bocheEnable=1;
            }else if(bocheEN==0){
                ServiceCarStatus.bocheEnable=0;
            }
        }else{
            ServiceCarStatus.bocheEnable=2;
        }



        if(ServiceCarStatus.bocheMode && vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 &&
                vehState!=dRever2  &&  vehState!=dRever3 && vehState!=dRever4&&  vehState!=reverseArr ){
            if(abs(realspeed)>0.1){
                dSpeed=0;
                minDecelerate=min(minDecelerate,-0.5f);
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
                gps_decition->wheel_angle=0;
                return gps_decition;
            }else{
                if(trumpet()>3000){
                    trumpetFirstCount=true;
                    vehState=dRever;
                }

            }
            //         ServiceCarStatus.bocheMode=false;
        }

    }


    //chuizhiPark

    if(ServiceCarStatus.mnParkType==0){
        if( vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle &&  vehState!=reverseDirect &&  vehState!=reverseArr ){
            int bocheEN=Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
            if(bocheEN==1){
                ServiceCarStatus.bocheEnable=1;
            }else if(bocheEN==0){
                ServiceCarStatus.bocheEnable=0;
            }
        }else{
            ServiceCarStatus.bocheEnable=2;
        }



        if(ServiceCarStatus.bocheMode && vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle &&  vehState!=reverseDirect&&  vehState!=reverseArr ){

            if(abs(realspeed)>0.1){
                dSpeed=0;
                minDecelerate=min(minDecelerate,-0.5f);
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
                gps_decition->wheel_angle=0;
                return gps_decition;
            }else{
                if(trumpet()>3000){
                    trumpetFirstCount=true;
                    vehState=reverseCar;
                }

            }
            //     ServiceCarStatus.bocheMode=false;

        }
    }
    // ChuiZhiTingChe
    if (vehState == reverseCar)
    {
        Compute00().bocheCompute(now_gps_ins, aim_gps_ins);

        vehState = reversing;
        qiedianCount=true;


    }
    if (vehState == reversing)
    {
        double dis = GetDistance(now_gps_ins, Compute00().nearTpoint);
        if (dis<2.0)//0.5
        {
            vehState = reverseCircle;
            qiedianCount = true;
            cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
        }
        else {
            controlAng = 0;
            dSpeed = 2;
            dSecSpeed = dSpeed / 3.6;
            gps_decition->wheel_angle = 0;
            gps_decition->speed = dSpeed;
            obsDistance=-1;
            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            //	gps_decition->accelerator = 0;
            return gps_decition;
        }
    }

    if (vehState == reverseCircle)
    {
        double dis = GetDistance(now_gps_ins, Compute00().farTpoint);
        double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
        if((((angdis<5)||(angdis>355)))&&(dis<3.0))
            //       if((((angdis<4)||(angdis>356)))&&(dis<2.0))
        {
            vehState = reverseDirect;
            qiedianCount = true;
            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;

        }
        else {
            if (qiedianCount && trumpet()<0)
                //         if (qiedianCount && trumpet()<1500)
            {

                //                gps_decition->brake = 10;
                //                gps_decition->torque = 0;
                dSpeed=0;
                minDecelerate=min(minDecelerate,-0.5f);
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            }
            else
            {
                qiedianCount = false;
                trumpetFirstCount = true;


                dSpeed = 2;
                dSecSpeed = dSpeed / 3.6;
                gps_decition->speed = dSpeed;
                obsDistance=-1;
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);

            }

            controlAng = Compute00().bocheAngle*16.5;
            gps_decition->wheel_angle = 0 - controlAng*1.05;

            cout<<"farTpointDistance================"<<dis<<endl;

            return gps_decition;
        }
    }

    if (vehState == reverseDirect)
    {
        //	double dis = GetDistance(now_gps_ins, aim_gps_ins);
        Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);

        if ((pt.y)<0.5)
        {

            qiedianCount=true;
            vehState=reverseArr;
            cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
            //            gps_decition->accelerator = -3;
            //            gps_decition->brake = 10;
            //            gps_decition->torque = 0;
            gps_decition->wheel_angle=0;
            dSpeed=0;
            minDecelerate=min(minDecelerate,-0.5f);
            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            return gps_decition;




        }
        else {

            //if (qiedianCount && trumpet()<0)
            if (qiedianCount && trumpet()<1000)
            {

                //                gps_decition->brake = 10;
                //                gps_decition->torque = 0;
                dSpeed=0;
                minDecelerate=min(minDecelerate,-0.5f);
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            }
            else
            {
                qiedianCount = false;
                trumpetFirstCount = true;
                dSpeed = 1;
                dSecSpeed = dSpeed / 3.6;
                gps_decition->speed = dSpeed;
                obsDistance=-1;
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            }



            controlAng = 0;

            gps_decition->wheel_angle = 0;

            return gps_decition;
        }
    }



    if (vehState == reverseArr)
    {
        //

        ServiceCarStatus.bocheMode=false;
        if (qiedianCount && trumpet()<1500)
        {

            //            gps_decition->brake = 10;
            //            gps_decition->torque = 0;
            dSpeed=0;
            minDecelerate=min(minDecelerate,-0.5f);
            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            ServiceCarStatus.bocheMode=false;
        }
        else
        {
            qiedianCount = false;
            trumpetFirstCount = true;
            ServiceCarStatus.bocheEnable=0;
            vehState=normalRun;
            ServiceCarStatus.mbRunPause=true;
            ServiceCarStatus.mnBocheComplete=100;
            cout<<"泊车daowei mbRunPause+++++++++++++++++++++++++++++++++"<<endl;
        }


        gps_decition->wheel_angle = 0 ;



        return gps_decition;

    }



    //ceFangWeiBoChe

    if (vehState == dRever)
    {
        GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);

        Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);

        vehState = dRever0;
        qiedianCount=true;


        std::cout<<"enter side boche mode"<<std::endl;



    }
    if (vehState == dRever0)
    {
        double dis = GetDistance(now_gps_ins, Compute00().dTpoint0);
        if (dis<2.0)
        {
            vehState = dRever1;
            qiedianCount = true;
            cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
        }
        else {
            controlAng = 0;
            dSpeed = 2;
            dSecSpeed = dSpeed / 3.6;
            gps_decition->wheel_angle = 0;
            gps_decition->speed = dSpeed;
            obsDistance=-1;
            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            //	gps_decition->accelerator = 0;
            return gps_decition;
        }
    }


    if (vehState == dRever1)
    {
        double dis = GetDistance(now_gps_ins, Compute00().dTpoint1);

        if(dis<2.0)
        {
            vehState = dRever2;
            qiedianCount = true;
            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;

        }
        else {
            if (qiedianCount && trumpet()<1000)

            {

                //                gps_decition->brake = 10;
                //                gps_decition->torque = 0;
                dSpeed=0;
                minDecelerate=min(minDecelerate,-0.5f);
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            }
            else
            {
                qiedianCount = false;
                trumpetFirstCount = true;


                dSpeed = 2;
                dSecSpeed = dSpeed / 3.6;
                gps_decition->speed = dSpeed;
                obsDistance=-1;
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);

            }

            controlAng = Compute00().dBocheAngle*16.5;
            gps_decition->wheel_angle = 0 - controlAng;

            cout<<"farTpointDistance================"<<dis<<endl;

            return gps_decition;
        }
    }


    if (vehState == dRever2)
    {
        double dis = GetDistance(now_gps_ins, Compute00().dTpoint2);
        Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
        Point2D pt2 = Coordinate_Transfer(Compute00().dTpoint2.gps_x,Compute00().dTpoint2.gps_y, aim_gps_ins);
        double xx= (pt1.x-pt2.x);
        // if(xx>0)
        if(xx>-0.5)
        {
            GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
            Point2D ptt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
            double xxx=ptt.x;
            double yyy =ptt.y;
            //            if(xxx<-1.5||xx>1){
            //                int a=0;
            //            }
            vehState = dRever3;
            qiedianCount = true;
            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
            cout<<"xxx+++++++++++++++++++++++++++++++++"<<xxx<<endl;
            cout<<"yyy+++++++++++++++++++++++++++++++++"<<yyy<<endl;


        }
        else {
            if (qiedianCount && trumpet()<1000)

            {

                /*  gps_decition->brake = 10;
                gps_decition->torque = 0;  */
                dSpeed=0;
                minDecelerate=min(minDecelerate,-0.5f);
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            }
            else
            {
                qiedianCount = false;
                trumpetFirstCount = true;


                dSpeed = 2;
                dSecSpeed = dSpeed / 3.6;
                gps_decition->speed = dSpeed;
                obsDistance=-1;
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);

            }


            gps_decition->wheel_angle = 0 ;

            cout<<"farTpointDistance================"<<dis<<endl;

            return gps_decition;
        }
    }


    if (vehState == dRever3)
    {
        double dis = GetDistance(now_gps_ins, Compute00().dTpoint3);
        double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
        if((((angdis<5)||(angdis>355)))&&(dis<10.0))
        {
            vehState = dRever4;
            qiedianCount = true;
            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;

        }
        else {
            if (qiedianCount && trumpet()<1000)

            {

                //                gps_decition->brake = 10;
                //                gps_decition->torque = 0;
                dSpeed=0;
                minDecelerate=min(minDecelerate,-0.5f);
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            }
            else
            {
                qiedianCount = false;
                trumpetFirstCount = true;


                dSpeed = 2;
                dSecSpeed = dSpeed / 3.6;
                gps_decition->speed = dSpeed;
                obsDistance=-1;
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);

            }

            controlAng = 0-Compute00().dBocheAngle*16.5;
            gps_decition->wheel_angle = 0 - controlAng*0.95;

            cout<<"farTpointDistance================"<<dis<<endl;

            return gps_decition;
        }
    }



    if (vehState == dRever4)
    {
        //	double dis = GetDistance(now_gps_ins, aim_gps_ins);
        Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);

        if ((pt.y)<0.5)
        {

            qiedianCount=true;
            vehState=reverseArr;
            cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
            //            gps_decition->accelerator = -3;
            //            gps_decition->brake =10 ;
            dSpeed=0;
            minDecelerate=min(minDecelerate,-0.5f);
            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);

            gps_decition->wheel_angle=0;
            return gps_decition;




        }
        else {

            //if (qiedianCount && trumpet()<0)
            if (qiedianCount && trumpet()<1000)
            {

                //                gps_decition->brake = 10;
                //                gps_decition->torque = 0;
                dSpeed=0;
                minDecelerate=min(minDecelerate,-0.5f);
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            }
            else
            {
                qiedianCount = false;
                trumpetFirstCount = true;
                dSpeed = 2;
                dSecSpeed = dSpeed / 3.6;
                gps_decition->speed = dSpeed;
                obsDistance=-1;
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            }



            controlAng = 0;

            gps_decition->wheel_angle = 0;

            return gps_decition;
        }
    }



    if (vehState == reverseArr)
    {
        //
        ServiceCarStatus.bocheMode=false;

        if (qiedianCount && trumpet()<1500)
        {

            //            gps_decition->brake = 10;
            //            gps_decition->torque = 0;
            dSpeed=0;
            minDecelerate=min(minDecelerate,-0.5f);
            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
        }
        else
        {
            qiedianCount = false;
            trumpetFirstCount = true;
            ServiceCarStatus.bocheEnable=0;
            vehState=normalRun;
            ServiceCarStatus.mbRunPause=true;
            ServiceCarStatus.mnBocheComplete=100;
            cout<<"泊车daowei mbRunPause+++++++++++++++++++++++++++++++++"<<endl;
        }


        gps_decition->wheel_angle = 0 ;



        return gps_decition;

    }











    obsDistance = -1;
    esrIndex = -1;
    lidarDistance = -1;

    obsDistanceAvoid = -1;
    esrIndexAvoid = -1;
    roadPre = -1;











    gpsTraceOri.clear();
    gpsTraceNow.clear();
    gpsTraceAim.clear();
    gpsTraceAvoid.clear();
    gpsTraceBack.clear();







    if (!isFirstRun)
    {
        //   PathPoint = Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
        //    if(PathPoint<0){
        PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
        //    }

    }


    if (PathPoint<0)
    {

        minDecelerate=-1.0;
        phaseSpeedDecition(gps_decition, secSpeed, -1, 0,now_gps_ins);
        return gps_decition;

    }

    DecideGps00::lastGpsIndex = PathPoint;








    //2020-01-03, kkcai
    //if(!circleMode && PathPoint>gpsMapLine.size()-200){
    if(!circleMode && PathPoint>gpsMapLine.size()-100){
        minDecelerate=-1.0;
        std::cout<<"map finish brake"<<std::endl;
    }



    if(!ServiceCarStatus.inRoadAvoid){
        roadOri = gpsMapLine[PathPoint]->roadOri;
        roadSum = gpsMapLine[PathPoint]->roadSum;
    }else{
        roadOri=gpsMapLine[PathPoint]->roadOri*3+1;
        roadSum = gpsMapLine[PathPoint]->roadSum*3;
    }

    if(ServiceCarStatus.avoidObs){
         gpsMapLine[PathPoint]->runMode=1;
    }else{
         gpsMapLine[PathPoint]->runMode=0;
    }

    if(roadNowStart){
        roadNow=roadOri;
        roadNowStart=false;
    }

 //   avoidX = (roadOri-roadNow)*roadWidth;
    avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);


    if ( gpsMapLine[PathPoint]->runMode == 0 &&(vehState == avoidObs||vehState ==stopObs||vehState == preAvoid
                                                ||vehState == avoiding||vehState == backOri||vehState ==preBack||vehState ==waitAvoid ) )
    {
        vehState = normalRun;
        roadNow = roadOri;
    }



    //    if (PathPoint!=-1&&((now_gps_ins.rtk_status==6)||(now_gps_ins.rtk_status==5))&&GetDistance(now_gps_ins,*gpsMapLine[PathPoint])<30)
    //    {
    //        DecideGps00::lastGpsIndex = PathPoint;
    //        gpsMissCount = 0;

    //    }
    //    else
    //    {
    //        gpsMissCount++;
    //    }

    gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
    //    gpsTraceOri = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
    if(gpsTraceOri.empty()){
        gps_decition->wheel_angle = 0;
        gps_decition->speed = dSpeed;

        gps_decition->accelerator = -0.5;
        gps_decition->brake=10;
        return gps_decition;
    }



    traceDevi=gpsTraceOri[0].x;

    //起步进行frenet规划后，车辆行驶至地图路线上，起步操作完毕。
    //如果车辆本身启动位置在地图路线上，可不用进行frenet规划。
    //    if(GetDistance(now_gps_ins,*gpsMapLine[PathPoint]) < 1){
    //        startingFlag = false;
    //    }
    startingFlag = false;
    if(startingFlag){
        //        gpsTraceAim = gpsTraceOri;
        if(abs(gpsTraceOri[0].x)>1){
            cout<<"frenetPlanner->getPath:pre"<<endl;
            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
            cout<<"frenetPlanner->getPath,gpsTraceNow.size: "<<gpsTraceNow.size()<<endl;
            if(gpsTraceNow.size()==0){
                gps_decition->accelerator = 0;
                gps_decition->brake=10;
                gps_decition->wheel_angle = lastAngle;
                gps_decition->speed = 0;
                return gps_decition;
            }
        }else{
            startingFlag = false;
        }
    }


    if(useFrenet){
        //如果车辆在变道中，启用frenet规划局部路径
        if(vehState == avoiding || vehState == backOri){
            //avoidX = (roadOri - roadNow)*roadWidth;
             avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
            //gpsTraceAim = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
            gpsTraceAim = getGpsTraceOffset(gpsTraceOri, avoidX);
            if(gpsTraceNow.size()==0){
                gps_decition->accelerator = 0;
                gps_decition->brake=10;
                gps_decition->wheel_angle = lastAngle;
                gps_decition->speed = 0;
                return gps_decition;
            }
        }
    }

    if(gpsTraceNow.size()==0){
        if (roadNow==roadOri)
        {
            gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
            //gpsTraceNow = getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex);
        }else
        {
            //	gpsTraceNow = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-roadNow))], lastGpsIndex);
            gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
            //gpsTraceNow = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
        }
    }


    //  dSpeed = getSpeed(gpsTraceNow);
    dSpeed = 80;

    //   planTrace = gpsTraceNow; //Add By YuChuli, 2020.11.26

    planTrace.clear();
    for(int i=0;i<gpsTraceNow.size();i++){
        TracePoint pt(gpsTraceNow[i].x,gpsTraceNow[i].y);
        planTrace.push_back(pt);
    }


    dSpeed = limitSpeed(controlAng, dSpeed);



    controlAng= getAngle(gpsTraceNow,now_gps_ins,gps_decition);
    if(!circleMode && PathPoint>(gpsMapLine.size()-100)){
        controlAng=0;
    }



    //1220
    if(ServiceCarStatus.daocheMode){
        controlAng=0-controlAng;
    }

    //2020-0106
    if(vehState==avoiding){
        controlAng=max(-150.0,controlAng);
        controlAng=min(150.0,controlAng);
    }
    if(vehState==backOri){
        controlAng=max(-120.0,controlAng);
        controlAng=min(120.0,controlAng);
    }

    //准备驻车

    if (readyZhucheMode)
    {

        cout<<"ready ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
        cout<<"zhuche point : "<<zhuchePointNum<<endl;

        double dis = GetDistance(*gpsMapLine[zhuchePointNum], now_gps_ins);

        if (dis<35)
        {
            Point2D pt = Coordinate_Transfer((*gpsMapLine[zhuchePointNum]).gps_x, (*gpsMapLine[zhuchePointNum]).gps_y, now_gps_ins);
            double zhucheDistance = pt.y;
#if 0

            if (dSpeed > 15)
            {
                dSpeed = 15;
            }



            if (zhucheDistance < 20 && dis < 25)
            {
                dSpeed = min(dSpeed, 5.0);
            }
#else
            if (dSpeed > 12)
            {
                dSpeed = 12;
            }



            if (zhucheDistance < 9 && dis < 9)
            {
                dSpeed = min(dSpeed, 5.0);
            }
#endif


            if (zhucheDistance < 3.0 && dis < 3.5)
            {
                dSpeed = min(dSpeed, 5.0);
                zhucheMode = true;
                readyZhucheMode = false;
                cout<<"jinru ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;

                //1125
                //                 gps_decition->brake=20;
                //                 gps_decition->accelerator = -3;
                //                 gps_decition->wheel_angle = 0-controlAng;
                //                 gps_decition->speed = 0;
                //                 gps_decition->torque=0;
                //                 return gps_decition;





            }

        }

    }





    if (readyParkMode)
    {
        double parkDis = GetDistance(now_gps_ins, *gpsMapLine[finishPointNum]);
        hasZhuched = true;

        if (parkDis < 25)
        {
            Point2D pt = Coordinate_Transfer((*gpsMapLine[finishPointNum]).gps_x, (*gpsMapLine[finishPointNum]).gps_y, now_gps_ins);
            double parkDistance = pt.y;


            if (dSpeed > 15)
            {
                dSpeed = 15;
            }

            //dSpeed = 15;//////////////////////////////

            if (parkDistance < 15 && parkDistance >= 12.5 && parkDis<20)
            {
                dSpeed = min(dSpeed, 8.0);
            }else if (parkDistance < 12.5 && parkDistance >= 8.5 && parkDis<15.0){
                dSpeed = min(dSpeed, 5.0);
            }else if (parkDistance < 8.5 && parkDistance >= 5.5 && parkDis<9.0){
                dSpeed = min(dSpeed, 3.0);
            }else if(parkDistance < 5.5 && parkDis<6.0){
                dSpeed = min(dSpeed, 1.0);
            }


            //            float stopDis=2;
            //            if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
            //                stopDis=1.6;
            //            } else if(ServiceCarStatus.msysparam.mvehtype=="vv7"){
            //                stopDis=1.8;
            //            }

            if (parkDistance < 1.6  && parkDis<2.0)
            {
                // dSpeed = 0;
                parkMode = true;
                readyParkMode = false;
            }


        }
    }



    if (gpsMapLine[PathPoint]->roadMode ==0)
    {
        //dSpeed = min(10.0,dSpeed);

        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
        gps_decition->leftlamp = false;
        gps_decition->rightlamp = false;

    }else  if (gpsMapLine[PathPoint]->roadMode == 5)
    {
        //dSpeed = min(8.0,dSpeed);

        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode5,dSpeed);
        gps_decition->leftlamp = false;
        gps_decition->rightlamp = false;

    }else if (gpsMapLine[PathPoint]->roadMode == 11)
    {
        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode11,dSpeed);
        gps_decition->leftlamp = false;
        gps_decition->rightlamp = false;

    }else if (gpsMapLine[PathPoint]->roadMode == 14)
    {
        //dSpeed = min(8.0,dSpeed);

        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode14,dSpeed);
        gps_decition->leftlamp = true;
        gps_decition->rightlamp = false;
    }else if (gpsMapLine[PathPoint]->roadMode == 15)
    {
        //dSpeed = min(8.0,dSpeed);

        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode15,dSpeed);
        gps_decition->leftlamp = false;
        gps_decition->rightlamp = true;
    }else if (gpsMapLine[PathPoint]->roadMode == 16)
    {
        // dSpeed = min(10.0,dSpeed);
        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode16,dSpeed);
        //gps_decition->leftlamp = true;
        //gps_decition->rightlamp = false;
    }else if (gpsMapLine[PathPoint]->roadMode == 17)
    {
        //  dSpeed = min(10.0,dSpeed);
        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode17,dSpeed);
        //gps_decition->leftlamp = false;
        //gps_decition->rightlamp = true;
    }else if (gpsMapLine[PathPoint]->roadMode == 18)
    {
        // dSpeed = min(10.0,dSpeed);
        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode18,dSpeed);
        gps_decition->leftlamp = false;
        gps_decition->rightlamp = false;

        /*if(gpsMapLine[PathPoint]->light_left_or_right == 1)
        {
            gps_decition->leftlamp = true;
            gps_decition->rightlamp = false;
        }
        else if(gpsMapLine[PathPoint]->light_left_or_right == 2)
        {
            gps_decition->leftlamp = false;
            gps_decition->rightlamp = true;
        }*/
    }else if (gpsMapLine[PathPoint]->roadMode == 1)
    {
        dSpeed = min(10.0,dSpeed);
    }else if (gpsMapLine[PathPoint]->roadMode == 2)
    {
        dSpeed = min(15.0,dSpeed);
    }
    else if (gpsMapLine[PathPoint]->roadMode == 7)
    {
        dSpeed = min(15.0,dSpeed);
        xiuzhengCs=1.5;
    }else if (gpsMapLine[PathPoint]->roadMode == 4) //1220
    {
        dSpeed = min(4.0,dSpeed);

    }else{
        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
        gps_decition->leftlamp = false;
        gps_decition->rightlamp = false;

    }





    if (gpsMapLine[PathPoint]->speed_mode == 2)
    {
        dSpeed = min(25.0,dSpeed);

    }

    if((gpsMapLine[PathPoint]->speed)>0.001)
    {
        dSpeed = min((gpsMapLine[PathPoint]->speed*3.6),dSpeed);
    }


    dSecSpeed = dSpeed / 3.6;






    std::cout<<"juecesudu2="<<dSpeed<<std::endl;







    if(vehState==changingRoad){
        if(gpsTraceNow[0].x>1.0){
            gps_decition->leftlamp = false;
            gps_decition->rightlamp = true;
        }else if(gpsTraceNow[0].x<-1.0){
            gps_decition->leftlamp = true;
            gps_decition->rightlamp = false;
        }else{
            vehState==normalRun;
        }
    }

    //    qDebug("decide0 time is %d",xTime.elapsed());

    //1220
    if(!ServiceCarStatus.daocheMode){
        computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
    }else{
        gpsTraceRear.clear();
        for(int i=0;i<gpsTraceNow.size();i++){
            Point2D pt;
            pt.x=0-gpsTraceNow[i].x;
            pt.y=0-gpsTraceNow[i].y;
            gpsTraceRear.push_back(pt);
        }
        computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
        //  obsDistance=-1; //1227
    }




    //old_bz--------------------------------------------------------------------------------------------------

    if (vehState == avoiding)
    {
        cout<<"\n==================avoiding=================\n"<<endl;
        //  vehState=normalRun; //1025
        if (dSpeed > 10) {
            dSpeed = 10;
        }
        //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
        if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
            vehState = normalRun;
            //            useFrenet = false;
            //            useOldAvoid = true;
        }
        else if (useOldAvoid && abs(gpsTraceNow[0].x)<1.0) {
            // vehState = avoidObs; 0929
            vehState = normalRun;
        }
        else if (gpsTraceNow[0].x>0)
        {
            gps_decition->leftlamp = false;
            gps_decition->rightlamp = true;
        }
        else if(gpsTraceNow[0].x<0)
        {
            gps_decition->leftlamp = true;
            gps_decition->rightlamp = false;
        }
    }


    if (vehState==preBack)
    {
        vehState = backOri;
    }


    if (vehState==backOri)
    {
        cout<<"\n================backOri===========\n"<<endl;
        //  vehState=normalRun; //1025
        if (dSpeed > 10) {
            dSpeed = 10;
        }

        //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
        if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
            vehState = normalRun;
            //            useFrenet = false;
            //            useOldAvoid = true;
        }
        else if (useOldAvoid && abs(gpsTraceNow[0].x)<1.0) {
            // vehState = avoidObs; 0929
            vehState = normalRun;
        }
        else if (gpsTraceNow[0].x>0)
        {
            gps_decition->leftlamp = false;
            gps_decition->rightlamp = true;
        }
        else if (gpsTraceNow[0].x<0)
        {
            gps_decition->leftlamp = true;
            gps_decition->rightlamp = false;
        }
    }

    std::cout<<"\n原始RoadOri:%d\n"<<roadOri<<endl;

    cout<<"\n当前RoadNum:%d\n"<<roadNow<<endl;









    //   计算回归原始路线

    if (roadNow!=roadOri)
    {
        //        useFrenet = true;
        //        useOldAvoid = false;

        if(useFrenet){
            if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)))
            {
                double offsetL = -(roadSum - 1)*roadWidth;
                double offsetR = (roadOri - 0)*roadWidth;
                roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
            }
        }
        else if(useOldAvoid){
            roadPre = chooseBackRoad(lidarGridPtr, now_gps_ins, gpsMapLine,lidar_per);
          //  avoidX = (roadOri - roadNow)*roadWidth; //1012
             avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
        }
    }
    if (roadNow != roadOri && roadPre!=-1)
    {

        roadNow = roadPre;
     //   avoidX = (roadOri - roadNow)*roadWidth;
         avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
        gpsTraceNow.clear();
        if(useOldAvoid){
            gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
        }
        else if(useFrenet){
            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
        }

        vehState = backOri;
        hasCheckedBackLidar = false;
        //  checkBackObsTimes = 0;

        cout<<"\n回归RoadNum:%d\n"<< roadNow<<endl;

    }

    //shiyanbizhang 0929
    if(obsDistance>0&& (obsDistance<ServiceCarStatus.socfDis+20.0) &&(abs(realspeed)<1.0) &&
            (vehState==normalRun||vehState==backOri || vehState==avoiding) &&(ObsTimeStart==-1)
            && (gpsMapLine[PathPoint]->runMode==1)){
        ObsTimeStart=GetTickCount();
        cout<<"\n====================preAvoid time count start==================\n"<<endl;
    }
    if(ObsTimeStart!=-1){
        ObsTimeEnd=GetTickCount();
    }
    if(ObsTimeEnd!=-1){
        ObsTimeSpan=ObsTimeEnd-ObsTimeStart;
    }
    if(ObsTimeSpan>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
        vehState=avoidObs;
        ObsTimeStart=-1;
        ObsTimeEnd=-1;
        ObsTimeSpan=-1;
        cout<<"\n====================preAvoid time count finish==================\n"<<endl;
    }

    if(obsDistance<0 ||obsDistance>ServiceCarStatus.socfDis){
        ObsTimeStart=-1;
        ObsTimeEnd=-1;
        ObsTimeSpan=-1;

    }


    //避障模式


    if (vehState == avoidObs   || vehState==waitAvoid ) {

        //      if (obsDistance <20 && obsDistance >0)
        if (obsDistance <ServiceCarStatus.aocfDis && obsDistance >0)
        {
            dSpeed = min(7.0,dSpeed);
            avoidTimes++;
            //          if (avoidTimes>=6)
            if (avoidTimes>=ServiceCarStatus.aocfTimes)
            {
                vehState = preAvoid;
                avoidTimes = 0;
            }

        }
        //        else if (obsDistance < 35 && obsDistance >0 && dSpeed>10)
        //        {
        //            dSpeed = 10;
        //            avoidTimes = 0;
        //        }
        else if (obsDistance < 40 && obsDistance >0 && dSpeed>15)
        {
            dSpeed =  min(15.0,dSpeed);
            avoidTimes = 0;
        }
        else
        {
            avoidTimes = 0;
        }

    }


    if (vehState == preAvoid)
    {
        cout<<"\n====================preAvoid==================\n"<<endl;
        /* if (obsDisVector[roadNow]>30)*/  //if (obsDisVector[roadNow]>30||obsDisVector[roadNow]<0)
        if (obsDisVector[roadNow]>ServiceCarStatus.aocfDis)
        {
            // vehState = avoidObs; 0929
            vehState=normalRun;
        }
        else
        {
            //roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine);
            if(useOldAvoid){
                roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine,lidar_per);
              //  avoidX = (roadOri - roadNow)*roadWidth;  //1012
                 avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
            }
            else if(useFrenet){
                double offsetL = -(roadSum - 1)*roadWidth;
                double offsetR = (roadOri - 0)*roadWidth;
                roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
            }

            if (roadPre == -1 && obsDisVector[roadNow]<1.5 && obsDisVector[roadNow]>0)
            {
                //  vehState = waitAvoid; 0929
                vehState = avoidObs;
            }
            else if (roadPre != -1)
            {
                if(useOldAvoid){
                    roadNow = roadPre;
                //    avoidX = (roadOri - roadNow)*roadWidth;
                     avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
                    gpsTraceNow.clear();
                    gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
                }
                else if(useFrenet){
                    if(roadPre != roadNow){
                        avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                        startAvoidGpsInsVector[roadNow] = now_gps_ins;
                    }
                    roadNow = roadPre;
                 //   avoidX = (roadOri - roadNow)*roadWidth;
                     avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
                    gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
                }


                vehState = avoiding;

                hasCheckedAvoidLidar = false;

                cout<<"\n避障到RoadNum:%d\n"<<roadNow<<endl;

            }
        }
    }

    //--------------------------------------------------------------------------------old_bz_end





    //TOUCHEPINGBI














    if (parkMode)
    {
        minDecelerate=-0.4;

        if(finishPointNum<0||finishPointNum>=gpsMapLine.size()){
            parkMode=false;
        }else if(GetDistance(now_gps_ins, *gpsMapLine[finishPointNum])>4){
            parkMode=false;
        }

    }



    //驻车

    if (zhucheMode)
    {
        int mzcTime = ServiceCarStatus.msysparam.mzhuchetime * 1000;

        //        if(trumpet()<16000)
        if (trumpet()<mzcTime)
        {
            //            if (trumpet()<12000){
            cout<<"ZHECHE ING STATE+++++++++++++++++++++++++++++++++"<<endl;
            minDecelerate=-1.0;
            if(abs(realspeed)<0.2){
                controlAng=0;  //tju
            }
        }
        else
        {
            hasZhuched = true; //1125
            zhucheMode = false;
            trumpetFirstCount = true;
            cout<<"ZHECHE STATE JIESHU +++++++++++++++++++++++++++++++++"<<endl;
        }

    }


    //判断驻车标志位
    if (hasZhuched && GetDistance(now_gps_ins,*gpsMapLine[zhuchePointNum])>20)
    {
        hasZhuched = false;
    }











    if ( vehState==changingRoad || vehState==chaocheBack)
    {
        double lastAng = 0.0 - lastAngle;
        if (controlAng>40)
        {
            controlAng =40;
        }
        else if (controlAng<-40)
        {
            controlAng = - 40;
        }


    }


    //速度结合角度的限制
    controlAng = limitAngle(realspeed, controlAng);


    std::cout << "\n呼车指令状态:%d\n" <<  ServiceCarStatus.carState << std::endl;



    //1220
    if(PathPoint+80<gpsMapLine.size()-1){
        if(gpsMapLine[PathPoint+80]->roadMode==4  && !ServiceCarStatus.daocheMode){
            changingDangWei=true;
        }
    }

    if(changingDangWei){
        if(abs(realspeed)>0.1){
            dSpeed=0;
        }else{
            dSpeed=0;
            gps_decition->dangWei=2;
            ServiceCarStatus.daocheMode=true;
        }
    }
    //1220 end




    for(int i = 0;i<ServiceCarStatus.car_stations.size();i++)
    {
        GPS_INS gpsIns;
        GaussProjCal(ServiceCarStatus.car_stations[i].longtitude, ServiceCarStatus.car_stations[i].latitude,  &gpsIns.gps_x, &gpsIns.gps_y);

        double dis = GetDistance(gpsIns, now_gps_ins);
        if(dis<20)
            ServiceCarStatus.currentstation = ServiceCarStatus.car_stations[i].ID;
    }


    //  解析云平台数据
    if(ServiceCarStatus.stationCmd.received==true)
    {
        std::vector<Station>  station_received;
        Station station_aa,station_nearest;

        if(ServiceCarStatus.stationCmd.has_emergencyStop)
        {
            if(ServiceCarStatus.stationCmd.emergencyStop==0x01)
            {
                //ServiceCarStatus.carState = 0;
                //busMode=true;
                ServiceCarStatus.emergencyStop=1;
            }
            else
            {
                //ServiceCarStatus.carState = 1;
                //busMode=false;
                ServiceCarStatus.emergencyStop=0;
            }
        }

        if(ServiceCarStatus.stationCmd.has_stationStop)
        {
            //寻找最近站点
            station_received.clear();
            for(int i=0;i<ServiceCarStatus.stationCmd.stationTotalNum;i++)
            {
                station_aa.index=i;
                station_aa.station_location.gps_lat=ServiceCarStatus.stationCmd.stationGps[i].gps_lat;
                station_aa.station_location.gps_lng=ServiceCarStatus.stationCmd.stationGps[i].gps_lng;
                GaussProjCal(station_aa.station_location.gps_lng,station_aa.station_location.gps_lat, &station_aa.station_location.gps_x, &station_aa.station_location.gps_y);
                station_received.push_back(station_aa);
            }
            station_nearest=getNearestStation(now_gps_ins,station_received,gpsMapLine);

            qDebug("station_nearest: %d, lat: %.7f, lon: %.7f", station_nearest.map_index, station_nearest.station_location.gps_lat, ServiceCarStatus.amilng=station_nearest.station_location.gps_lng);
            givlog->debug("brain_v2x","station_nearest: %d, lat: %.7f, lon: %.7f",
                          station_nearest.map_index, station_nearest.station_location.gps_lat,
                          station_nearest.station_location.gps_lng);
            //进入站点模式
            if((ServiceCarStatus.stationCmd.stationStop==0x00))
            {
                ServiceCarStatus.carState = 2;
                ServiceCarStatus.amilat=station_nearest.station_location.gps_lat;
                ServiceCarStatus.amilng=station_nearest.station_location.gps_lng;
                busMode=true;
            }else
            {
                ServiceCarStatus.carState = 1;
                busMode=false;
                ServiceCarStatus.station_control_door=0;
                ServiceCarStatus.station_control_door_option=false;
            }
        }
        if(ServiceCarStatus.stationCmd.has_carMode)
        {
            if(ServiceCarStatus.stationCmd.carMode==0x00)
            {
                ServiceCarStatus.stationCmd.mode_manual_drive=true;
            }else
            {
                ServiceCarStatus.stationCmd.mode_manual_drive=false;
            }
        }

        ServiceCarStatus.stationCmd.received=false;
    }




    //carState == 0，紧急停车
    if ((ServiceCarStatus.emergencyStop==1))  //||(adjuseultra()==true))
    {
        minDecelerate=-1.0;
    }

    if (ServiceCarStatus.carState == 3 && busMode)
    {

        if(realspeed<0.1){
            ServiceCarStatus.carState=0;
            minDecelerate=-1.0;
        }else{
            nearStation=true;
            dSpeed=0;
        }

    }

    //carState==2,站点停车
    if ((ServiceCarStatus.carState==2)&&busMode)
    {

        aim_gps_ins.gps_lat=ServiceCarStatus.amilat;
        aim_gps_ins.gps_lng=ServiceCarStatus.amilng;

        GaussProjCal( aim_gps_ins.gps_lng, aim_gps_ins.gps_lat,  &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
        double dis = GetDistance(aim_gps_ins, now_gps_ins);
        Point2D pt = Coordinate_Transfer((aim_gps_ins).gps_x, (aim_gps_ins).gps_y, now_gps_ins);

        if (dis<20 && pt.y<5 && abs(pt.x)<3)
        {
            dSpeed = 0;
            nearStation=true;
            //is_arrivaled = true;
            //ServiceCarStatus.status[0] = true;
            ServiceCarStatus.istostation=1;
            minDecelerate=-1.0;
        }
        else if (dis<20 && pt.y<15 && abs(pt.x)<3)
        {
            nearStation=true;
            dSpeed = min(8.0, dSpeed);
        }
        else if (dis<30 && pt.y<20 && abs(pt.x)<3)
        {
            dSpeed = min(15.0, dSpeed);
        }
        else if (dis<50 && abs(pt.x)<3)
        {
            dSpeed = min(20.0, dSpeed);
        }

        if((pt.y<5)&&(abs(realspeed)<0.1)&&(ServiceCarStatus.station_control_door==0))
        {
            ServiceCarStatus.station_control_door=1;   //open door
        }

        //站点停车log
        givlog->debug("brain_v2x","aim_gps_ins.gps_lat: %.7f, aim_gps_ins.gps_lng: %.7f, dis: %.2f, dSpeed: %.1f",
                      aim_gps_ins.gps_lat, aim_gps_ins.gps_lng,
                      dis,dSpeed);

    }


    dSecSpeed = dSpeed / 3.6;
    gps_decition->speed = dSpeed;



    if (gpsMapLine[PathPoint]->runMode == 2)
    {
        obsDistance = -1;
    }

    std::cout<<"juecesudu0="<<dSpeed<<std::endl;

    //-------------------------------traffic light----------------------------------------------------------------------------------------

    if(traffic_light_gps.gps_lat!=0 && traffic_light_gps.gps_lng!=0){
        traffic_light_pathpoint = Compute00().getNoAngleNearestPointIndex(traffic_light_gps, gpsMapLine);
        //    traffic_light_pathpoint=130;
        float traffic_speed=ComputeTrafficLightSpeed(ServiceCarStatus.iTrafficeLightColor,ServiceCarStatus.iTrafficeLightTime,gpsMapLine,
                                                     traffic_light_pathpoint ,PathPoint,secSpeed,dSpeed);
        dSpeed = min((double)traffic_speed,dSpeed);
        if(traffic_speed==0){
            minDecelerate=-2.0;
        }
    }
    //-------------------------------traffic light  end-----------------------------------------------------------------------------------------------


    //-------------------------------v2x traffic light-----------------------------------------------------------------------------------------


    double v2xTrafficSpeed= ComputeV2XTrafficLightSpeed( trafficLight,  gpsMapLine, v2xTrafficVector,
                                                         PathPoint, secSpeed, dSpeed,  circleMode);


    dSpeed = min(v2xTrafficSpeed,dSpeed);

    //------------------------------v2x traffic light end--------------------------------------------------------------------------------------


    transferGpsMode2(gpsMapLine);



    if((ServiceCarStatus.msysparam.mvehtype=="hapo")&&(abs(realspeed)<18)){
        if(obsDistance>0 && obsDistance<6){
            dSpeed=0;
        }
    }else if(obsDistance>0 && obsDistance<10){
        dSpeed=0;
    }

    //  obsDistance=-1; //1227

    if(ServiceCarStatus.group_control){
        dSpeed = ServiceCarStatus.group_comm_speed;
    }
    if(dSpeed==0){
        minDecelerate=min(-1.0f,minDecelerate);
    }

    std::cout<<"dSpeed= "<<dSpeed<<std::endl;

    // givlog->debug("SPEED","dSpeed is %f",dSpeed);
    gps_decition->wheel_angle = 0.0 - controlAng;
    phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);



    lastAngle=gps_decition->wheel_angle;

    //   qDebug("decide1 time is %d",xTime.elapsed());

    for(int i=0;i<ServiceCarStatus.obsTraceLeft.size();i++){
        givlog->debug("obs_distance","obsTraceLeftX: %f, obsTraceLeftY: %f",
                      ServiceCarStatus.obsTraceLeft.at(i).x, ServiceCarStatus.obsTraceLeft.at(i).y);
    }


    return gps_decition;
}

iv::Station iv::decision::DecideGps00::getNearestStation(iv::GPS_INS now_gps_ins,std::vector<Station> station_n,std::vector<GPSData> gpsMap){

    int now_index=0,front_index=0;
    int station_size=station_n.size();

    for(int i=0;i<station_size;i++)
    {
        int minDistance=10;
        for (int j = 0; j < gpsMap.size(); j++)
        {
            double tmpdis = GetDistance(station_n[i].station_location, (*gpsMap[j]));
            if (tmpdis < minDistance )
            {
                minDistance = tmpdis;
                station_n[i].map_index=j;
            }
        }
        givlog->debug("brain_v2x","stationi: %d, map_index: %d",
                      i,station_n[i].map_index);

    }
    int minDistance=10;
    for (int j = 0; j < gpsMap.size(); j++)
    {
        double tmpdis = GetDistance(now_gps_ins, (*gpsMap[j]));
        if (tmpdis < minDistance )
        {
            minDistance = tmpdis;
            now_index=j;
        }
    }

    givlog->debug("brain_v2x","now_index: %d",
                  now_index);

    int min_index=gpsMap.size()-1;
    int station_index=0;
    for(int i=0;i<station_size;i++)
    {
        if(station_n[i].map_index>now_index)
        {
            front_index=station_n[i].map_index;
            if(front_index<min_index)
            {
                min_index=front_index;
                station_index=i;
            }
        }
    }

    qDebug("station_index:%d",station_index);

    return station_n[station_index];

}

void iv::decision::DecideGps00::initMethods(){

    pid_Controller= new PIDController();
    ge3_Adapter = new Ge3Adapter();
    qingyuan_Adapter = new QingYuanAdapter();
    vv7_Adapter = new VV7Adapter();
    zhongche_Adapter = new ZhongcheAdapter();
    bus_Adapter = new BusAdapter();
    hapo_Adapter = new HapoAdapter();
    yuhesen_Adapter = new YuHeSenAdapter();


    mpid_Controller.reset(pid_Controller);
    mbus_Adapter.reset(bus_Adapter);
    mhapo_Adapter.reset(hapo_Adapter);
    myuhesen_Adapter.reset(yuhesen_Adapter);

    frenetPlanner = new FrenetPlanner();
    //    laneChangePlanner = new LaneChangePlanner();

    mfrenetPlanner.reset(frenetPlanner);
    //    mlaneChangePlanner.reset(laneChangePlanner);

}


void iv::decision::DecideGps00::phaseSpeedDecition(iv::decision::Decition  decition, double secSpeed, double obsDis, double obsSpeed, GPS_INS gpsIns ) {

    pid_Controller->getControlDecition( gpsIns, gpsTraceNow,  dSpeed,  obsDis,  obsSpeed, false, true, &decition);

    if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
        ge3_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
    }else if( ServiceCarStatus.msysparam.mvehtype=="qingyuan"){
        qingyuan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
    }else if( ServiceCarStatus.msysparam.mvehtype=="vv7"){
        vv7_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
    }else if( ServiceCarStatus.msysparam.mvehtype=="zhongche"){
        zhongche_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
    }else if(ServiceCarStatus.msysparam.mvehtype=="bus"){
        bus_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
    }else if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
        hapo_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
    }
    else if(ServiceCarStatus.msysparam.mvehtype=="yuhesen"){
        yuhesen_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
    }
}



std::vector<iv::Point2D> iv::decision::DecideGps00::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode) {
    vector<iv::Point2D> trace;
    //	int  PathPoint = Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
    /*if (PathPoint != -1)
        lastGpsIndex = PathPoint;*/

    if (gpsMapLine.size() > 600 && PathPoint >= 0) {
        int aimIndex;
        if(circleMode){
            aimIndex=PathPoint+600;
        }else{
            aimIndex=min((PathPoint+600),(int)gpsMapLine.size());
        }

        for (int i = PathPoint; i < aimIndex; i++)
        {
            int index = i % gpsMapLine.size();
            Point2D pt = Coordinate_Transfer((*gpsMapLine[index]).gps_x, (*gpsMapLine[index]).gps_y, now_gps_ins);
            pt.x += offset_real * 0.032;
            pt.v1 = (*gpsMapLine[index]).speed_mode;
            pt.v2 = (*gpsMapLine[index]).mode2;
            pt.roadMode=(*gpsMapLine[index]).roadMode;
            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
            {
                readyZhucheMode = true;
                DecideGps00::zhuchePointNum = index;
            }



            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
            {
                readyZhucheMode = true;
                DecideGps00::zhuchePointNum = index;
            }

            //csvv7
            if (gpsMapLine[index]->speed_mode==22 || gpsMapLine[index]->speed_mode==23)
            {
                readyParkMode = true;
                DecideGps00::finishPointNum = index;
            }

            //			if (pt.v1 == 22 || pt.v1 == 23)
            //			{
            //				readyParkMode = true;
            //				DecideGps00::finishPointNum = i;
            //			}


            switch (pt.v1)
            {
            case 0:
                pt.speed = 80;
                break;
            case 1:
                pt.speed = 25;
                break;
            case 2:
                pt.speed =25;
                break;
            case 3:
                pt.speed = 20;
                break;
            case 4:
                pt.speed =18;
                break;
            case 5:
                pt.speed = 18;
                break;
            case 7:
                pt.speed = 10;
                break;
            case 22:
                pt.speed = 5;
                break;
            case 23:
                pt.speed = 5;
                break;
            default:
                break;
            }
            trace.push_back(pt);
        }
    }
    return trace;
}

std::vector<iv::Point2D>  iv::decision::DecideGps00::getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset) {

    if (offset==0)
    {
        return gpsTrace;
    }

    std::vector<iv::Point2D> trace;
    for (int j = 0; j < gpsTrace.size(); j++)
    {
        double sumx1 = 0, sumy1 = 0, count1 = 0;
        double sumx2 = 0, sumy2 = 0, count2 = 0;
        for (int k = max(0, j - 4); k <= j; k++)
        {
            count1 = count1 + 1;
            sumx1 += gpsTrace[k].x;
            sumy1 += gpsTrace[k].y;
        }
        for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
        {
            count2 = count2 + 1;
            sumx2 += gpsTrace[k].x;
            sumy2 += gpsTrace[k].y;
        }
        sumx1 /= count1; sumy1 /= count1;
        sumx2 /= count2; sumy2 /= count2;

        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);

        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);

        double avoidLenth = abs(offset);
        if (offset<0)
        {
            Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
                           carFronty + avoidLenth  * sin(anglevalue + PI / 2));
            ptLeft.speed = gpsTrace[j].speed;
            ptLeft.v1 = gpsTrace[j].v1;
            ptLeft.v2 = gpsTrace[j].v2;
            trace.push_back(ptLeft);
        }
        else
        {
            Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - PI / 2),
                            carFronty + avoidLenth  * sin(anglevalue - PI / 2));
            ptRight.speed = gpsTrace[j].speed;
            ptRight.v1 = gpsTrace[j].v1;
            ptRight.v2 = gpsTrace[j].v2;


            trace.push_back(ptRight);
        }
    }
    return trace;
}


double iv::decision::DecideGps00::getAngle(std::vector<Point2D> gpsTrace,GPS_INS gpsIns,iv::decision::Decition decition) {
    double angle=0;
    if ( abs(iv::decision::DecideGps00().minDis) < 20 && abs(iv::decision::DecideGps00().maxAngle) < 100)
    {
        //   angle = iv::decition::Compute00().getDecideAngle(gpsTrace, realspeed);
        pid_Controller->getControlDecition(gpsIns, gpsTrace,  -1,  -1,  -1, true, false, &decition);
        angle= decition->wheel_angle;
    }
    return angle;
}

double iv::decision::DecideGps00::getSpeed(std::vector<Point2D> gpsTrace) {
    double speed=0;
    int speedPoint = iv::decision::Compute00().getSpeedPointIndex(gpsTrace, max(10.0, realspeed));
    speed = gpsTrace[speedPoint].speed;
    for (int i = 0; i < speedPoint; i++) {
        speed = min(speed, gpsTrace[i].speed);
    }
    return speed;
}




//void iv::decition::DecideGps00::getEsrObs(std::vector<iv::ObstacleBasic> obsRadars) {
//
//	if (!obsRadars.empty())
//	{
//		esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, obsRadars);
//
//		if (esrIndex != -1)
//		{
//			 esrDistance = obsRadars[esrIndex].nomal_y;
//
//
//
//			obsSpeed = obsRadars[esrIndex].speed_y;
//
//		}
//		else {
//			esrDistance = -1;
//		}
//
//	}
//	else
//	{
//		esrIndex = -1;
//		esrDistance = -1;
//	}
//	if (esrDistance < 0) {
//		ODS("\n------------------>ESR障碍物距离:%f\n", esrDistance);
//	}
//	else {
//		ODS("\n------------------>ESR障碍物距离:%f   ( %.05f , %.05f )\n", esrDistance, obsRadars[esrIndex].nomal_x, obsRadars[esrIndex].nomal_y);
//	}
//
//	ODS("\n------------------>ESR障碍物速度:%f\n", obsSpeed);
//}



void iv::decision::DecideGps00::getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {


    int esrPathpoint;

    esrIndex = iv::decision::Compute00().getEsrIndex(gpsTrace, roadNum, &esrPathpoint);

    if (esrIndex != -1)
    {
        //优化
        //        double distance = 0.0;
        //        for(int i=0; i<esrPathpoint; ++i){
        //            distance+=DirectDistance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
        //        }
        //        esrDistance = distance - Esr_Y_Offset;
        //        if(esrDistance<=0){
        //            esrDistance=1;
        //        }


        esrDistance = ServiceCarStatus.obs_radar[esrIndex].nomal_y;
        obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;

    }
    else {
        esrDistance = -1;
    }
}




void iv::decision::DecideGps00::getEsrObsDistanceAvoid() {

    esrIndexAvoid = iv::decision::Compute00().getEsrIndexAvoid(gpsTraceAvoid);

    if (esrIndexAvoid != -1)
    {
        esrDistanceAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y;
        obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;

    }
    else {
        esrDistanceAvoid = -1;
    }

    if (esrDistanceAvoid < 0) {
        std::cout << "\nESR障碍物距离Avoid:%f\n" << esrDistanceAvoid << std::endl;
    }
    else {
        std::cout << "\nESR障碍物距离Avoid:%f   %d:( %.05f , %.05f ,%.05f )\n" << esrDistanceAvoid << esrIndexAvoid << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_x << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y << std::endl;
    }

    std::cout << "\nESR障碍物速度Avoid:%f\n" << obsSpeedAvoid <<std::endl;
}




double iv::decision::DecideGps00::limitAngle(double speed, double angle) {
    double preAngle = angle;

    if (speed > 15)
    {
        if (preAngle > 350)
        {
            preAngle = 350;
        }
        if (preAngle < -350)
        {
            preAngle = -350;
        }
    }
    if (speed > 22)
    {
        if (preAngle > 200)
        {
            preAngle = 200;
        }
        if (preAngle < -200)
        {
            preAngle = -200;
        }
    }
    if (speed > 25)
    {
        if (preAngle > 150)
        {
            preAngle = 150;
        }
        if (preAngle < -150)
        {
            preAngle = -150;
        }
    }
    if (speed > 30)
    {
        if (preAngle > 70)
        {
            preAngle = 70;
        }
        if (preAngle < -70)
        {
            preAngle = -70;
        }
    }
    if (speed > 45)                     //20
    {
        if (preAngle > 15)
        {
            preAngle = 15;
        }
        if (preAngle < -15)
        {
            preAngle = -15;
        }
    }
    return preAngle;
}



double iv::decision::DecideGps00::limitSpeed(double angle, double speed) {

    if (abs(angle) > 500 && speed > 8) speed = 8;
    else if (abs(angle) > 350 && speed > 14) speed = 14;
    else if (abs(angle) > 200 && speed > 21) speed = 21;
    else if (abs(angle) > 150 && speed > 24) speed = 24;
    else if (abs(angle) > 60 && speed > 29) speed = 29;
    else if (abs(angle) > 20 && speed > 34) speed = 34;
    return max(0.0, speed);
}


bool iv::decision::DecideGps00::checkAvoidEnable(double avoidX, iv::LidarGridPtr lidarGridPtr,int roadNum) {

    if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<obsDisVector[roadNow]+ServiceCarStatus.msysparam.vehLenth)||
            (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<15))
    {
        return false;
    }

    if (roadNum-roadNow>1)
    {
        for (int i = roadNow+1; i < roadNum; i++)
        {
            if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
                return false;
            }
        }

    }

    else if (roadNow-roadNum>1)
    {
        for (int i = roadNow-1; i >roadNum; i--)
        {
            if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
                return false;
            }
        }
    }
    return true;
}



bool iv::decision::DecideGps00::checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum) {
    //lsn
    if (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<20)
    {
        return false;
    }
    if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNum]< obsDisVector[roadNow]) ||
            (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNow]<0))
    {
        return false;
    }


    if (roadNum - roadNow>1)
    {
        for (int i = roadNow + 1; i < roadNum; i++)
        {
            if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
                return false;
            }
        }

    }

    else if (roadNow - roadNum>1)
    {
        for (int i = roadNow - 1; i >roadNum; i--)
        {
            if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
                return false;
            }
        }
    }
    return true;
}


void iv::decision::DecideGps00::getObsAvoid(iv::LidarGridPtr lidarGridPtr) {



    if (lidarGridPtr == NULL)
    {
        iv::decision::DecideGps00::lidarDistanceAvoid = iv::decision::DecideGps00::lastLidarDisAvoid;
    }
    else {
        obsPointAvoid = Compute00().getLidarObsPointAvoid(gpsTraceAvoid, lidarGridPtr);
        iv::decision::DecideGps00::lastLidarDisAvoid = iv::decision::DecideGps00::lidarDistanceAvoid;
    }
    std::cout << "\nLidarAvoid距离:%f\n" << iv::decision::DecideGps00::lidarDistanceAvoid << std::endl;
    getEsrObsDistanceAvoid();

    //lidarDistanceAvoid = -1;   //20200103 apollo_fu

    if (esrDistanceAvoid>0 && iv::decision::DecideGps00::lidarDistanceAvoid > 0)
    {
        if (iv::decision::DecideGps00::lidarDistanceAvoid >= esrDistanceAvoid)
        {
            obsDistanceAvoid = esrDistanceAvoid;
            obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
        }
        else if (!ServiceCarStatus.obs_radar.empty())
        {
            obsDistanceAvoid = iv::decision::DecideGps00::lidarDistanceAvoid;
            obsSpeedAvoid = Compute00().getObsSpeed(obsPointAvoid, secSpeed);
            std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
        }
        else
        {
            obsDistanceAvoid = iv::decision::DecideGps00::lidarDistanceAvoid;
            obsSpeedAvoid = obsSpeedAvoid = 0 - secSpeed;;
            std::cout << "\n毫米波无数据，计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
        }
    }
    else if (esrDistanceAvoid>0)
    {
        obsDistanceAvoid = esrDistanceAvoid;
        obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
    }
    else if (iv::decision::DecideGps00::lidarDistanceAvoid > 0)
    {
        obsDistanceAvoid = iv::decision::DecideGps00::lidarDistanceAvoid;
        obsSpeedAvoid = Compute00().getObsSpeed(obsPointAvoid, secSpeed);
        std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
    }
    else {
        obsDistanceAvoid = esrDistanceAvoid;
        obsSpeedAvoid = 0 - secSpeed;
    }

    std::cout << "\n最终得出的obsDistanceAvoid:%f\n" << obsDistanceAvoid << std::endl;





    if (iv::decision::DecideGps00::obsDistanceAvoid <0 && obsLostTimeAvoid<4)
    {
        iv::decision::DecideGps00::obsDistanceAvoid = iv::decision::DecideGps00::lastDistanceAvoid;
        obsLostTimeAvoid++;
    }
    else
    {
        obsLostTimeAvoid = 0;
        iv::decision::DecideGps00::lastDistanceAvoid = -1;
    }




    if (obsDistanceAvoid>0)
    {
        iv::decision::DecideGps00::lastDistanceAvoid = obsDistanceAvoid;
    }

    std::cout << "\nODSAvoid距离:%f\n" << iv::decision::DecideGps00::obsDistanceAvoid << std::endl;
}

void iv::decision::DecideGps00::init() {
    for (int i = 0; i < roadSum; i++)
    {
        lastEsrIdVector.push_back(-1);
        lastEsrCountVector.push_back(0);
        GPS_INS gps_ins;
        gps_ins.gps_x = 0;
        gps_ins.gps_y = 0;
        startAvoidGpsInsVector.push_back(gps_ins);
        avoidMinDistanceVector.push_back(0);
    }
}



#include <QTime>

void iv::decision::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
                                                 const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {

    //    QTime xTime;
    //   xTime.start();
    //    qDebug("time 0 is %d ",xTime.elapsed());
    double obs,obsSd;
    if (lidarGridPtr == NULL)
    {
        lidarDistance = lastLidarDis;
        //	lidarDistance = lastlidarDistance;

    }
    else {
        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
        float lidarXiuZheng=0;
        if(!ServiceCarStatus.useMobileEye){
            lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
        }
        lidarDistance = obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
        //    lidarDistance=-1;
        if (lidarDistance<0)
        {
            lidarDistance = -1;
        }
        lastLidarDis = lidarDistance;
    }


    // obsPredict start
    if(ServiceCarStatus.useObsPredict){
        float preObsDis=200;
        if(!lidar_per.empty()){
            preObsDis=PredictObsDistance(  secSpeed,  gpsTrace, lidar_per);
            lastPreObsDistance=preObsDis;
        }else{
            preObsDis=lastPreObsDistance;
        }
        if(preObsDis<lidarDistance || lidarDistance==-1){
            lidarDistance=preObsDis;
        }
    }
    // obsPredict end

    //    qDebug("time 1 is %d ",xTime.elapsed());
    //    if(gpsMapLine[PathPoint]->roadMode==14||gpsMapLine[PathPoint]->roadMode==15){
    //        lidarDistance=-1;
    //    }

    getEsrObsDistance(gpsTrace, roadNum);

    //   qDebug("time 2 is %d ",xTime.elapsed());
    //  esrDistance=-1;

    //    if(PathPoint>2992 && PathPoint<4134){
    //        lidarDistance=-1;
    //    }


    //    if(PathPoint>10193 && PathPoint<10929){
    //        esrDistance=-1;
    //    }


    if(lidarDistance<0){
        lidarDistance=500;
    }
    if(esrDistance>30){
        esrDistance=500;
    }



    if(esrDistance<0){
        esrDistance=500;
    }


    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;


    //        if(esrDistance>30){
    //            esrDistance=-1;
    //        }



    //            if(esrDistance>=15 && esrDistance<=50&& ServiceCarStatus.mLidarS>0&&ServiceCarStatus.mLidarPer>0 ){
    //                if(abs(lidarDistance-esrDistance)>5){

    //                    esrDistance=-1;
    //                }

    //            }



    //            if(esrDistance>=1&& esrDistance<=50&& ServiceCarStatus.mLidarS>0&&ServiceCarStatus.mLidarPer>0
    //                && gpsMapLine[PathPoint]->runMode == 1    ){
    //                if(abs(lidarDistance-esrDistance)>5){

    //                    esrDistance=-1;
    //                }

    //            }



    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波距离:" << esrDistance << std::endl;
    myesrDistance = esrDistance;



    if(lidarDistance==500){
        lidarDistance=-1;
    }
    if(esrDistance==500){
        esrDistance=-1;
    }


    ServiceCarStatus.mRadarObs = esrDistance;
    ServiceCarStatus.mLidarObs = lidarDistance;


    //zhuanwan pingbi haomibo
    if(gpsTraceNow[0].v1==4||gpsTraceNow[0].v1==5){
        esrDistance=-1;
    }

    if (esrDistance>0 && lidarDistance > 0)
    {
        if (lidarDistance >= esrDistance)
        {
            //	obsDistance = esrDistance;
            obs = esrDistance;
            //	obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
            obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
        }
        else if (!ServiceCarStatus.obs_radar.empty())
        {
            //	obsDistance = lidarDistance;
            obs = lidarDistance;
            //	obsSpeed = Compute00().getObsSpeed(obsPoint, secSpeed);
            obsSd = Compute00().getObsSpeed(obsPoint, secSpeed);
            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
        }
        else
        {
            //	obsDistance = lidarDistance;
            obs=lidarDistance;
            //	obsSpeed = 0 - secSpeed;
            obsSd = 0 -secSpeed;
            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据，计算obsSpeed:" << obsSpeed << std::endl;
        }
    }
    else if (esrDistance>0)
    {
        //		obsDistance = esrDistance;
        //		obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
        obs = esrDistance;
        obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
    }
    else if (lidarDistance > 0)
    {
        //		obsDistance = lidarDistance;
        //		obsSpeed = Compute00().getObsSpeed(obsPoint, secSpeed);
        obs = lidarDistance;
        obsSd = Compute00().getObsSpeed(obsPoint, secSpeed);
        std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
    }
    else {
        //		obsDistance = esrDistance;
        //		obsSpeed = 0 - secSpeed;
        obs = esrDistance;
        obsSd = 0 - secSpeed;
    }

    if(roadNum==roadNow){
        obsDistance=obs;
        obsSpeed=obsSd;
    }


    //    if (obsDistance <0 && obsLostTime<4)
    //    {
    //        obsDistance = lastDistance;
    //        obsLostTime++;
    //    }
    //    else
    //    {
    //        obsLostTime = 0;
    //        lastDistance = -1;
    //    }


    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;

    ServiceCarStatus.mObs = obsDistance;
    if(ServiceCarStatus.mObs>100){
        ServiceCarStatus.mObs =-1;
    }

    if (obsDistance>0)
    {
        lastDistance = obsDistance;
    }

    //lsn
    if(obs<0){
        obsDisVector[roadNum]=500;
    }else{
        obsDisVector[roadNum]=obs;
    }

    givlog->debug("obs_distance","obs: %f, lidar_obs: %f, Radar_obs: %f, lidar_obs_x: %f, lidar_obs_y: %f",
                  ServiceCarStatus.mObs, ServiceCarStatus.mLidarObs,
                  ServiceCarStatus.mRadarObs,obsPoint.x,lidarDistance);

    //   qDebug("time 3 is %d ",xTime.elapsed());

}




//1220
void iv::decision::DecideGps00::computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
                                                     const std::vector<GPSData> gpsMapLine) {

    double obs,obsSd;

    if(!ServiceCarStatus.obs_rear_radar.empty()){
        getRearEsrObsDistance(gpsTrace, roadNum);
    }
    if (lidarGridPtr == NULL)
    {
        lidarDistance = lastLidarDis;
        //	lidarDistance = lastlidarDistance;

    }
    else {
        obsPoint = Compute00().getLidarRearObsPoint(gpsTrace, lidarGridPtr);
        float lidarXiuZheng=0;
        if(!ServiceCarStatus.useMobileEye){
            lidarXiuZheng = ServiceCarStatus.msysparam.rearGpsXiuzheng;
        }
        if(abs(obsPoint.y)>lidarXiuZheng)
            lidarDistance = abs(obsPoint.y)-lidarXiuZheng;   //激光距离推到车头 1220

        //    lidarDistance=-1;
        if (lidarDistance<0)
        {
            lidarDistance = -1;
        }
        lastLidarDis = lidarDistance;
    }

    //    if(gpsMapLine[PathPoint]->roadMode==14||gpsMapLine[PathPoint]->roadMode==15){
    //        lidarDistance=-1;
    //    }

    //  getEsrObsDistance(gpsTrace, roadNum);
    esrDistance=-1;


    //    if(PathPoint>2992 && PathPoint<4134){
    //        lidarDistance=-1;
    //    }


    //    if(PathPoint>10193 && PathPoint<10929){
    //        esrDistance=-1;
    //    }


    if(lidarDistance<0){
        lidarDistance=500;
    }
    if(esrDistance<0){
        esrDistance=500;
    }


    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;


    //        if(esrDistance>30){
    //            esrDistance=-1;
    //        }



    //            if(esrDistance>=15 && esrDistance<=50&& ServiceCarStatus.mLidarS>0&&ServiceCarStatus.mLidarPer>0 ){
    //                if(abs(lidarDistance-esrDistance)>5){

    //                    esrDistance=-1;
    //                }

    //            }



    //            if(esrDistance>=1&& esrDistance<=50&& ServiceCarStatus.mLidarS>0&&ServiceCarStatus.mLidarPer>0
    //                && gpsMapLine[PathPoint]->runMode == 1    ){
    //                if(abs(lidarDistance-esrDistance)>5){

    //                    esrDistance=-1;
    //                }

    //            }



    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波距离:" << esrDistance << std::endl;
    myesrDistance = esrDistance;



    if(lidarDistance==500){
        lidarDistance=-1;
    }
    if(esrDistance==500){
        esrDistance=-1;
    }


    ServiceCarStatus.mRadarObs = esrDistance;
    ServiceCarStatus.mLidarObs = lidarDistance;


    //zhuanwan pingbi haomibo
    if(gpsTraceNow[0].v1==4||gpsTraceNow[0].v1==5){
        esrDistance=-1;
    }

    if (esrDistance>0 && lidarDistance > 0)
    {
        if (lidarDistance >= esrDistance)
        {
            //	obsDistance = esrDistance;
            obs = esrDistance;
            //	obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
            obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
        }
        else if (!ServiceCarStatus.obs_radar.empty())
        {
            //	obsDistance = lidarDistance;
            obs = lidarDistance;
            //	obsSpeed = Compute00().getObsSpeed(obsPoint, secSpeed);
            obsSd = Compute00().getObsSpeed(obsPoint, secSpeed);
            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
        }
        else
        {
            //	obsDistance = lidarDistance;
            obs=lidarDistance;
            //	obsSpeed = 0 - secSpeed;
            obsSd = 0 -secSpeed;
            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据，计算obsSpeed:" << obsSpeed << std::endl;
        }
    }
    else if (esrDistance>0)
    {
        //		obsDistance = esrDistance;
        //		obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
        obs = esrDistance;
        obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
    }
    else if (lidarDistance > 0)
    {
        //		obsDistance = lidarDistance;
        //		obsSpeed = Compute00().getObsSpeed(obsPoint, secSpeed);
        obs = lidarDistance;
        obsSd = Compute00().getObsSpeed(obsPoint, secSpeed);
        std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
    }
    else {
        //		obsDistance = esrDistance;
        //		obsSpeed = 0 - secSpeed;
        obs = esrDistance;
        obsSd = 0 - secSpeed;
    }

    if(roadNum==roadNow){
        obsDistance=obs;
        obsSpeed=obsSd;
    }


    if (obsDistance <0 && obsLostTime<4)
    {
        obsDistance = lastDistance;
        obsLostTime++;
    }
    else
    {
        obsLostTime = 0;
        lastDistance = -1;
    }


    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;

    ServiceCarStatus.mObs = obsDistance;
    if(ServiceCarStatus.mObs>100){
        ServiceCarStatus.mObs =-1;
    }

    if (obsDistance>0)
    {
        lastDistance = obsDistance;
    }

    //lsn
    if(obs<0){
        obsDisVector[roadNum]=500;
    }else{
        obsDisVector[roadNum]=obs;
    }

}



void iv::decision::DecideGps00::predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed){
    double preObsDis;

    if(!lidar_per.empty()){
        preObsDis=PredictObsDistance(  realSpeed,  gpsTrace, lidar_per);
        lastPreObsDistance=preObsDis;

    }else{
        preObsDis=lastPreObsDistance;
    }
    if(preObsDis<obsDistance){
        obsDistance=preObsDis;
        lastDistance=obsDistance;
    }
}

int iv::decision::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
    roadPre = -1;

    //    if (roadNow<roadOri)
    //    {
    //        for (int i = 0; i < roadNow; i++)
    //        {
    //            gpsTraceAvoid.clear();
    //            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
    //            avoidX = (roadWidth)*(roadOri - i);
    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
    //            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
    //            computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
    //        }


    //        for (int i = roadOri+1; i < roadSum; i++)
    //        {
    //            gpsTraceAvoid.clear();
    //            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
    //            avoidX = (roadWidth)*(roadOri - i);
    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
    //            //	bsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
    //            computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
    //        }
    //    }
    //    else if (roadNow>roadOri)
    //    {
    //        for (int i = 0; i < roadOri; i++)
    //        {
    //            gpsTraceAvoid.clear();
    //            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
    //            avoidX = (roadWidth)*(roadOri - i);
    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
    //            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
    //           computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
    //        }
    //        for (int i = roadNow + 1; i < roadSum; i++)
    //        {
    //            gpsTraceAvoid.clear();
    //            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
    //            avoidX = (roadWidth)*(roadOri - i);
    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
    //            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
    //           computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
    //        }

    //    }

    //    else
    //    {
    //        for (int i = 0; i < roadOri; i++)
    //        {
    //            gpsTraceAvoid.clear();
    //            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
    //            avoidX = (roadWidth)*(roadOri - i);
    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
    //            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
    //           computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
    //        }
    //        for (int i = roadOri + 1; i < roadSum; i++)
    //        {
    //            gpsTraceAvoid.clear();
    //            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
    //            avoidX = (roadWidth)*(roadOri - i);
    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
    //            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
    //           computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
    //        }

    //    }

    for (int i =  0; i < roadSum; i++)
    {
        gpsTraceAvoid.clear();
        //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
     //   avoidX = (roadWidth)*(roadOri - i);
         avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
        gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
        //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
        computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
    }


    if (lidarGridPtr!=NULL)
    {
        hasCheckedAvoidLidar = true;
    }

    for(int i=0; i<roadSum;i++){
        std::cout<<"\odsjuli====================:\n"<<i<<"chedaojuli:"<<obsDisVector[i]<<endl;

    }

    checkAvoidObsTimes++;
    if (checkAvoidObsTimes<4 || hasCheckedAvoidLidar==false)
    {
        return - 1;
    }


    for (int i = 1; i < roadSum; i++)
    {
        if (roadNow + i < roadSum) {
        //    avoidX = (roadOri-roadNow-i)*roadWidth;
             avoidX=computeAvoidX(roadNow+i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
            if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow+i))
            {
                /*if (roadNow==roadOri)
                {
                    avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                    startAvoid_gps_ins = now_gps_ins;
                }	*/
                avoidMinDistanceVector[roadNow]= obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                startAvoidGpsInsVector[roadNow] = now_gps_ins;
                roadPre = roadNow + i;
                return roadPre;
            }
        }

        if (roadNow - i >= 0)
        {
         //   avoidX = (roadOri - roadNow+i)*roadWidth;
             avoidX=computeAvoidX(roadNow-i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
            if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow - i))
            {
                /*if (roadNow == roadOri)
                {
                    avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                    startAvoid_gps_ins = now_gps_ins;
                }*/
                avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                startAvoidGpsInsVector[roadNow] = now_gps_ins;
                roadPre = roadNow - i;
                return roadPre;
            }
        }

    }
    return roadPre;
}

int iv::decision::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
    roadPre = -1;

    //   computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);



    //    if (roadNow>=roadOri+1)
    //    {
    //        for (int i = roadOri+1; i < roadNow; i++)
    //        {
    //            gpsTraceBack.clear();
    //            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i-roadOri))], lastGpsIndex);
    //            avoidX = (roadWidth)*(roadOri - i);
    //            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
    //            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
    //        }
    //    }
    //    else if (roadNow <= roadOri - 1) {

    //        for (int i = roadOri - 1; i > roadNow; i--)
    //        {
    //            gpsTraceBack.clear();
    //            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
    //            avoidX = (roadWidth)*(roadOri - i);
    //            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
    //            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
    //        }

    //    }



    for (int i =  0; i <roadSum; i++)
    {
        gpsTraceBack.clear();
        //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
     //   avoidX = (roadWidth)*(roadOri - i);
         avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
        gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
        computeObsOnRoad(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per);
    }



    if (lidarGridPtr != NULL)
    {
        hasCheckedBackLidar = true;
    }



    checkBackObsTimes++;
    if (checkBackObsTimes<4 || hasCheckedBackLidar == false)
    {
        return -1;
    }







    //if ((GetDistance(now_gps_ins, startAvoid_gps_ins)>avoidRunDistance)&&
    //(checkReturnEnable(avoidX, lidarGridPtr,roadOri)))
    if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)) &&
            (checkReturnEnable(avoidX, lidarGridPtr, roadOri)))
    {
        roadPre = roadOri;
        return roadPre;
    }

    if (roadNow-roadOri>1)
    {
        for (int i = roadOri + 1;i < roadNow;i++) {
            if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>  avoidMinDistanceVector[i])&&
                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 30.0))
            {
                roadPre = i;
                return roadPre;
            }
        }
    }

    else if (roadNow <roadOri-1)
    {
        for (int i = roadOri - 1;i > roadNow;i--) {
            if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>avoidMinDistanceVector[i])&&
                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>  30.0))
            {
                roadPre = i;
                return roadPre;
            }
        }
    }

    return roadPre;
}


double iv::decision::DecideGps00::trumpet() {
    if (trumpetFirstCount)
    {
        trumpetFirstCount = false;
        trumpetLastTime= GetTickCount();
        trumpetTimeSpan = 0.0;
    }
    else
    {
        trumpetStartTime= GetTickCount();
        trumpetTimeSpan += trumpetStartTime - trumpetLastTime;
        trumpetLastTime = trumpetStartTime;
    }

    return trumpetTimeSpan;
}




double iv::decision::DecideGps00::transferP() {
    if (transferFirstCount)
    {
        transferFirstCount = false;
        transferLastTime= GetTickCount();
        transferTimeSpan = 0.0;
    }
    else
    {
        transferStartTime= GetTickCount();
        transferTimeSpan += transferStartTime - transferLastTime;
        transferLastTime = transferStartTime;
    }

    return transferTimeSpan;
}



void  iv::decision::DecideGps00::handBrakePark(iv::decision::Decition  decition, long duringTime, GPS_INS now_gps_ins) {

    if (abs(now_gps_ins.speed)>0.1)
    {
        decition->accelerator = 0;
        decition->brake = 20;
        decition->wheel_angle = 0;

    }
    else
    {

        decition->accelerator = 0;
        decition->brake = 20;
        decition->wheel_angle = 0;
        handPark = true;
        handParkTime = duringTime;
    }

}




void iv::decision::DecideGps00::getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins) {
    gmapsL.clear();
    gmapsR.clear();
    for (int i = 0; i < 31; i++)
    {
        std::vector<iv::GPSData> gpsMapLineBeside;
        //	gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, -0.5*i);
        gpsMapLineBeside = iv::decision::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, -0.5*i);

        gmapsL.push_back(gpsMapLineBeside);
    }


    for (int i = 0; i < 31; i++)
    {
        std::vector<iv::GPSData> gpsMapLineBeside;
        //		gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, 0.5*i);
        gpsMapLineBeside = iv::decision::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, 0.5*i);

        gmapsR.push_back(gpsMapLineBeside);
    }

}


bool iv::decision::DecideGps00::checkChaoCheBack(iv::LidarGridPtr lidarGridPtr) {

    if (lidarGridPtr == NULL)
    {
        return false;
        //	lidarDistance = lastlidarDistance;

    }
    else {




        obsPoint = Compute00().getLidarObsPoint(gpsTraceOri, lidarGridPtr);
        double lidarDistance = obsPoint.y - 2.5;   //激光距离推到车头
        //     ODS("\n超车雷达距离:%f\n", lidarDistance);
        if (lidarDistance >-20 && lidarDistance<35)
        {
            checkChaoCheBackCounts = 0;
            return false;
        }
        else {
            checkChaoCheBackCounts++;
        }

        if (checkChaoCheBackCounts>2) {
            checkChaoCheBackCounts = 0;
            return true;
        }
    }

    return false;

}

void iv::decision::DecideGps00::updateGroupDate(GPS_INS now_gps_ins,float realspeed,float theta,float s){
    Point2D pt = Coordinate_Transfer( now_gps_ins.gps_x, now_gps_ins.gps_y, group_ori_gps);

    ServiceCarStatus.group_x_local=pt.x;
    //  ServiceCarStatus.group_y_local=pt.y;
    ServiceCarStatus.group_y_local=s;
    if(realspeed<0.36){
        ServiceCarStatus.group_velx_local=0;
        ServiceCarStatus.group_vely_local=0;
    }else{
        ServiceCarStatus.group_velx_local=realspeed*sin(theta)/3.6;
        ServiceCarStatus.group_vely_local=realspeed*cos(theta)/3.6;
    }


    ServiceCarStatus.group_pathpoint=PathPoint;

}



float  iv::decision::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
                                                           int pathpoint,float secSpeed,float dSpeed){
    float traffic_speed=200;
    float traffic_dis=0;
    float passTime;
    float passSpeed;
    bool passEnable=false;

    if(abs(secSpeed)<0.1){
        secSpeed=0;
    }



    if(pathpoint <= traffic_light_pathpoint){
        for(int i=pathpoint;i<traffic_light_pathpoint;i++){
            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
        }
    }else{
        for(int i=pathpoint;i<gpsMapLine.size()-1;i++){
            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
        }
        for(int i=0;i<traffic_light_pathpoint;i++){
            traffic_dis+=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
        }
    }

    //    if(traffic_light_color != 0)
    //    {
    //        int a = 3;
    //    }


    if(traffic_light_color==0 && traffic_dis<10){
        traffic_speed=0;
    }
    //    else   //20200108
    //    {
    //        traffic_speed=10;
    //    }
    return  traffic_speed;

    passSpeed = min((float)(dSpeed/3.6),secSpeed);
    passTime = traffic_dis/(dSpeed/3.6);





    switch(traffic_light_color){
    case 0:
        if(passTime>traffic_light_time+1 && traffic_dis>10){
            passEnable=true;
        }else{
            passEnable=false;
        }
        break;
    case 1:
        if(passTime<traffic_light_time-1 && traffic_dis<10){
            passEnable=true;
        }else{
            passEnable = false;
        }
        break;
    case 2:
        if(passTime<traffic_light_time){
            passEnable= true;
        }else{
            passEnable=false;
        }
        break;


    default:
        break;
    }

    if(!passEnable){
        if(traffic_dis<5){
            traffic_speed=0;
        }else if(traffic_dis<10){
            traffic_speed=5;
        }else if(traffic_dis<20){
            traffic_speed=15;
        }else if(traffic_dis<30){
            traffic_speed=25;
        }else if(traffic_dis<50){
            traffic_speed=30;
        }
    }
    return traffic_speed;

}

void iv::decision::DecideGps00::computeObsOnRoadByFrenet(iv::LidarGridPtr lidarGridPtr, const std::vector<Point2D>& gpsTrace, double & obs, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps)
{
    //    Point2D obsCombinePoint = Point2D(-1,-1);
    iv::decision::FrenetPoint car_now_frenet_point = iv::decision::FrenetPlanner::getFrenetfromXY(0,0,gpsTrace,gpsMap,pathpoint,nowGps);
    double obsSd;
    if (lidarGridPtr == NULL)
    {
        lidarDistance = lastLidarDis;
        //	lidarDistance = lastlidarDistance;
    }
    else {
        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
        //        lidarDistance = obsPoint.y-2.5;   //激光距离推到车头
        iv::decision::FrenetPoint lidarFPointTmp = iv::decision::FrenetPlanner::getFrenetfromXY(this->obsPoint.x,this->obsPoint.y,gpsTrace,gpsMap,pathpoint,nowGps);
        lidarDistance = lidarFPointTmp.s - car_now_frenet_point.s - 2.5;

        //    lidarDistance=-1;
        if (lidarDistance<0)
        {
            lidarDistance = -1;
        }
        lastLidarDis = lidarDistance;
    }

    FrenetPoint esr_obs_frenet_point;
    getEsrObsDistanceByFrenet(gpsTrace, car_now_frenet_point,esr_obs_frenet_point,gpsMap,pathpoint,nowGps);

    if(lidarDistance<0){
        lidarDistance=500;
    }
    if(esrDistance<0){
        esrDistance=500;
    }

    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;

    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波距离:" << esrDistance << std::endl;
    myesrDistance = esrDistance;

    if(lidarDistance==500){
        lidarDistance=-1;
    }
    if(esrDistance==500){
        esrDistance=-1;
    }

    ServiceCarStatus.mRadarObs = esrDistance;
    ServiceCarStatus.mLidarObs = lidarDistance;

    //    //zhuanwan pingbi haomibo
    //    if(gpsTraceNow[0].v1==4||gpsTraceNow[0].v1==5){
    //        esrDistance=-1;
    //    }

    if (esrDistance>0 && lidarDistance > 0)
    {
        if (lidarDistance >= esrDistance)
        {
            obs = esrDistance;
            //            obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
            obsSd = obsSpeed;
            //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
            //            obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
        }
        else if (!ServiceCarStatus.obs_radar.empty())
        {
            obs = lidarDistance;
            //            obsCombinePoint = obsPoint;
            //            obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace);
            obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
        }
        else
        {
            obs=lidarDistance;
            //            obsCombinePoint = obsPoint;
            obsSd = 0 -secSpeed*cos(car_now_frenet_point.tangent_Ang-PI/2);
            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据，计算obsSpeed:" << obsSpeed << std::endl;
        }
    }
    else if (esrDistance>0)
    {
        obs = esrDistance;
        //        obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
        obsSd = obsSpeed;
        //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
        //        obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
    }
    else if (lidarDistance > 0)
    {
        obs = lidarDistance;
        //        obsCombinePoint = obsPoint;
        obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
        std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
    }
    else {
        obs = esrDistance;
        //        obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
        obsSd = 0 - secSpeed*cos(car_now_frenet_point.tangent_Ang-PI/2);
    }

    obsDistance=obs;
    obsSpeed=obsSd;

    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;

    ServiceCarStatus.mObs = obsDistance;
    if(ServiceCarStatus.mObs>100){
        ServiceCarStatus.mObs =-1;
    }

    if (obsDistance>0)
    {
        lastDistance = obsDistance;
    }

    if(obs<0){
        obsDistance=500;
    }else{
        obsDistance=obs;
    }
}

void iv::decision::DecideGps00::getRearEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {


    esrIndex = iv::decision::Compute00().getRearEsrIndex(gpsTrace, roadNum);

    if (esrIndex != -1)
    {
        esrDistance = ServiceCarStatus.obs_rear_radar[esrIndex].nomal_y;
        obsSpeed = ServiceCarStatus.obs_rear_radar[esrIndex].speed_y;

    }
    else {
        esrDistance = -1;
    }
}

void iv::decision::DecideGps00::getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, const FrenetPoint car_now_frenet_point, FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps) {

    esrIndex = iv::decision::Compute00().getEsrIndexByFrenet(gpsTrace,esrObs_F_Point,gpsMap,pathpoint,nowGps);

    if (esrIndex != -1)
    {
        //障碍物的距离，是障碍物和车辆沿道路上的距离，即s。并不直接是它的x坐标或y坐标或两点直线坐标。
        //严格来说应是 esrDistance=障碍物的s - 车辆的s - Esr_Y_Offset，但这是根据车辆位置实时搜索路径，故车辆的s极其小。
        esrDistance = esrObs_F_Point.s - car_now_frenet_point.s - Esr_Y_Offset;  //-Esr_Y_Offset是把距离推到车头，换算frenet坐标时，把它加上了的。故需减去。

        double speedx=ServiceCarStatus.obs_radar[esrIndex].speed_x;  //障碍物相对于车辆x轴的速度
        double speedy=ServiceCarStatus.obs_radar[esrIndex].speed_y;  //障碍物相对于车辆y轴的速度
        double speed_combine = sqrt(speedx*speedx+speedy*speedy);    //将x、y轴两个方向的速度求矢量和
        //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
        //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
        double Etheta = esrObs_F_Point.tangent_Ang - atan2(speedy,speedx);

        obsSpeed = speed_combine*cos(Etheta);  //由speed_combine分解的s轴方向上的速度
    }
    else {
        esrDistance = -1;
    }
}

void iv::decision::DecideGps00::getV2XTrafficPositionVector(const std::vector<GPSData> gpsMapLine){
    v2xTrafficVector.clear();
    for (int var = 0; var < gpsMapLine.size(); var++) {
        if(gpsMapLine[var]->roadMode==6 || gpsMapLine[var]->mode2==1000001){
            v2xTrafficVector.push_back(var);
        }
    }
}

float iv::decision::DecideGps00::ComputeV2XTrafficLightSpeed(iv::TrafficLight trafficLight, const std::vector<GPSData> gpsMapLine,std::vector<int> v2xTrafficVector,
                                                             int pathpoint,float secSpeed,float dSpeed, bool circleMode){
    float trafficSpeed=200;
    int nearTraffixPoint=-1;
    float nearTrafficDis=0;
    int traffic_color=0;
    int traffic_time=0;
    bool passThrough=false;
    float dSecSpeed=dSpeed/3.6;

    if(v2xTrafficVector.empty()){
        return trafficSpeed;
    }
    if(!circleMode){
        if(pathpoint>v2xTrafficVector.back()){
            return trafficSpeed;
        }else {
            for(int i=0; i< v2xTrafficVector.size();i++){
                if (pathpoint<= v2xTrafficVector[i]){
                    nearTraffixPoint=v2xTrafficVector[i];
                    break;
                }
            }
        }
    }else if(circleMode){
        if(pathpoint>v2xTrafficVector.back()){
            nearTraffixPoint=v2xTrafficVector[0];
        }else {
            for(int i=0; i< v2xTrafficVector.size();i++){
                if (pathpoint<= v2xTrafficVector[i]){
                    nearTraffixPoint=v2xTrafficVector[i];
                    break;
                }
            }
        }
    }


    if(nearTraffixPoint!=-1){
        for(int i=pathpoint;i<nearTraffixPoint;i++){
            nearTrafficDis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
        }
    }

    if(nearTrafficDis>50){
        return trafficSpeed;
    }


    int roadMode = gpsMapLine[pathpoint]->roadMode;
    if(roadMode==14 || roadMode==16){
        traffic_color=trafficLight.leftColor;
        traffic_time=trafficLight.leftTime;
    }else if(roadMode==15 ||roadMode==17){
        traffic_color=trafficLight.rightColor;
        traffic_time=trafficLight.rightTime;
    }else {
        traffic_color=trafficLight.straightColor;
        traffic_time=trafficLight.straightTime;
    }


    passThrough=computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,secSpeed,dSecSpeed);
    if(passThrough){
        return trafficSpeed;
    }else{
        trafficSpeed=computeTrafficSpeedLimt(nearTrafficDis);
        if(nearTrafficDis<6){
            float decelerate =0-( secSpeed*secSpeed*0.5/nearTrafficDis);
            minDecelerate=min(minDecelerate,decelerate);
        }
        return trafficSpeed;
    }

    return trafficSpeed;
}

bool iv::decision::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed){
    float passTime=0;
    if (trafficColor==2 || trafficColor==3){
        return false;
    }else if(trafficColor==0){
        return true;
    }else{
        passTime=trafficDis/dSecSpeed;
        if(passTime+1< trafficTime){
            return true;
        }else{
            return false;
        }

    }

}

float iv::decision::DecideGps00::computeTrafficSpeedLimt(float trafficDis){
    float limit=200;
    if(trafficDis<10){
        limit = 0;
    }else if(trafficDis<15){
        limit = 5;
    }else if(trafficDis<20){
        limit=10;
    }else if(trafficDis<30){
        limit=15;
    }
    return limit;
}



bool iv::decision::DecideGps00::adjuseultra(){
    bool front=false,back=false,left=false,right=false;
    for(int i=1;i<=13;i++)
    {
        if((i==2)||(i==3)||(i==4)||(i==5))   //front
        {
            if(ServiceCarStatus.ultraDistance[i]<100)
            {
                front=true;
            }
        }else if((i==1)||(i==12)||(i==6)||(i==7))   //left,right
        {
            if(ServiceCarStatus.ultraDistance[i]<30)
            {
                left=true;
            }
        }else if((i==8)||(i==9)||(i==10)||(i==11))   //back
        {
            if(ServiceCarStatus.ultraDistance[i]<10)
            {
                back=true;
            }
        }
    }

    if(front||left||back)
        return true;
    else
        return false;

}

void iv::decision::DecideGps00::transferGpsMode2( const std::vector<GPSData> gpsMapLine){
    if(  gpsMapLine[PathPoint]->mode2==3000){
        if(obsDistance>5){
            obsDistance=200;
        }
        dSpeed=min(dSpeed,5.0);
    }
}
float iv::decision::DecideGps00::computeAvoidX(int roadAim,int roadOri,GPSData gps,float vehWidth){
    if(roadAim==roadOri){
        return 0;
    }
    float x=0;
    float veh_to_roadSide=(gps->mfRoadWidth- ServiceCarStatus.msysparam.vehWidth)*0.5;
    float roadSide_to_roadSide=ServiceCarStatus.msysparam.vehWidth;
    if(!ServiceCarStatus.inRoadAvoid){
        x= (roadOri-roadAim)*gps->mfRoadWidth;
    }else{
        int num=roadOri-roadAim;
        switch (abs(num%3)) {
        case 0:
            x=(num/3)*gps->mfRoadWidth;
            break;
        case 1:
            if(num>0){
                x=(num/3)*gps->mfRoadWidth +veh_to_roadSide;
            }else{
                x=(num/3)*gps->mfRoadWidth -veh_to_roadSide;
            }
            break;
        case 2:
            if(num>0){
                x=(num/3)*gps->mfRoadWidth +veh_to_roadSide+roadSide_to_roadSide;
            }else{
                x=(num/3)*gps->mfRoadWidth -veh_to_roadSide-roadSide_to_roadSide;
            }

            break;
        default:
            break;
        }
    }

    return x;
}