modularization/src1/decision/decision_brain/ivdecision_brain.cpp

#include "ivdecision_brain.h"

#include "common/obs_predict.h"
#include "ivlog.h"

extern iv::Ivlog * givlog;

bool handPark;
long handParkTime;
bool rapidStop;
int gpsMissCount;
bool changeRoad;
double avoidX;
bool parkBesideRoad;
double steerSpeed;
bool transferPieriod;
bool transferPieriod2;
double traceDevi;


using namespace  iv::decition;

namespace iv {


ivdecision_brain::ivdecision_brain()
{
    mvehState = VehState::normalRun;
}


int ivdecision_brain::getdecision(brain::decition &xdecition,iv::brain::brainstate & xbs)
{
    static qint64 nlastdecisiontime = 0;
    static qint64 nLastMapUpdate = 0;
    iv::GPSData now_gps_ins;
    if(GetGPS(now_gps_ins) == false)
    {
        return -1;   //No GPS Position
    }
    if(IsMAPUpdate(nLastMapUpdate))
    {
        GetMAP(mgpsMapLine,nLastMapUpdate);
        mbisFirstRun = true;
    }

    iv::LidarGridPtr lidarptr;
    GetLIDARGrid(lidarptr);

    std::vector<iv::Perception::PerceptionOutput> xvectorper;

    iv::TrafficLight xtrafficlight;
    std::shared_ptr<iv::vbox::vbox> xvbox_ptr;
    if(Getvboxmsg(xvbox_ptr))
    {
        xtrafficlight.leftColor=xvbox_ptr->st_left();
        xtrafficlight.rightColor=xvbox_ptr->st_right();
        xtrafficlight.straightColor=xvbox_ptr->st_straight();
        xtrafficlight.uturnColor=xvbox_ptr->st_turn();
        xtrafficlight.leftTime=xvbox_ptr->time_left();
        xtrafficlight.rightTime=xvbox_ptr->time_right();
        xtrafficlight.straightTime=xvbox_ptr->time_straight();
        xtrafficlight.uturnTime=xvbox_ptr->time_turn();
    }

    updatev2x();
    updateultra();
    updateradar();

    iv::decition::Decition decition= getDecideFromGPS(*now_gps_ins,mgpsMapLine,lidarptr,xvectorper,xtrafficlight);

    xdecition.set_accelerator(decition->accelerator);
    xdecition.set_brake(decition->brake);
    xdecition.set_leftlamp(decition->leftlamp);
    xdecition.set_rightlamp(decition->rightlamp);
    xdecition.set_speed(decition->speed);
    xdecition.set_wheelangle(decition->wheel_angle);
    xdecition.set_wheelspeed(decition->angSpeed);
    xdecition.set_torque(decition->torque);
    xdecition.set_mode(decition->mode);
    xdecition.set_gear(decition->dangWei);
    xdecition.set_handbrake(decition->handBrake);
    xdecition.set_grade(decition->grade);
    xdecition.set_engine(decition->engine);
    xdecition.set_brakelamp(decition->brakeLight);
    xdecition.set_ttc(ServiceCarStatus.mfttc);
    xdecition.set_air_enable(decition->air_enable);
    xdecition.set_air_temp(decition->air_temp);
    xdecition.set_air_mode(decition->air_mode);
    xdecition.set_wind_level(decition->wind_level);
    xdecition.set_roof_light(decition->roof_light);
    xdecition.set_home_light(decition->home_light);
    xdecition.set_air_worktime(decition->air_worktime);
    xdecition.set_air_offtime(decition->air_offtime);
    xdecition.set_air_on(decition->air_on);
    xdecition.set_door(decition->door);

    xbs.set_mbbocheenable(ServiceCarStatus.bocheEnable);
    xbs.set_mbbrainrunning(true);  //apollo_fu debug ui 20200417
    xbs.set_mflidarobs(ServiceCarStatus.mLidarObs);
    xbs.set_mfradarobs(ServiceCarStatus.mRadarObs);
    xbs.set_mfobs(ServiceCarStatus.mObs);
    xbs.set_decision_period((QDateTime::currentMSecsSinceEpoch() - nlastdecisiontime));
    nlastdecisiontime = QDateTime::currentMSecsSinceEpoch();

    return 0;
}

void ivdecision_brain::updatev2x()
{
    std::shared_ptr<iv::v2x::v2x> xv2x_ptr;

    if(false == Getv2xmsg(xv2x_ptr))
    {
        return;
    }

    ServiceCarStatus.stationCmd.received=true;

    ServiceCarStatus.stationCmd.has_carID=xv2x_ptr->has_carid();
    if(ServiceCarStatus.stationCmd.has_carID)
    {
            ServiceCarStatus.stationCmd.carID=xv2x_ptr->carid();
    }

    ServiceCarStatus.stationCmd.has_carMode=xv2x_ptr->has_carmode();
    if(ServiceCarStatus.stationCmd.has_carMode)
    {
            ServiceCarStatus.stationCmd.carMode=xv2x_ptr->carmode();

            qDebug("ServiceCarStatus.stationCmd.carMode:",ServiceCarStatus.stationCmd.carMode);
            givlog->debug("brain_v2x","ServiceCarStatus.stationCmd.carMode:%d",ServiceCarStatus.stationCmd.carMode);
    }

    ServiceCarStatus.stationCmd.has_emergencyStop=xv2x_ptr->has_emergencystop();
    if(ServiceCarStatus.stationCmd.has_emergencyStop)
    {
            ServiceCarStatus.stationCmd.emergencyStop=xv2x_ptr->emergencystop();

            qDebug("ServiceCarStatus.stationCmd.emergencyStop:",ServiceCarStatus.stationCmd.emergencyStop);
            givlog->debug("brain_v2x","ServiceCarStatus.stationCmd.emergencyStop:%d",ServiceCarStatus.stationCmd.emergencyStop);
    }

    ServiceCarStatus.stationCmd.has_stationStop=xv2x_ptr->has_stationstop();
    if(ServiceCarStatus.stationCmd.has_stationStop)
    {
            ServiceCarStatus.stationCmd.stationStop=xv2x_ptr->stationstop();

            qDebug("ServiceCarStatus.stationCmd.stationStop:",ServiceCarStatus.stationCmd.stationStop);
            givlog->debug("brain_v2x","ServiceCarStatus.stationCmd.stationStop:%d",ServiceCarStatus.stationCmd.stationStop);
    }
    int num=xv2x_ptr->stationid_size();
    if(num>0)
    {
        ServiceCarStatus.stationCmd.stationTotalNum=num;
        for(int i=0;i<ServiceCarStatus.stationCmd.stationTotalNum;i++)
        {
            ServiceCarStatus.stationCmd.stationGps[i].gps_lat=xv2x_ptr->stgps(i).lat();
            ServiceCarStatus.stationCmd.stationGps[i].gps_lng=xv2x_ptr->stgps(i).lon();

            qDebug("stationGps: %d, lat: %.7f, lon: %.7f", xv2x_ptr->stationid(i), ServiceCarStatus.stationCmd.stationGps[i].gps_lat, ServiceCarStatus.stationCmd.stationGps[i].gps_lng);
            givlog->debug("brain_v2x","stationGps: %d, lat: %.7f, lon: %.7f",
                           xv2x_ptr->stationid(i),ServiceCarStatus.stationCmd.stationGps[i].gps_lat,
                          ServiceCarStatus.stationCmd.stationGps[i].gps_lng);
        }
        ServiceCarStatus.mbRunPause=false;
    }

}

void ivdecision_brain::updateultra()
{
    std::shared_ptr<iv::ultrasonic::ultrasonic> xultra_ptr;

    if(false == Getultrasonic(xultra_ptr))
    {
        return;
    }

    ServiceCarStatus.ultraDistance[1]=xultra_ptr->sigobjdist_flcorner();
    ServiceCarStatus.ultraDistance[2]=xultra_ptr->sigobjdist_flmiddle();
    ServiceCarStatus.ultraDistance[3]=xultra_ptr->sigobjdist_flside();
    ServiceCarStatus.ultraDistance[4]=xultra_ptr->sigobjdist_frcorner();
    ServiceCarStatus.ultraDistance[5]=xultra_ptr->sigobjdist_frmiddle();
    ServiceCarStatus.ultraDistance[6]=xultra_ptr->sigobjdist_frside();
    ServiceCarStatus.ultraDistance[7]=xultra_ptr->sigobjdist_rlcorner();
    ServiceCarStatus.ultraDistance[8]=xultra_ptr->sigobjdist_rlmiddle();
    ServiceCarStatus.ultraDistance[9]=xultra_ptr->sigobjdist_rlside();
    ServiceCarStatus.ultraDistance[10]=xultra_ptr->sigobjdist_rrcorner();
    ServiceCarStatus.ultraDistance[11]=xultra_ptr->sigobjdist_rrmiddle();
    ServiceCarStatus.ultraDistance[12]=xultra_ptr->sigobjdist_rrside();
}

void ivdecision_brain::updateradar()
{
    std::shared_ptr<iv::radar::radarobjectarray> xradar_ptr
            = std::shared_ptr<iv::radar::radarobjectarray>(new iv::radar::radarobjectarray);

    unsigned int i;
    if(false == GetRADAR(xradar_ptr))
    {
        for(i=0;i<64;i++)
        {
            ServiceCarStatus.obs_radar[i].valid = false;
        }
        return;
    }

    for(i=0;i<64;i++)
    {
        iv::radar::radarobject * pradarobj = xradar_ptr->mutable_obj(i);
        ServiceCarStatus.obs_radar[i].valid = pradarobj->bvalid();
        ServiceCarStatus.obs_radar[i].nomal_x = pradarobj->x();
        ServiceCarStatus.obs_radar[i].nomal_y = pradarobj->y();
        ServiceCarStatus.obs_radar[i].speed_relative = pradarobj->vel();
        ServiceCarStatus.obs_radar[i].speed_x = pradarobj->vx();
        ServiceCarStatus.obs_radar[i].speed_y = pradarobj->vy();
    }


}


//int ivdecision_brain::getdecision_normal(brain::decition &xdecition)
//{

//    iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);

//    double obsDistance = -1;
//    int esrLineIndex = -1;
//    double lidarDistance = -1;
//    int roadPre = -1;

//    return 0;
//}

//std::vector<iv::Perception::PerceptionOutput> lidar_per,



iv::decition::Decition ivdecision_brain::decision_reverseCar(GPS_INS now_gps_ins)
{
    iv::decition::Compute00().bocheCompute(now_gps_ins, aim_gps_ins);

    vehState = reversing;
    qiedianCount=true;
    return decision_reversing(now_gps_ins);
}




iv::decition::Decition ivdecision_brain::decision_reversing(GPS_INS now_gps_ins)
{

    iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);

    double dis = GetDistance(now_gps_ins, iv::decition::Compute00().nearTpoint);
    if (dis<2.0)//0.5
    {
        vehState = reverseCircle;
        qiedianCount = true;
        cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
        return decision_reverseCircle(now_gps_ins);
    }
    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;
    }
}


iv::decition::Decition ivdecision_brain::decision_reverseCircle(GPS_INS now_gps_ins)
{
    iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
    double dis = GetDistance(now_gps_ins, iv::decition::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;
        return decision_reverseDirect(now_gps_ins);

    }
    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 = iv::decition::Compute00().bocheAngle*16.5;
        gps_decition->wheel_angle = 0 - controlAng*1.05;

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

        return gps_decition;
    }
}

iv::decition::Decition ivdecision_brain::decision_reverseDirect(GPS_INS now_gps_ins)
{
    iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);

    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;
    }
}


iv::decition::Decition ivdecision_brain::decision_reverseArr(iv::GPS_INS now_gps_ins)
{
    iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);

    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;


}

iv::decition::Decition ivdecision_brain::decision_dRever(GPS_INS now_gps_ins)
{
    GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);

    iv::decition::Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);

    vehState = dRever0;
    qiedianCount=true;


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

    return decision_dRever0(now_gps_ins);
}

iv::decition::Decition ivdecision_brain::decision_dRever0(GPS_INS now_gps_ins)
{
    iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);

    double dis = GetDistance(now_gps_ins, iv::decition::Compute00().dTpoint0);
    if (dis<2.0)
    {
        vehState = dRever1;
        qiedianCount = true;
        cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
        return decision_dRever1(now_gps_ins);
    }
    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;
    }
}



iv::decition::Decition ivdecision_brain::decision_dRever1(GPS_INS now_gps_ins)
{
    iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);

    double dis = GetDistance(now_gps_ins, iv::decition::Compute00().dTpoint1);

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

        return decision_dRever2(now_gps_ins);

    }
    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 = iv::decition::Compute00().dBocheAngle*16.5;
        gps_decition->wheel_angle = 0 - controlAng;

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

        return gps_decition;
    }
}
iv::decition::Decition ivdecision_brain::decision_dRever2(GPS_INS now_gps_ins)
{
    iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);

    double dis = GetDistance(now_gps_ins, iv::decition::Compute00().dTpoint2);
    Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
    Point2D pt2 = Coordinate_Transfer(iv::decition::Compute00().dTpoint2.gps_x,iv::decition::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;

        return decision_dRever3(now_gps_ins);


    }
    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;
    }
}
iv::decition::Decition ivdecision_brain::decision_dRever3(GPS_INS now_gps_ins)
{
    iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
    double dis = GetDistance(now_gps_ins, iv::decition::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;
        return decision_dRever4(now_gps_ins);

    }
    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-iv::decition::Compute00().dBocheAngle*16.5;
        gps_decition->wheel_angle = 0 - controlAng*0.95;

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

        return gps_decition;
    }
}
iv::decition::Decition ivdecision_brain::decision_dRever4(GPS_INS now_gps_ins)
{
    iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);

    //	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;
    }
}


void ivdecision_brain::decision_firstRun(GPS_INS now_gps_ins,
                                         const std::vector<GPSData> & gpsMapLine)
{

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

        if(ServiceCarStatus.speed_control==true){
            iv::decition::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 =  iv::decition::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;
    }


}

int ivdecision_brain::decision_ParkCalc(GPS_INS now_gps_ins,iv::decition::Decition & gps_decition)
{
    //sidePark

    if(ServiceCarStatus.mnParkType==1){

        if( vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 &&  vehState!=dRever2  &&  vehState!=dRever3 && vehState!=dRever4 && vehState!=reverseArr ){
            int bocheEN=iv::decition::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 1;
            }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=iv::decition::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 1;
            }else{
                if(trumpet()>3000){
                    trumpetFirstCount=true;
                    vehState=reverseCar;
                }

            }
            //     ServiceCarStatus.bocheMode=false;

        }
    }

    return 0;


}

void ivdecision_brain::decision_readyZhucheMode(GPS_INS now_gps_ins, const std::vector<GPSData> &gpsMapLine)
{
    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;





        }

    }

}

void ivdecision_brain::decision_readyParkMode(GPS_INS now_gps_ins, const std::vector<GPSData> &gpsMapLine)
{
    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;
        }


    }
}

void ivdecision_brain::decision_speedctrl(Decition &gps_decition,const std::vector<GPSData> & gpsMapLine)
{
    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;
}

//std::vector<iv::Perception::PerceptionOutput> lidar_per,
iv::decition::Decition ivdecision_brain::getDecideFromGPS(GPS_INS now_gps_ins,
                                                                   const std::vector<GPSData> gpsMapLine,
                                                                   iv::LidarGridPtr lidarGridPtr,
                                                          std::vector<iv::Perception::PerceptionOutput> lidar_per,
                                                                   iv::TrafficLight trafficLight)
{

    iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);


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


    if (isFirstRun)
    {
        decision_firstRun(now_gps_ins,gpsMapLine);
    }


    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);
    }


    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;
    }


    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;
    }

    //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;

    realspeed = now_gps_ins.speed;

    secSpeed = realspeed / 3.6;



    if(decision_ParkCalc(now_gps_ins,gps_decition) == 1)
    {
        return gps_decition;
    }

    switch (vehState) {
    // ChuiZhiTingChe
    case reverseCar:
        return decision_reverseCar(now_gps_ins);
        break;
    case reversing:
        return decision_reversing(now_gps_ins);
        break;
    case reverseCircle:
        return decision_reverseCircle(now_gps_ins);
        break;
    case reverseDirect:
        return decision_reverseDirect(now_gps_ins);
        break;
    case reverseArr:
        return decision_reverseArr(now_gps_ins);
        break;
         //ceFangWeiBoChe
    case dRever:
        return decision_dRever(now_gps_ins);
        break;
    case dRever0:
        return decision_dRever0(now_gps_ins);
        break;
    case dRever1:
        return decision_dRever1(now_gps_ins);
        break;
    case dRever2:
        return decision_dRever2(now_gps_ins);
        break;
    case dRever3:
        return decision_dRever3(now_gps_ins);
        break;
    case dRever4:
        return decision_dRever4(now_gps_ins);
        break;
    default:
        break;
    }



    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 = iv::decition::Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, lastGpsIndex, minDis, maxAngle);
        //    if(PathPoint<0){
        PathPoint = iv::decition::Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine, lastGpsIndex, minDis, maxAngle);
        //    }

    }


    if (PathPoint<0)
    {

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

    }

    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;
    }


    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)
    {
        decision_readyZhucheMode(now_gps_ins,gpsMapLine);
    }


    if (readyParkMode)
    {
        decision_readyParkMode(now_gps_ins,gpsMapLine);
    }

    decision_speedctrl(gps_decition,gpsMapLine);

    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




    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 = iv::decition::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  ivdecision_brain::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  ivdecision_brain::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  ivdecision_brain::phaseSpeedDecition(iv::decition::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>  ivdecision_brain::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode) {
    vector<iv::Point2D> trace;
    //	int  PathPoint = iv::decition::Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, lastGpsIndex, minDis, 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;
                zhuchePointNum = index;
            }



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

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

            //			if (pt.v1 == 22 || pt.v1 == 23)
            //			{
            //				readyParkMode = true;
            //				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>   ivdecision_brain::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 + M_PI / 2),
                           carFronty + avoidLenth  * sin(anglevalue + M_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 - M_PI / 2),
                            carFronty + avoidLenth  * sin(anglevalue - M_PI / 2));
            ptRight.speed = gpsTrace[j].speed;
            ptRight.v1 = gpsTrace[j].v1;
            ptRight.v2 = gpsTrace[j].v2;


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


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

double  ivdecision_brain::getSpeed(std::vector<Point2D> gpsTrace) {
    double speed=0;
    int speedPoint = iv::decition::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  ivdecision_brain::getEsrObs(std::vector<iv::ObstacleBasic> obsRadars) {
//
//	if (!obsRadars.empty())
//	{
//		esrIndex = iv::decition::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  ivdecision_brain::getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {


    int esrPathpoint;

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

    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  ivdecision_brain::getEsrObsDistanceAvoid() {

    esrIndexAvoid = iv::decition::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  ivdecision_brain::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  ivdecision_brain::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  ivdecision_brain::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  ivdecision_brain::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  ivdecision_brain::getObsAvoid(iv::LidarGridPtr lidarGridPtr) {



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

    //lidarDistanceAvoid = -1;   //20200103 apollo_fu

    if (esrDistanceAvoid>0 && lidarDistanceAvoid > 0)
    {
        if (lidarDistanceAvoid >= esrDistanceAvoid)
        {
            obsDistanceAvoid = esrDistanceAvoid;
            obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
        }
        else if (!ServiceCarStatus.obs_radar.empty())
        {
            obsDistanceAvoid = lidarDistanceAvoid;
            obsSpeedAvoid = iv::decition::Compute00().getObsSpeed(obsPointAvoid, secSpeed);
            std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
        }
        else
        {
            obsDistanceAvoid = 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 (lidarDistanceAvoid > 0)
    {
        obsDistanceAvoid = lidarDistanceAvoid;
        obsSpeedAvoid = iv::decition::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 (obsDistanceAvoid <0 && obsLostTimeAvoid<4)
    {
        obsDistanceAvoid = lastDistanceAvoid;
        obsLostTimeAvoid++;
    }
    else
    {
        obsLostTimeAvoid = 0;
        lastDistanceAvoid = -1;
    }




    if (obsDistanceAvoid>0)
    {
        lastDistanceAvoid = obsDistanceAvoid;
    }

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

void  ivdecision_brain::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  ivdecision_brain::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 = iv::decition::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);

    double mobieye_ttc,mobieye_obsid,mobieye_speed,mobieye_distance;

    double fveh_width = 2.0;
#ifdef USE_MOBIEYE_OBS
    MobieyeInst.GetObsFromMobieye(&gpsTrace,mobieye_distance,mobieye_speed,mobieye_ttc,mobieye_obsid,fveh_width);
#endif
    //   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;
    }

#ifdef USE_MOBIEYE_OBS
    esrDistance = mobieye_distance;

    if(esrDistance>150){
        esrDistance=500;
    }
#else
    if(esrDistance>30){
        esrDistance=500;
    }
#endif



    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)
        {
#ifdef USE_MOBIEYE_OBS
            obs = esrDistance;
            obsSd = mobieye_speed;
#else
            //	obsDistance = esrDistance;
            obs = esrDistance;
            //	obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
            obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
#endif
        }
        else if (!ServiceCarStatus.obs_radar.empty())
        {
            //	obsDistance = lidarDistance;
            obs = lidarDistance;
            //	obsSpeed = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
            obsSd = iv::decition::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;

#ifdef USE_MOBIEYE_OBS
        obs = esrDistance;
        obsSd = mobieye_speed;
#else
        obs = esrDistance;
        obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
#endif
    }
    else if (lidarDistance > 0)
    {
        //		obsDistance = lidarDistance;
        //		obsSpeed = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
        obs = lidarDistance;
        obsSd = iv::decition::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  ivdecision_brain::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 = iv::decition::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 = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
            obsSd = iv::decition::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 = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
        obs = lidarDistance;
        obsSd = iv::decition::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  ivdecision_brain::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  ivdecision_brain::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  ivdecision_brain::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  ivdecision_brain::trumpet() {
    if (trumpetFirstCount)
    {
        trumpetFirstCount = false;
        trumpetLastTime= GetTickCount();
        trumpetTimeSpan = 0.0;
    }
    else
    {
        trumpetStartTime= GetTickCount();
        trumpetTimeSpan += trumpetStartTime - trumpetLastTime;
        trumpetLastTime = trumpetStartTime;
    }

    return trumpetTimeSpan;
}




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

    return transferTimeSpan;
}



void   ivdecision_brain::handBrakePark(iv::decition::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  ivdecision_brain::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::decition::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::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, 0.5*i);

        gmapsR.push_back(gpsMapLineBeside);
    }

}


bool  ivdecision_brain::checkChaoCheBack(iv::LidarGridPtr lidarGridPtr) {

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

    }
    else {




        obsPoint = iv::decition::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  ivdecision_brain::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   ivdecision_brain::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  ivdecision_brain::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::decition::FrenetPoint car_now_frenet_point = iv::decition::FrenetPlanner::getFrenetfromXY(0,0,gpsTrace,gpsMap,pathpoint,nowGps);
    double obsSd;
    if (lidarGridPtr == NULL)
    {
        lidarDistance = lastLidarDis;
        //	lidarDistance = lastlidarDistance;
    }
    else {
        obsPoint = iv::decition::Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
        //        lidarDistance = obsPoint.y-2.5;   //激光距离推到车头
        iv::decition::FrenetPoint lidarFPointTmp = iv::decition::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 = iv::decition::Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace);
            obsSd = iv::decition::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-M_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 = iv::decition::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-M_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  ivdecision_brain::getRearEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {


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

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

    }
    else {
        esrDistance = -1;
    }
}

void  ivdecision_brain::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::decition::Compute00().getEsrIndexByFrenet(gpsTrace,esrObs_F_Point,gpsMap,pathpoint,nowGps,xiuzhengCs);

    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  ivdecision_brain::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  ivdecision_brain::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  ivdecision_brain::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  ivdecision_brain::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  ivdecision_brain::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  ivdecision_brain::transferGpsMode2( const std::vector<GPSData> gpsMapLine){
    if(  gpsMapLine[PathPoint]->mode2==3000){
        if(obsDistance>5){
            obsDistance=200;
        }
        dSpeed=min(dSpeed,5.0);
    }
}
float  ivdecision_brain::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;
}


}