modularization/src/decition/decition_brain_sf_changan_shenlan/decition/brain.cpp

#include <decition/brain.h>
#include <fstream>
#include <time.h>
#include <exception>
#include <decition/adc_tools/gnss_coordinate_convert.h>
#include <common/logout.h>
#include <decition/adc_tools/transfer.h>

#include <iostream>
#include "xmlparam.h"
#include "qcoreapplication.h"
#include <fstream>
#include <QTime>

#include "ivfault.h"
extern iv::Ivfault * givfault;

using namespace std;

extern std::string gstrmemdecition;
extern std::string gstrmembrainstate;
extern iv::Ivlog * givlog;
extern std::string gstrmemchassis;
extern std::string gstrmembraincarstate;

#define LOG_ENABLE

namespace iv {
namespace decition {
        iv::decition::BrainDecition * gbrain;
        double gplanbrakeacc=0.3;
        double gplanacc=1.0;

        void ListenTraceMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
        {
            gbrain->UpdateMap(strdata,nSize);
            gbrain->UpdateSate();
        }

        void ListenVbox(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
        {
            gbrain->UpdateVbox(strdata,nSize);
            gbrain->UpdateSate();
        }

        void ListenV2r(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
        {
            gbrain->UpdateV2r(strdata,nSize);
//            gbrain->UpdateSate();
        }

        void ListenHMI(const char * strdata,const unsigned int nSize ,const unsigned int index,const QDateTime * dt,const char * strmemname)
        {
            gbrain->UpdateHMI(strdata,nSize);
        }

        void ListenPlatform(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
        {
            gbrain->UpdatePlatform(strdata,nSize);
        }

        void ListenGroup(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
        {
            gbrain->UpdateGroupInfo(strdata,nSize);
        }

        void ListenFusion(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
        {
            gbrain->GetFusion(strdata,nSize);
        }
        void ListenTrafficSign(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname) //20230814,交通标志牌识别数据更新
        {
            gbrain->GetTrafficSign(strdata,nSize);
        }
}

}



iv::decition::BrainDecition::BrainDecition()
{
    givfault->SetFaultState(0,0,"Application Initializing.");


    obs_lidar_grid_ = NULL;
    old_obs_lidar_grid_ = NULL;
    obs_camera_ = NULL;
    obs_fusion_grid_ = NULL;
    eyes = new iv::perception::Eyes();

    decitionGps00  = new iv::decition::DecideGps00();
    decitionLine00 = new iv::decition::DecideLine00();

    iv::decition::gbrain = this;

    mpa              = iv::modulecomm::RegisterRecv("tracemap",iv::decition::ListenTraceMap);

    mpvbox           = iv::modulecomm::RegisterRecv("vbox",iv::decition::ListenVbox);

    mpaVechicleState = iv::modulecomm::RegisterSend(gstrmembrainstate.data(),10000,10);
    mpaCarstate      = iv::modulecomm::RegisterSend(gstrmembraincarstate.data(),10000,10);
    mpaBrainRunState = iv::modulecomm::RegisterSend("brainrunstate",10000,1);

    mpfusion         = iv::modulecomm::RegisterRecv("li_ra_fusion",ListenFusion);


    mpaToPlatform    = iv::modulecomm::RegisterSend("toplatform",10*sizeof(iv::platform::PlatFormMsg),10);
    mpaDecition      = iv::modulecomm::RegisterSend(gstrmemdecition.data(),10*sizeof(iv::decition::DecitionBasic),1);

    mpaHMI           = iv::modulecomm::RegisterRecv("hmi",iv::decition::ListenHMI);

    void * ppad      = iv::modulecomm::RegisterRecv("pad",iv::decition::ListenHMI);

    mpaPlatform      = iv::modulecomm::RegisterRecv("platformfrom",iv::decition::ListenPlatform);

    mpaGroup         = iv::modulecomm::RegisterRecv("p900_send",iv::decition::ListenGroup);

    mpamapreq        = iv::modulecomm::RegisterSend("mapreq",1000,1);

    mpv2r            = iv::modulecomm::RegisterRecv("v2r_send",iv::decition::ListenV2r); //20211009,cxw

    mpaTrafficSign   = iv::modulecomm::RegisterRecv("signarray",iv::decition::ListenTrafficSign); //20230814,交通标志牌识别数据更新


    ModuleFun funchassis =std::bind(&iv::decition::BrainDecition::UpdateChassis,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpaChassis = iv::modulecomm::RegisterRecvPlus(gstrmemchassis.data(),funchassis);


    ModuleFun fungroupgrpc =std::bind(&iv::decition::BrainDecition::UpdateGRPCGroupMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mapgrpcgroup = iv::modulecomm::RegisterRecvPlus("groupmsg",fungroupgrpc);

    mpaPlanTrace     = iv::modulecomm::RegisterSend("plantrace",100000,1);
    mpaObsTraceLeft  = iv::modulecomm::RegisterSend("obstraceleft",100000,1);
    mpaObsTraceRight = iv::modulecomm::RegisterSend("obstraceright",100000,1);


    mTime.start();
    mnOldTime = mTime.elapsed();

}

iv::decition::BrainDecition::~BrainDecition()
{

    iv::modulecomm::Unregister(mpaObsTraceRight);
    iv::modulecomm::Unregister(mpaObsTraceLeft);
    iv::modulecomm::Unregister(mpaPlanTrace);
    iv::modulecomm::Unregister(mpaChassis);
    iv::modulecomm::Unregister(mpamapreq);
    iv::modulecomm::Unregister(mpaGroup);
    iv::modulecomm::Unregister(mpaPlatform);
    iv::modulecomm::Unregister(mpaHMI);
    iv::modulecomm::Unregister(mpaDecition);
    iv::modulecomm::Unregister(mpaToPlatform);
    iv::modulecomm::Unregister(mpfusion);
    iv::modulecomm::Unregister(mpaCarstate);
    iv::modulecomm::Unregister(mpaVechicleState);
    iv::modulecomm::Unregister(mpvbox);
    iv::modulecomm::Unregister(mpa);
    iv::modulecomm::Unregister(mpaTrafficSign);
    iv::modulecomm::Unregister(mpv2r);

    delete eyes;
    std::cout<<"Brain Unialize."<<std::endl;
}


void iv::decition::BrainDecition::inialize() {
    eyes->inialize();
}


void iv::decition::BrainDecition::run() {

    givfault->SetFaultState(0,0,"Start Running.");
    double last = 0;

    iv::decition::Decition decition_gps = NULL;
    iv::decition::Decition decition_lidar = NULL;
    iv::decition::Decition decition_radar = NULL;
    iv::decition::Decition decition_camera = NULL;
    iv::decition::Decition decition_localmap = NULL;

    iv::decition::Decition dgtemp(new iv::decition::DecitionBasic);
    decition_gps = dgtemp;
    decition_gps->brake = 0;
    decition_gps->accelerator = 0;
    decition_gps->wheel_angle = 0;


    QString strpath = QCoreApplication::applicationDirPath();
    strpath = strpath + "/ADS_decision.xml";
    iv::xmlparam::Xmlparam xp(strpath.toStdString());

    ServiceCarStatus.msysparam.mstrvin = xp.GetParam("vin","10000000000000001");
    ServiceCarStatus.msysparam.mstrid = xp.GetParam("id","1");
    ServiceCarStatus.msysparam.mstriccid = xp.GetParam("iccid","11111111111111111111");
    ServiceCarStatus.msysparam.mvehtype= xp.GetParam("vehType","ge3");

    std::string strvehtype = ServiceCarStatus.msysparam.mvehtype;

    ServiceCarStatus.msysparam.vehWidth = atof(xp.GetParam("vehWidth","2.1").data());
    ServiceCarStatus.msysparam.vehLenth = atof(xp.GetParam("vehLenth","4.6").data());

    ServiceCarStatus.msysparam.mbSideDrive = xp.GetParam("SideDrive",false);
    ServiceCarStatus.msysparam.mfSideDis = xp.GetParam("SideDis",0.3);

    std::string str_zhuchetime = xp.GetParam("zhuchetime","16");
    ServiceCarStatus.msysparam.mzhuchetime =atoi(str_zhuchetime.data());

    std::string strepsoff = xp.GetParam("epsoff","0.0");
    ServiceCarStatus.mfEPSOff = atof(strepsoff.data());

    std::string strroadmode_vel;
    strroadmode_vel= xp.GetParam("roadmode0","10");
    ServiceCarStatus.mroadmode_vel.mfmode0 = atof(strroadmode_vel.data());

    strroadmode_vel= xp.GetParam("roadmode1","10");
    ServiceCarStatus.mroadmode_vel.mfmode1 = atof(strroadmode_vel.data());

    strroadmode_vel = xp.GetParam("roadmode5","10");
    ServiceCarStatus.mroadmode_vel.mfmode5 = atof(strroadmode_vel.data());

    strroadmode_vel = xp.GetParam("roadmode11","30");
    ServiceCarStatus.mroadmode_vel.mfmode11 = atof(strroadmode_vel.data());

    strroadmode_vel = xp.GetParam("roadmode13","5");
    ServiceCarStatus.mroadmode_vel.mfmode13 = atof(strroadmode_vel.data());

    strroadmode_vel = xp.GetParam("roadmode14","15");
    ServiceCarStatus.mroadmode_vel.mfmode14 = atof(strroadmode_vel.data());

    strroadmode_vel = xp.GetParam("roadmode15","15");
    ServiceCarStatus.mroadmode_vel.mfmode15 = atof(strroadmode_vel.data());

    strroadmode_vel = xp.GetParam("roadmode16","8");
    ServiceCarStatus.mroadmode_vel.mfmode16 = atof(strroadmode_vel.data());

    strroadmode_vel = xp.GetParam("roadmode17","8");
    ServiceCarStatus.mroadmode_vel.mfmode17 = atof(strroadmode_vel.data());

    strroadmode_vel = xp.GetParam("roadmode18","5");
    ServiceCarStatus.mroadmode_vel.mfmode18 = atof(strroadmode_vel.data());


    std::string group_cotrol = xp.GetParam("group","false");
    if(group_cotrol=="true"){
        ServiceCarStatus.group_control=true;
    }else{
        ServiceCarStatus.group_control=false;
    }

    std::string speed_control = xp.GetParam("speed","false");
    if(speed_control=="true"){
        ServiceCarStatus.speed_control=true;
    }else{
        ServiceCarStatus.speed_control=false;
    }

    std::string log_on= xp.GetParam("log","false");
    if(log_on=="true"){
        ServiceCarStatus.txt_log_on=true;
    }else{
        ServiceCarStatus.txt_log_on=false;
    }

    if(ServiceCarStatus.txt_log_on==true){
        QDateTime dt = QDateTime::currentDateTime();
        //将数据写入文件开始
        ofstream outfile;
        outfile.open("control_log.txt", ostream::ate);
        outfile.flush();
        outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()<<'\t'
              <<"vehState"<<'\t'<<"PathPoint"<<'\t'<<setprecision(4)<<"dSpeed"<<'\t'<<"obsDistance"<<'\t'<<"obsSpeed"<<'\t'<<"realspeed"<<'\t'<<"minDecelerate"<<'\t'
             <<"torque"<<'\t'<<"brake"<<'\t'<<"wheel_angle"<<'\t'<<"now_x"<<'\t'<<"now_y"<<'\t'<<"now_s"<<'\t'<<"now_d"<<endl;
        outfile.close();
        //将数据写入文件结束
    }

    std::string table_look_up_on= xp.GetParam("table","false");
    if(table_look_up_on=="true"){
        ServiceCarStatus.table_look_up_on=true;
    }else{
        ServiceCarStatus.table_look_up_on=false;
    }

    std::string strparklat = xp.GetParam("parklat","39.0624557");
    std::string strparklng = xp.GetParam("parklng","117.3575493");
    std::string strparkheading = xp.GetParam("parkheading","112.5");
    std::string strparktype = xp.GetParam("parktype","1");

    ServiceCarStatus.mfParkLat = atof(strparklat.data());
    ServiceCarStatus.mfParkLng = atof(strparklng.data());
    ServiceCarStatus.mfParkHeading = atof(strparkheading.data());
    ServiceCarStatus.mnParkType = atoi(strparktype.data());

    ServiceCarStatus.mLidarRotation = atof(xp.GetParam("LidarRotation","90.0").data());
    ServiceCarStatus.mLidarRangeUnit = atof(xp.GetParam("LidarRangeUnit","5.0").data());

//    std::string lightlon = xp.GetParam("lightlon","0");
//    std::string lightlat = xp.GetParam("lightlat","0");
//    ServiceCarStatus.iTrafficeLightLat=atof(lightlat.data());
//    ServiceCarStatus.iTrafficeLightLon=atof(lightlon.data());  //20230310,红绿灯信息从V2R中传来


    //mobileEye
    std::string   timeWidth =xp.GetParam("waitGpsTimeWidth","5000");
    std::string   garageLong =xp.GetParam("outGarageLong","10");
    ServiceCarStatus.waitGpsTimeWidth=atof(timeWidth.data());
    ServiceCarStatus.outGarageLong=atof(garageLong.data());
    //mobileEye end


    //lidar start
    std::string lidarGpsXiuzheng = xp.GetParam("lidarGpsXiuzheng","1.0");
    std::string radarGpsXiuzheng = xp.GetParam("radarGpsXiuzheng","3.0");
    std::string rearRadarGpsXiuzheng = xp.GetParam("rearRadarGpsXiuzheng","0.5");
    std::string rearLidarGpsXiuzheng = xp.GetParam("rearLidarGpsXiuzheng","1.0");
    std::string frontGpsXiuzheng = xp.GetParam("frontGpsXiuzheng","3.0");
    std::string rearGpsXiuzheng = xp.GetParam("rearGpsXiuzheng","1.0");
    std::string gpsOffset_x = xp.GetParam("gpsOffset_X","0");
    std::string gpsOffset_y = xp.GetParam("gpsOffset_Y","0");
    std::string strexternmpc = xp.GetParam("ExternMPC","false");  //If Use MPC set true
    std::string lidar_per_predict = xp.GetParam("lidarPP","false");  //If Use MPC set true

    std::string groupID = xp.GetParam("groupid","1");
    ServiceCarStatus.curID = atoi(groupID.data());

    adjuseGpsLidarPosition();

    if(strexternmpc == "true")
    {
        mbUseExternMPC = true;
    }

    if(lidar_per_predict == "true"){
        ServiceCarStatus.useLidarPerPredict = true;
    }

    ServiceCarStatus.msysparam.lidarGpsXiuzheng = atof(lidarGpsXiuzheng.data());
    ServiceCarStatus.msysparam.radarGpsXiuzheng = atof(radarGpsXiuzheng.data());
    ServiceCarStatus.msysparam.rearRadarGpsXiuzheng = atof(rearRadarGpsXiuzheng.data());
    ServiceCarStatus.msysparam.rearLidarGpsXiuzheng = atof(rearLidarGpsXiuzheng.data());
    ServiceCarStatus.msysparam.frontGpsXiuzheng = atof(frontGpsXiuzheng.data());
    ServiceCarStatus.msysparam.rearGpsXiuzheng = atof(rearGpsXiuzheng.data());
    ServiceCarStatus.msysparam.gpsOffset_x = atof(gpsOffset_x.data());
    ServiceCarStatus.msysparam.gpsOffset_y = atof(gpsOffset_y.data());
    // lidar end

    std::string strObsPredict = xp.GetParam("obsPredict","false");  //If Use MPC set true
    if(strObsPredict == "true")
    {
        ServiceCarStatus.useObsPredict = true;
    }
    //map

    std::string inRoadAvoid = xp.GetParam("inRoadAvoid","false");  //If Use MPC set true
    if(inRoadAvoid == "true")
    {
        ServiceCarStatus.inRoadAvoid = true;
    }else{
        ServiceCarStatus.inRoadAvoid = false;
    }

    std::string avoidObs = xp.GetParam("avoidObs","false");  //If Use MPC set true
    if(avoidObs == "true")
    {
        ServiceCarStatus.avoidObs = true;
    }else{
        ServiceCarStatus.avoidObs = false;
    }

    std::string socfDis = xp.GetParam("socfDis","15");
    std::string aocfDis = xp.GetParam("aocfDis","25");
    std::string aocfTimes = xp.GetParam("aocfTimes","3");


    ServiceCarStatus.socfDis = atof(socfDis.data());
    ServiceCarStatus.aocfDis = atof(aocfDis.data());
    ServiceCarStatus.aocfTimes = atof(aocfTimes.data());

    //20230814,jiaotong biaozhipaishibie

    std::string objectCon = xp.GetParam("objectCon","0.80");
    std::string objectWidth = xp.GetParam("objectWidth","29");
    ServiceCarStatus.objectCon = atof(objectCon.data());
    ServiceCarStatus.objectWidth = atof(objectWidth.data());


    std::string angdebug = xp.GetParam("angDebug","0.0");
    std::string tordebug = xp.GetParam("torqueDebug","0.0");
    std::string brkdebug = xp.GetParam("brakeDebug","3");



    ServiceCarStatus.ang_debug = atof(angdebug.data());
    ServiceCarStatus.torque_or_acc = atof(tordebug.data());
    ServiceCarStatus.brake = atof(brkdebug.data());

    std::string strbasepitch = xp.GetParam("basepitch","0.0");
    ServiceCarStatus.mfbasepitch = atof(strbasepitch.data());

    ServiceCarStatus.mbUseRampAssit = xp.GetParam("UseRamp",false);

    gplanbrakeacc = fabs(xp.GetParam("planbrakeacc",0.3));
    if(gplanbrakeacc <= 0.01)gplanbrakeacc = 0.01;

    gplanacc = fabs(xp.GetParam("planacc",1.0));
    if(gplanacc <0.1)gplanacc = 0.1;


    decitionGps00->mstopbrake = fabs(xp.GetParam("stopbrake",0.6));
    if(decitionGps00->mstopbrake < 0.1)decitionGps00->mstopbrake = 0.1;


//    bool mbUseDynamics = true;
//    bool mbLimitSpeed = true;
//    double mfMaxAcc = 3.0;
//    double mfChassisMaxBrake = 6.0; //Chassis Limit Max Brake
//    double mfPerceptionMax = 40.0; // Max 40 meters Obs
//    double mfMaxSpeed = 80.0; //Max Speed is calculate, mfMaxSpeed = sqrt(2.0 * mfChassisMaxBrake * (mfPerceptionMax - 5.0)) * 3.6;

    ServiceCarStatus.mbUseDynamics = xp.GetParam("UseDynamics",true);
    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
        ServiceCarStatus.mbLimitSpeed = xp.GetParam("LimitSpeed",true);
    else
        ServiceCarStatus.mbLimitSpeed = xp.GetParam("LimitSpeed",false);
    ServiceCarStatus.mfMaxAcc = xp.GetParam("MaxAcc",3.0);
    if(ServiceCarStatus.mfMaxAcc<0.0)ServiceCarStatus.mfMaxAcc = 0.1;
    ServiceCarStatus.mfChassisMaxBrake = xp.GetParam("ChassisMaxBrake",6.0);
    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
        ServiceCarStatus.mfChassisMaxBrake = xp.GetParam("ChassisMaxBrake",1.5);
    ServiceCarStatus.mfPerceptionMax = xp.GetParam("PerceptionMax",40);
    if(ServiceCarStatus.mfPerceptionMax <= 6.0)ServiceCarStatus.mfPerceptionMax = 6.0;
    if(ServiceCarStatus.mfChassisMaxBrake < 0.1)ServiceCarStatus.mfChassisMaxBrake = 0.1;
    ServiceCarStatus.mfMaxSpeed = sqrt(2.0 * ServiceCarStatus.mfChassisMaxBrake * (ServiceCarStatus.mfPerceptionMax - 5.0)) * 3.6;

    if((ServiceCarStatus.msysparam.mvehtype=="shenlan") &&(ServiceCarStatus.mbLimitSpeed))
    {
        ServiceCarStatus.mfMaxSpeed = 20.0;
        ServiceCarStatus.mfChassisMaxBrake = 1.5;
    }


    if(ServiceCarStatus.msysparam.mvehtype=="hunter")
    {
        ServiceCarStatus.mfMaxSpeed = 5.0;
    }

    //add by lyp, for setting distance to trafficlight stopline 
    std::string tlStopDis = xp.GetParam("tlStopDis","5");
    ServiceCarStatus.tlStopDis = atof(tlStopDis.data());
    //max tlStopDis is 10
    if(ServiceCarStatus.tlStopDis > 10)
        ServiceCarStatus.tlStopDis = 10;

    std::cout<<" Max Acc: "<<ServiceCarStatus.mfMaxAcc<<" PerceptionMax: "<<ServiceCarStatus.mfPerceptionMax
            <<" Chassis Max Brake: "<<ServiceCarStatus.mfChassisMaxBrake<<" Max Speed: "<<ServiceCarStatus.mfMaxSpeed<<std::endl;

    mstrmemmap_index = xp.GetParam("msg_mapindex","map_index");


    givfault->SetFaultState(0,0,"OK.");

    while (eyes->isAllSensorRunning() &&(!boost::this_thread::interruption_requested()))
    {
        if(navigation_data.size() <1)
        {
            iv::map::mapreq x;
            x.set_maptype(1);
            int nsize = x.ByteSize();
            char * str = new char[nsize];
            if(x.SerializeToArray(str,nsize))
            {
                iv::modulecomm::ModuleSendMsg(mpamapreq,str,nsize);
            }
            else
            {
                std::cout<<"iv::map::mapreq serialize error."<<std::endl;
            }
            delete str;
            if(ServiceCarStatus.txt_log_on==true)
            {
                givlog->debug("decition_brain","no plan");
            }
        }


        iv::GPSData gps_data_;			//gps 惯导数据

        std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> lidar_per(new std::vector<iv::Perception::PerceptionOutput>);

        bool brun =ServiceCarStatus.mbRunPause;
        if(ServiceCarStatus.mRSUupdateTimer.elapsed()>60*1000)
         {
            ServiceCarStatus.mbRunPause=false;//长时间收不到V2R信息认为没有V2R
         }

        if(ServiceCarStatus.mnBocheComplete > 0)
        {
            ServiceCarStatus.mbBrainCtring = false;
            ServiceCarStatus.mbRunPause = true;
            ServiceCarStatus.mnBocheComplete--;
        }
        if((ServiceCarStatus.mbRunPause)||(navigation_data.size()<1)||(ServiceCarStatus.mnBocheComplete>0))
        {
            ServiceCarStatus.mbBrainCtring = false;
//            decition_gps->brake = 6;
//            decition_gps->accelerator = -0.5;
//            decition_gps->wheel_angle = 0;
//            decition_gps->torque = 0;
//            decition_gps->mode = 0;
            if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
            {
                if(ServiceCarStatus.speed>0.1)
                {
                    decition_gps->brake = -2;  //当车辆进入自动驾驶以后，如果平台直接发送停止自动驾驶，保证车辆能够减速停止，需要测试下车辆能否泄压
                }
                else
                {
                    decition_gps->brake = -2;//保证在进入自动驾驶的时候车辆能够泄压,否则进入自动驾驶车辆无法加速
                }
                decition_gps->torque=0.0;
                decition_gps->acc_active = 0;//ACC扭矩请求激活（为1激活，为0不激活），不激活后不执行扭矩请求，对于深蓝没什么用，因为控制模块直接给的1
                decition_gps->brake_active = 0;//ACC减速激活（为1制动激活，为0制动不激活）
                decition_gps->brake_type = 0;//ADC请求类型（制动时为1，其余情况为0）
                decition_gps->angle_active = 0;//横向控制激活模式
                decition_gps->angle_mode = 0; //横向控制激活，和上一条同时满足才执行横向请求角度
                decition_gps->auto_mode = 0; //3为自动控制模式
               // ServiceCarStatus.ang_debug=ServiceCarStatus.ang_debug*15.9;//如果xml中配置的是车轮转角
                ServiceCarStatus.ang_debug=max((double)-430.0,ServiceCarStatus.ang_debug);
                ServiceCarStatus.ang_debug=min((double)430.0,ServiceCarStatus.ang_debug);
                decition_gps->wheel_angle = ServiceCarStatus.ang_debug;//0;


                double dSpeed=200;
                //20230814,交通标志识别添加 start
                if(ServiceCarStatus.mTrafficSignUpdateTimer.elapsed()>10*1000)
                 {
                    ServiceCarStatus.iTrafficsignSpeed=200;
                    ServiceCarStatus.itrafficsignTurn=200;
                 }

                if(ServiceCarStatus.itrafficsignTurn==2)
                {
                    decition_gps->leftlamp = true;
                    decition_gps->rightlamp = false;
                }
                else if(ServiceCarStatus.itrafficsignTurn==3)
                {
                    decition_gps->leftlamp = false;
                    decition_gps->rightlamp = true;
                }
                else
                {
                    decition_gps->leftlamp = false;
                    decition_gps->rightlamp = false;;
                }
                std::cout<<"  TrafficSignleft,2 is turnleft:"<<ServiceCarStatus.itrafficsignTurn<<std::endl;
                std::cout<<"  TrafficSignright,3 is turnright:"<<ServiceCarStatus.itrafficsignTurn<<std::endl;
                std::cout<<"  TrafficSigndSpeed:"<<ServiceCarStatus.iTrafficsignSpeed<<std::endl;
                dSpeed=min(dSpeed,ServiceCarStatus.iTrafficsignSpeed);
                //20230814,交通标志识别添加 end
                std::cout<<"===================v2x========================"<<std::endl;
                //print V2X honglvdeng xinxi
                std::cout<<"    iTrafficeLightTime:"<<ServiceCarStatus.iTrafficeLightTime
                         <<"    iTrafficeLightLat:"<<ServiceCarStatus.iTrafficeLightLat
                         <<"    iTrafficeLightLon:"<<ServiceCarStatus.iTrafficeLightLon
                         <<"    LightType:"<<ServiceCarStatus.iTrafficeLightColor
                         <<std::endl;
            }
            else
            {
                decition_gps->brake = 6;
                decition_gps->accelerator = -0.5;
                decition_gps->wheel_angle = 0;
                decition_gps->torque = 0;
                decition_gps->mode = 0;
            }
            ShareDecition(decition_gps);
            ServiceCarStatus.mfAcc = decition_gps->accelerator;
            ServiceCarStatus.mfWheel = decition_gps->wheel_angle;
            ServiceCarStatus.mfBrake = decition_gps->brake;

            iv::brain::brainstate xbs;
            xbs.set_mbbocheenable(ServiceCarStatus.bocheEnable);
            xbs.set_mbbrainrunning(false);
            xbs.set_mflidarobs(ServiceCarStatus.mLidarObs);
            xbs.set_mfradarobs(ServiceCarStatus.mRadarObs);
            xbs.set_mfobs(ServiceCarStatus.mObs);
            xbs.set_decision_period(decision_period);
            ShareBrainstate(xbs);

            usleep(10000);
            std::cout<<"enter mbRunPause"<<std::endl;
            continue;

        }

        givlog->debug("decition_brain"," ServiceCarStatus mbRunPause is : %d",ServiceCarStatus.mbRunPause);

        ServiceCarStatus.mbBrainCtring = true;
        obs_lidar_ = obs_radar_ = NULL;

        int j;

        if(obs_camera_ != NULL)free(obs_camera_);

        obs_camera_ = NULL;

        eyes->getSensorObstacle(obs_radar_, obs_camera_,gps_data_, obs_lidar_grid_);	//从传感器线程临界区交换数据




        ServiceLidar.copylidarperto(lidar_per);


        if(lidar_per->empty()){
            miss_lidar_per_count++;
            miss_lidar_per_count=min(miss_lidar_per_limit,miss_lidar_per_count);
            if(miss_lidar_per_count<miss_lidar_per_limit && old_lidar_per!=NULL){
                lidar_per=old_lidar_per;
            }
        }else{
            old_lidar_per=lidar_per;
            miss_lidar_per_count=0;
        }


        iv::ObsLiDAR obs_lidar_tmp(new std::vector<iv::ObstacleBasic>);
        iv::ObsRadar obs_radar_tmp(new std::vector<iv::ObstacleBasic>);
        iv::decition::Decition decition_final(new iv::decition::DecitionBasic);

        if (obs_lidar_) {	//激光雷达有障碍物
            //          std::cout<<"obs lidar size = "<<obs_lidar_->size()<<std::endl;
            //std::cout << "大脑处理激光雷达:" << obs_lidar_->at(0).nomal_x << " " << obs_lidar_->at(0).nomal_y << " " << obs_lidar_->at(0).nomal_z << std::endl;
            obs_lidar_tmp = NULL;
            obs_lidar_tmp.swap(obs_lidar_);
        }
        else
        {
            //           std::cout<<"no lidar obs"<<std::endl;
        }
        //ServiceCarStatus.obs_radar;
        //毫米波雷达障碍物信息

        if (obs_radar_) {
            //std::cout << "大脑处理毫米波雷达:" << obs_radar_->at(0).nomal_x << " " << obs_radar_->at(0).nomal_y << " " << obs_radar_->at(0).nomal_z << std::endl;
            obs_radar_tmp = NULL;
            obs_radar_tmp.swap(obs_radar_);
        }
        if (obs_camera_) {
            //std::cout << "大脑处理摄像头雷达:" << obs_camera_->at(0).nomal_x << " " << obs_camera_->at(0).nomal_y << " " << obs_camera_->at(0).nomal_z << std::endl;
        }


        //useMobileEye chuku
        if(ServiceCarStatus.m_bOutGarage){
            GPS_INS gps ;
            if(gps_data_){
                gps=*gps_data_;
            }
            decition_gps = decitionLine00->getDecideFromGPS(gps, navigation_data, obs_lidar_grid_,*lidar_per);  //chedaoxian

            std::cout << "使用mobileEye决策"<< std::endl;
        }
        //useMobileEye chuku end;

        if (gps_data_  &&!handPark && !ServiceCarStatus.m_bOutGarage){ //处理GPS数据

            double start = GetTickCount();

            if ( ServiceCarStatus.epsMode==0 && ServiceCarStatus.receiveEps) {
                ServiceCarStatus.mbRunPause=true;

            }
            ServiceCarStatus.receiveEps=false;

            lastMode=ServiceCarStatus.epsMode;
            lastPause=ServiceCarStatus.mbRunPause;

            if((fabs(mTime.elapsed() - mnOldTime)>40))
            {
                if((fabs(mTime.elapsed() - mnOldTime)<10000))
                {
                    ServiceCarStatus.mfCalAcc = (ServiceCarStatus.speed - mfSpeedLast)*1000/(fabs(mTime.elapsed() - mnOldTime))/3.6;
                    ServiceCarStatus.mfVehAcc = (ServiceCarStatus.vehSpeed_st - mfVehSpeedLast)*1000/(fabs(mTime.elapsed() - mnOldTime))/3.6;
                }else
                {
                    ServiceCarStatus.mfCalAcc = 0;
                    ServiceCarStatus.mfVehAcc = 0;
                }
                mnOldTime = mTime.elapsed();
                mfSpeedLast = ServiceCarStatus.speed;
                mfVehSpeedLast=ServiceCarStatus.vehSpeed_st;
            }

            iv::LidarGridPtr  templidar;
            templidar = NULL;
            mMutex_.lock();

            if(fusion_ptr_ != NULL)
            {
                if(old_obs_lidar_grid_ != NULL)
                {
                    free(old_obs_lidar_grid_);
                }
                old_obs_lidar_grid_ = fusion_ptr_;
                templidar = fusion_ptr_;

            }
            fusion_ptr_ = NULL;
            mMutex_.unlock();


            if(templidar == NULL)
                templidar = old_obs_lidar_grid_;


            mMutexMap.lock();
            decition_gps = decitionGps00->getDecideFromGPS(*gps_data_, navigation_data, templidar,*lidar_per,trafficLight);  // my dicition=============================

            mMutexMap.unlock();
            if(mbUseExternMPC)
            {
                fanya::GPS_INS xgpsins;
                xgpsins.speed_y = gps_data_->speed/3.6;
                xgpsins.gps_lat = gps_data_->gps_lat;
                xgpsins.gps_lng = gps_data_->gps_lng;
                xgpsins.ins_heading = gps_data_->ins_heading_angle;
                mmpcapi.SetGPS(xgpsins);
                mmpcapi.SetDesiredspeed(decition_gps->speed/3.6);
                double mpc_speed,mpc_decision,mpc_wheel;
                if(mmpcapi.GetDecision(mpc_speed,mpc_decision,mpc_wheel) == 0)
                {
                    decition_gps->wheel_angle = mpc_wheel;
                }
            }

            int ntpsize = sizeof(iv::TracePoint);
            iv::modulecomm::ModuleSendMsg(mpaPlanTrace,(char *)decitionGps00->planTrace.data(),decitionGps00->planTrace.size()*sizeof(iv::TracePoint));

            iv::modulecomm::ModuleSendMsg(mpaObsTraceLeft,(char *)(ServiceCarStatus.obsTraceLeft.data()),ServiceCarStatus.obsTraceLeft.size()*sizeof(iv::TracePoint));
            iv::modulecomm::ModuleSendMsg(mpaObsTraceRight,(char *)(ServiceCarStatus.obsTraceRight.data()),ServiceCarStatus.obsTraceRight.size()*sizeof(iv::TracePoint));


            decision_period = start - last;
            std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n周期时长:%f\n" << decision_period << std::endl;

            iv::brain::brainstate xbs;
            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(decision_period);
            xbs.set_mbtraficlightstart(ServiceCarStatus.mLightStartSensorBtn);
            //20211011,cxw,增加1x车辆行驶状态和目标限速，begin
//            xbs.set_vehicle_state(ServiceCarStatus.vehicle_state_1x);
//            xbs.set_vehicle_spd(ServiceCarStatus.target_spd_1x);
            //20211011,cxw,增加1x车辆行驶状态和目标限速，end
            ShareBrainstate(xbs);
            ShareDecition(decition_gps);

            iv::brain::brainrunstate xbr;
            xbr.set_decision_period(decision_period);
            ShareBrainRunState(xbr);

            iv::brain::carstate car_xbs;
            car_xbs.set_mstate(ServiceCarStatus.mvehState);
            car_xbs.set_mavoidx(ServiceCarStatus.mavoidX);
            car_xbs.set_mobsdistance(ServiceCarStatus.mObsDistance);
            car_xbs.set_mobsspeed(ServiceCarStatus.mObsSpeed);
            ShareBraincarstate(car_xbs);

            givlog->debug("acc is %f brake is %f wheel is %f",decition_gps->accelerator,decition_gps->brake,decition_gps->wheel_angle);
            givlog->debug("period id %f",decision_period);

            std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n决策加速度:%f\n" << decition_gps->accelerator << std::endl;
            //		ODS("\n决策刹车:%f\n", decition_gps->brake);
            std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n决策方向:%f\n" << decition_gps->wheel_angle << std::endl;
            std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n决策速度:%f\n" << decition_gps->speed << std::endl;

            last = start;
        }

        if (handPark && decition_gps != NULL)
        {
            decition_gps->brake = 20;
            decition_gps->accelerator = 0;
            ShareDecition(decition_gps);
            usleep(2000000);
            decition_gps->brake = 0;
            decition_gps->accelerator = 0;
            ShareDecition(decition_gps);

            usleep(handParkTime*1000);
            handPark = false;
        }
        else
        {
            usleep(1000);
        }



        ServiceCarStatus.mfAcc = decition_gps->accelerator;
        ServiceCarStatus.mfWheel = decition_gps->wheel_angle;
        ServiceCarStatus.mfBrake = decition_gps->brake;

        iv::platform::PlatFormMsg toplat;
        toplat.carState = ServiceCarStatus.carState;	// 0:停车	1:正常循迹	2:前往站点
        toplat.istostation = ServiceCarStatus.istostation;
        toplat.currentstation = ServiceCarStatus.currentstation;
        toplat.clientto = ServiceCarStatus.clientto;
        toplat.amilng = ServiceCarStatus.amilng;
        toplat.amilat = ServiceCarStatus.amilat;

        iv::modulecomm::ModuleSendMsg(mpaToPlatform,(char*)&toplat,sizeof(iv::platform::PlatFormMsg));

    }
    std::cout << "\n\n\n\n\n\n\n\n\n\n\nbrain线程退出\n" << std::endl;
}

bool iv::decition::BrainDecition::loadMapFromFile(std::string fileName) {
    std::ifstream fis(fileName);//获取文件
    std::string line;
    std::vector<std::string> des;

    if (fis.is_open() == false)
        return false;
    try {
        while (std::getline(fis, line)) {//开始一行一行的读数据
            boost::split(des, line, boost::is_any_of("\t"));
            if (des.size() != 10)
                throw "error";
            iv::GPSData data(new iv::GPS_INS);
            data->index = atoi(des[0].c_str());
            data->gps_lng = atof(des[1].c_str());
            data->gps_lat = atof(des[2].c_str());
            data->speed_mode = atoi(des[3].c_str());
            data->mode2 = atoi(des[4].c_str());
            data->ins_heading_angle = atof(des[5].c_str());
            data->runMode = atoi(des[6].c_str());
            data->roadMode = atoi(des[7].c_str());
            data->roadSum = atoi(des[8].c_str());
            data->roadOri = atoi(des[9].c_str());

            GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);

            if(data->roadMode==4){
                data->ins_heading_angle=data->ins_heading_angle+180;
                if(data->ins_heading_angle>=360)
                    data->ins_heading_angle=data->ins_heading_angle-360;
            }

            navigation_data.push_back(data);
        }

    }
    catch (...) {
        fis.close();
        return false;
    }

    fis.close();
    return true;
}


void iv::decition::BrainDecition::start() {
    thread_run = new boost::thread(boost::bind(&iv::decition::BrainDecition::run, this));
}

void iv::decition::BrainDecition::stop() {
    thread_run->interrupt();
    thread_run->join();
}


void iv::decition::BrainDecition::SideMove(iv::GPS_INS &x)
{
    if(ServiceCarStatus.msysparam.mbSideDrive == false)return;
    double fhdg = (90 - x.ins_heading_angle)*M_PI/180.0;
    double rel_x,rel_y;
    GaussProjCal(x.gps_lng,x.gps_lat,&rel_x,&rel_y);
    double fmove = 0;
    fmove = ServiceCarStatus.msysparam.mfSideDis + ServiceCarStatus.msysparam.vehWidth/2.0 - x.mfLaneWidth +x.mfDisToLaneLeft;
    if(fmove == 0.0)return;
    double rel_x1,rel_y1;
    rel_x1 = rel_x + fmove * cos(fhdg + M_PI/2.0);
    rel_y1 = rel_y + fmove * sin(fhdg + M_PI/2.0);
    GaussProjInvCal(rel_x1,rel_y1,&x.gps_lng,&x.gps_lat);
    x.gps_x = rel_x1;
    x.gps_y = rel_y1;
}

void iv::decition::BrainDecition::UpdateMap(const char *mapdata, const int mapdatasize)
{
    int gpsunitsize = sizeof(iv::GPS_INS);
    int nMapSize = mapdatasize/gpsunitsize;

    if(nMapSize < 1)return;

    bool bUpdate = false;
    if(nMapSize != navigation_data.size())
    {
        bUpdate = true;
    }
    else
    {
        iv::GPS_INS * p = (iv::GPS_INS *)mapdata;
        if((p->gps_lat == navigation_data.at(0)->gps_lat)&&(p->ins_heading_angle == navigation_data.at(0)->ins_heading_angle))
        {
            bUpdate = false;
        }
        else
        {
            bUpdate = true;
        }
    }

    if(bUpdate)
    {
        std::vector<fanya::MAP_DATA> xvectorMAP;
        int i;
        mMutexMap.lock();
        navigation_data.clear();
        for(i=0;i<nMapSize;i++)
        {
            iv::GPS_INS x;
            memcpy(&x,mapdata + i*gpsunitsize,gpsunitsize);
            iv::GPSData data(new iv::GPS_INS);
            if(ServiceCarStatus.msysparam.mbSideDrive)SideMove(x);
            *data = x;
            navigation_data.push_back(data);

            fanya::MAP_DATA xmapdata;
            xmapdata.gps_lat = x.gps_lat;
            xmapdata.gps_lng = x.gps_lng;
            xmapdata.ins_heading = x.ins_heading_angle;
            xvectorMAP.push_back(xmapdata);
        }

        mmpcapi.SetMAP(xvectorMAP);

        mMutexMap.unlock();

    }
    else
    {
        //       qDebug("not need update");
    }
}

void iv::decition::BrainDecition::ShareDecition(iv::decition::Decition decition)
{
    iv::brain::decition xdecition;
    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);
    xdecition.set_dippedlight(decition->nearLight);

    xdecition.set_acc_active(decition->acc_active);
    xdecition.set_brake_active(decition->brake_active);
    xdecition.set_brake_type(decition->brakeType);
    xdecition.set_angle_mode(decition->angle_mode);
    xdecition.set_angle_active(decition->angle_active);
    xdecition.set_auto_mode(decition->auto_mode);
    xdecition.set_leftlamp(decition->leftlamp);
    xdecition.set_rightlamp(decition->rightlamp);
    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
    {
        std::cout<<"===================shareDecition========================"<<std::endl;
        std::cout<<"  torque_st:"<<ServiceCarStatus.torque_st
                <<"  decideTorque:"<<decition->torque <<"  decideBrake:"<<decition->brake
                <<"  decideAngle:"<<decition->wheel_angle
                <<"  decideAngActive:"<<decition->angle_active
                <<"  decideAutoMode:"<<decition->auto_mode
               <<"  decideLeftTurn:"<<decition->leftlamp
               <<"  decideRightTurn:"<<decition->rightlamp
                <<std::endl;
        std::cout<<"========================================="<<std::endl;
    }
    else
    {

        std::cout<<"===================shareDecition========================"<<std::endl;
        std::cout<<"  torque_st:"<<ServiceCarStatus.torque_st
                <<"  decideTorque:"<<decition->torque <<"  decideBrake:"<<decition->brake
               <<"  decition mode:"<<decition->mode
              <<std::endl;
        std::cout<<"========================================="<<std::endl;
    }

    static qint64 oldtime;

    if((QDateTime::currentMSecsSinceEpoch() - oldtime)>100)
    {
        //        givlog->warn("brain interval is more than 100ms.  value is %ld",QDateTime::currentMSecsSinceEpoch() - oldtime);
    }

    //givlog->verbose("decition time is %ld",QDateTime::currentMSecsSinceEpoch());
    oldtime = QDateTime::currentMSecsSinceEpoch();

    int nsize = xdecition.ByteSize();
    char * str = new char[nsize];
    std::shared_ptr<char> pstr;
    pstr.reset(str);
    if(xdecition.SerializeToArray(str,nsize))
    {
        if(ServiceCarStatus.mbRunPause == false)
        {

            iv::modulecomm::ModuleSendMsg(mpaDecition,str,nsize);
//            else

        }
        else
        {
            if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
            {
                iv::modulecomm::ModuleSendMsg(mpaDecition,str,nsize);
            }
        }
    }
    else
    {
        std::cout<<"iv::decition::BrainDecition::ShareDecition serialize error."<<std::endl;
    }
}

void iv::decition::BrainDecition::ShareBrainRunState(iv::brain::brainrunstate &xbs)
{
    int nsize = xbs.ByteSize();
    char * str = new char[nsize];
    std::shared_ptr<char> pstr;
    pstr.reset(str);
    if(xbs.SerializeToArray(str,nsize))
    {
        iv::modulecomm::ModuleSendMsg(mpaBrainRunState,str,nsize);
    }
    else
    {
        std::cout<<"iv::decition::BrainDecition::ShareBrainstate serialize error."<<std::endl;
    }
}

void iv::decition::BrainDecition::ShareBrainstate(iv::brain::brainstate & xbs)
{

    int nsize = xbs.ByteSize();
    char * str = new char[nsize];
    std::shared_ptr<char> pstr;
    pstr.reset(str);
    if(xbs.SerializeToArray(str,nsize))
    {
        iv::modulecomm::ModuleSendMsg(mpaVechicleState,str,nsize);
    }
    else
    {
        std::cout<<"iv::decition::BrainDecition::ShareBrainstate serialize error."<<std::endl;
    }
}

void iv::decition::BrainDecition::ShareBraincarstate(iv::brain::carstate xbs)
{
    int nsize = xbs.ByteSize();
    char * str = new char[nsize];
    std::shared_ptr<char> pstr;
    pstr.reset(str);
    if(xbs.SerializeToArray(str,nsize))
    {
        iv::modulecomm::ModuleSendMsg(mpaCarstate,str,nsize);
    }
    else
    {
        std::cout<<"iv::decition::BrainDecition::ShareBrainstate serialize error."<<std::endl;
    }
}



void iv::decition::BrainDecition::UpdateHMI(const char *pdata, const int ndatasize)
{
    if(ndatasize < sizeof(iv::hmi::HMIBasic))return;

    iv::hmi::hmimsg xhmimsg;
    if(!xhmimsg.ParseFromArray(pdata,ndatasize))
    {
        givlog->error("iv::decition::BrainDecition::UpdateHMI parse error");
        return;
    }

    ServiceCarStatus.mbRunPause = xhmimsg.mbpause();

    if(xhmimsg.mbbochemode()){
        ServiceCarStatus.bocheMode = true;
    }
    ServiceCarStatus.busmode = xhmimsg.mbbusmode();

    if( ServiceCarStatus.bocheMode ){
        int a=0;
    }

    bool bRun;
    bRun = !ServiceCarStatus.mbRunPause;


    ServiceCarStatus.has_mbdoor = xhmimsg.has_mbchemen();
    if(ServiceCarStatus.has_mbdoor){
        ServiceCarStatus.mbdoor = xhmimsg.mbchemen();

    }

    ServiceCarStatus.has_mbjinguang = xhmimsg.has_mbjinguang();
    if(ServiceCarStatus.has_mbjinguang){
        ServiceCarStatus.mbjinguang = xhmimsg.mbjinguang();
    }

    givlog->debug("decition_brain_bool","mbdoor: %d,mbjinguang: %d",
                  ServiceCarStatus.mbdoor,ServiceCarStatus.mbjinguang);


}

void iv::decition::BrainDecition::UpdatePlatform(const char *pdata, const int ndatasize)
{
    if(ndatasize < sizeof(iv::platform::PlatFormMsg))return;

    iv::platform::PlatFormMsg x;
    memcpy(&x,pdata,sizeof(iv::platform::PlatFormMsg));
    ServiceCarStatus.carState = x.carState;	// 0:停车	1:正常循迹	2:前往站点
    std::cout<<"car state "<<ServiceCarStatus.carState<<std::endl;
    ServiceCarStatus.istostation = x.istostation;
    ServiceCarStatus.currentstation = x.currentstation;
    ServiceCarStatus.clientto = x.clientto;
    ServiceCarStatus.amilng = x.amilng;
    ServiceCarStatus.amilat = x.amilat;

}

void iv::decition::BrainDecition::UpdateGRPCGroupMsg(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
{
    iv::group::groupinfo xgroupinfo;
    if(!xgroupinfo.ParseFromArray(strdata,nSize))
    {
        std::cout<<"iv::decition::BrainDecition::UpdateGRPCGroupMsg parse fail."<<std::endl;
        return;
    }

    int nvehsize = xgroupinfo.mvehinfo_size();

    mMutexGroupgrpc.lock();
    mnGroupgrpcUpdateTime = QDateTime::currentMSecsSinceEpoch();
    mgroupgrpcInfo.CopyFrom(xgroupinfo);

    mMutexGroupgrpc.unlock();

    ServiceCarStatus.mMutexgroupgrpc.lock();
    ServiceCarStatus.mgroupgrpcupdatetime = QDateTime::currentMSecsSinceEpoch();
    ServiceCarStatus.mgroupgrpcinfo.CopyFrom(xgroupinfo);
    ServiceCarStatus.mMutexgroupgrpc.unlock();


}

void iv::decition::BrainDecition::UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
{
    iv::chassis xchassis;
    static int ncount = 0;
    if(!xchassis.ParseFromArray(strdata,nSize))
    {
        std::cout<<"iv::decition::BrainDecition::UpdateChassis ParseFrom Array Error."<<std::endl;
        return;
    }

    ServiceCarStatus.epb = xchassis.epbfault();
    ServiceCarStatus.grade = xchassis.shift();
    ServiceCarStatus.driverMode = xchassis.drivemode();
    if(xchassis.has_epsmode()){
        ServiceCarStatus.epsMode = xchassis.epsmode();
        ServiceCarStatus.receiveEps=true;
        if(ServiceCarStatus.epsMode == 0)
        {
            ServiceCarStatus.mbRunPause = true;
        }
    }
    if(xchassis.has_torque())
    {
        ServiceCarStatus.torque_st = xchassis.torque();
        if(ServiceCarStatus.msysparam.mvehtype=="bus"){
            ServiceCarStatus.torque_st = xchassis.torque()*0.4;
        }
        std::cout<<"******* ServiceCarStatus.torque_st:"<< ServiceCarStatus.torque_st<<std::endl;
        givlog->warn(" ServiceCarStatus.torque_st: is %f",ServiceCarStatus.torque_st);

    }

    if(xchassis.has_engine_speed())
    {
        ServiceCarStatus.engine_speed = xchassis.engine_speed();

        std::cout<<"******* xchassis.engine_speed:"<< xchassis.engine_speed()<<std::endl;


    }
}

void iv::decition::BrainDecition::GetFusion(const char *pdata, const int ndatasize)
{
    std::shared_ptr<iv::fusion::fusionobjectarray> xfusion (new iv::fusion::fusionobjectarray);

    if(ndatasize<1)return;
    if(false == xfusion->ParseFromArray(pdata,ndatasize))
    {
        std::cout<<" Listenesrfront fail."<<std::endl;
        return;
    }
    else{
        //std::cout<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
    }

    iv::decition::BrainDecition::UpdateFusion(xfusion);
}

void iv::decition::BrainDecition::UpdateFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs)
{

    mMutex_.lock();
    fusion_obs.swap(mfusion_obs_);
    if(fusion_ptr_ != NULL)
    {
        free(fusion_ptr_);
        fusion_ptr_ = NULL;
    }
    fusion_ptr_ = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
    memset(fusion_ptr_,0,sizeof(Obs_grid[iv::grx][iv::gry]));
    for(int i =0; i<iv::grx; i++)     //复制到指针数组
    {
        for(int j =0; j<iv::gry; j++)
        {
            fusion_ptr_[i*(iv::gry)+j].ob = 0;
        }

    }

    for(int i =0; i<mfusion_obs_->obj_size();i++)
    {
        iv::fusion::fusionobject xobs = mfusion_obs_->obj(i);
        for(int j =0; j<xobs.nomal_centroid().size(); j++)
        {
            int dx, dy;
            dx = (xobs.nomal_centroid(j).nomal_x() + gridwide*(double)centerx)/gridwide;
            dy = (xobs.nomal_centroid(j).nomal_y() + gridwide*(double)centery)/gridwide;

            if((dx>=0)&&(dx<iv::grx)&&(dy>=0)&&(dy<iv::gry))
            {
                fusion_ptr_[dx*(iv::gry) + dy].id = xobs.id();
                fusion_ptr_[dx*(iv::gry) + dy].type = xobs.type();
                fusion_ptr_[dx*(iv::gry) + dy].high = xobs.dimensions().z();
                fusion_ptr_[dx*(iv::gry) + dy].speed_x = xobs.vel_relative().x();//我们不用
                fusion_ptr_[dx*(iv::gry) + dy].speed_y = xobs.vel_relative().x();//别人的x是我们的y  速度是激光追踪计算出来的
                fusion_ptr_[dx*(iv::gry) + dy].yaw = xobs.yaw();
                fusion_ptr_[dx*(iv::gry) + dy].pointcount = 5;
                fusion_ptr_[dx*(iv::gry) + dy].ob = 2;
            }
        }

    }
    mMutex_.unlock();

}

//20230814,交通标志识别
void iv::decition::BrainDecition::GetTrafficSign(const char *pdata, const int ndatasize)
{
    std::shared_ptr<iv::vision::cameraobjectarray> xtrafficsign (new iv::vision::cameraobjectarray);

    if(ndatasize<1)return;

    if(false == xtrafficsign->ParseFromArray(pdata,ndatasize))
    {
        std::cout<<" Listen traffic sign fail."<<std::endl;
        return;
    }
    else{
        //std::cout<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
    }

    ServiceCarStatus.mTrafficSignUpdateTimer.start();
//    ServiceCarStatus.iTrafficsignSpeed=200.0;
//    ServiceCarStatus.itrafficsignTurn=200.0;

    iv::decition::BrainDecition::UpdateTrafficSign(xtrafficsign);
}

void iv::decition::BrainDecition::UpdateTrafficSign(std::shared_ptr<iv::vision::cameraobjectarray> xtrafficsign)
{
    mMutexVison_.lock();

    xtrafficsign.swap(mvision_obs_);
    int flag;
    int objectID=0;
    float objectCon=ServiceCarStatus.objectCon;  //W>30
    float objectW=ServiceCarStatus.objectWidth;  //W>0.85
    for(int i =0;i<mvision_obs_->obj_size();i++)//{"Speed10","Speed5","Left","Right","Nopassing","Park"}共六类
    {
        if(mvision_obs_->obj(i).con()>=objectCon)
        {
            if(mvision_obs_->obj(i).w()>=objectW)
            {
                objectID=i;
                //objectCon =mvision_obs_->obj(i).con();
                objectW = mvision_obs_->obj(i).w();
            }
        }
    }
    if(mvision_obs_->obj(objectID).type()=="Speed10")
        flag=0;
    else if(mvision_obs_->obj(objectID).type()=="Speed5")
        flag=1;
    else if(mvision_obs_->obj(objectID).type()=="Left")
        flag=2;
    else if(mvision_obs_->obj(objectID).type()=="Right")
        flag=3;
    else if(mvision_obs_->obj(objectID).type()=="Nopassing")
        flag=4;
    else if(mvision_obs_->obj(objectID).type()=="Park")
        flag=5;
    else
        flag =200;

    if(flag==0)
    {
        ServiceCarStatus.iTrafficsignSpeed=10.0;
        ServiceCarStatus.itrafficsignTurn=200;
    }
    else if(flag==1)
    {
        ServiceCarStatus.iTrafficsignSpeed=5.0;
        ServiceCarStatus.itrafficsignTurn=200;
    }
    else if(flag==4)
    {
        ServiceCarStatus.iTrafficsignSpeed=0.0;
        ServiceCarStatus.itrafficsignTurn=200;
    }
    else if(flag==5)
    {
        ServiceCarStatus.iTrafficsignSpeed=0.0;
        ServiceCarStatus.itrafficsignTurn=200;
    }
    else if(flag==2)
    {
        ServiceCarStatus.iTrafficsignSpeed=200.0;
        ServiceCarStatus.itrafficsignTurn=2;
    }
    else if(flag==3)
    {
        ServiceCarStatus.iTrafficsignSpeed=200.0;
        ServiceCarStatus.itrafficsignTurn=3;
    }
    else
    {
        ServiceCarStatus.iTrafficsignSpeed=200.0;
        ServiceCarStatus.itrafficsignTurn=200;
    }

//      if((flag==0)||(flag==1)||(flag==4)|(flag==5))
//      {
//          switch(flag)
//          {
//              case 0:
//                    ServiceCarStatus.iTrafficsignSpeed=min(10.0 , ServiceCarStatus.iTrafficsignSpeed);
//              case 1:
//                    ServiceCarStatus.iTrafficsignSpeed=min(5.0,ServiceCarStatus.iTrafficsignSpeed);
//              case 4:
//                    ServiceCarStatus.iTrafficsignSpeed=min(0.0,ServiceCarStatus.iTrafficsignSpeed);
//              case 5:
//                    ServiceCarStatus.iTrafficsignSpeed=min(0.0,ServiceCarStatus.iTrafficsignSpeed);
//              default:
//                    ServiceCarStatus.iTrafficsignSpeed=200.0;
//          }
//      }
//      else if((flag==2)||(flag==3))
//      {
//          if(flag==2)
//          {
//               ServiceCarStatus.itrafficsignTurn=2;
//          }
//           else if(flag==3)
//          {
//               ServiceCarStatus.itrafficsignTurn=3;
//          }
//      }

//    }
    //givlog->debug("decition_brain","flag is : %d",flag);
    //givlog->debug("decition_brain","itrafficsignTurn is : %d",ServiceCarStatus.itrafficsignTurn);
    //givlog->debug("decition_brain","iTrafficsignSpeed is : %f",ServiceCarStatus.iTrafficsignSpeed);

    mMutexVison_.unlock();
}

void iv::decition::BrainDecition::UpdateGroupInfo(const char *pdata, const int ndatasize){

    iv::radio::radio_send group_message;
    if(false == group_message.ParseFromArray(pdata,ndatasize))
    {
        std::cout<<"Listencansend Parse fail."<<std::endl;
        return;
    }
    ServiceCarStatus.group_server_status = group_message.server_status();
    ServiceCarStatus.group_comm_speed= (float)group_message.car_control_speed();
    ServiceCarStatus.group_state= group_message.cmd_mode();

    qDebug("updateGroupInfo %ld",QDateTime::currentMSecsSinceEpoch());
    if(ServiceCarStatus.group_state!=2){
        ServiceCarStatus.group_comm_speed=0;
    }
    qDebug("serverSt: %d, speed: %f, state: %d", ServiceCarStatus.group_server_status, ServiceCarStatus.group_comm_speed, ServiceCarStatus.group_state);


}

void iv::decition::BrainDecition::UpdateVbox(const char *pdata, const int ndatasize){

    iv::vbox::vbox group_message;
    if(false == group_message.ParseFromArray(pdata,ndatasize))
    {
        std::cout<<"Listencansend Parse fail."<<std::endl;
        return;
    }

    //  解析box信息
    //    ServiceCarStatus.group_server_status = group_message.server_status();
    //    ServiceCarStatus.group_comm_speed= (float)group_message.car_control_speed();
    //    ServiceCarStatus.group_state= group_message.cmd_mode();


    trafficLight.leftColor=group_message.st_left();
    trafficLight.rightColor=group_message.st_right();
    trafficLight.straightColor=group_message.st_straight();
    trafficLight.uturnColor=group_message.st_turn();
    trafficLight.leftTime=group_message.time_left();
    trafficLight.rightTime=group_message.time_right();
    trafficLight.straightTime=group_message.time_straight();
    trafficLight.uturnTime=group_message.time_turn();



}

//shenlan2023022320211009,cxw,解析V2R信息，begin
void iv::decition::BrainDecition::UpdateV2r(const char *pdata, const int ndatasize)
{
    iv::v2r::v2r_send group_message;
    if(false == group_message.ParseFromArray(pdata,ndatasize))
    {
        std::cout<<"ListenV2Rmessage Parse fail."<<std::endl;
        return;
    }
    ServiceCarStatus.mRSUupdateTimer.start();

    //由于变量类型改成optional了所以要先判断有没有此变量再赋值,cxw
    if(group_message.has_mbpause())
    {
       ServiceCarStatus.mbRunPause = group_message.mbpause();

//      std::cout<<"enter  for butn %d  "<<ServiceCarStatus.mbRunPause <<std::endl;
       givlog->debug("decition_brain","group_message mbRunPause is : %d",group_message.mbpause());
    }
    if(group_message.has_speedlimit())
    {
         ServiceCarStatus.ui_limit_spd = group_message.speedlimit();
//         givlog->debug("decition_brain","ui_limit_spd is : %f",ServiceCarStatus.ui_limit_spd);
    }

    if(group_message.has_radiobroadcastgpslat())
    {
        ServiceCarStatus.rsu_gps_lat = group_message.radiobroadcastgpslat();
//        givlog->debug("decition_brain","ui_limit_spd is : %f",ServiceCarStatus.ui_limit_spd);
    }
    if(group_message.has_radiobroadcastgpslon())
    {
        ServiceCarStatus.rsu_gps_lng= group_message.radiobroadcastgpslon();
//        givlog->debug("decition_brain","ui_limit_spd is : %f",ServiceCarStatus.ui_limit_spd);
    }
    if(group_message.has_radiolighttype()) //1:绿灯 2：红灯 3：黄灯
    {
        ServiceCarStatus.iTrafficeLightColor = group_message.radiolighttype();
    }
    if(group_message.has_radiolightremain())
    {
        ServiceCarStatus.iTrafficeLightTime = group_message.radiolightremain();
        if(group_message.has_trafficlightstoplat())
            ServiceCarStatus.iTrafficeLightLat = group_message.trafficlightstoplat();
        if(group_message.has_trafficlightstoplon())
            ServiceCarStatus.iTrafficeLightLon = group_message.trafficlightstoplon();
        ServiceCarStatus.milightCheckTimer.start();

    }
    if(group_message.has_radiobroadcastrange())
    {
        ServiceCarStatus.rsu_radiation_distance = group_message.radiobroadcastrange();
    }
    if(group_message.has_radiobroadcasttraffictype())
    {
        ServiceCarStatus.rsu_traffic_type = group_message.radiobroadcasttraffictype();

        ServiceCarStatus.mRSUTrafficUpdateTimer.start();
    }
    if(group_message.has_radiobroadcastspeedlimit())
    {
        ServiceCarStatus.rsu_trafficelimit_spd = group_message.radiobroadcastspeedlimit();
    }
    if(group_message.has_radiowarningtype())
    {
        ServiceCarStatus.rsu_warning_type = group_message.radiowarningtype();
        ServiceCarStatus.mRSUWarnUpdateTimer.start();
    }
    if(group_message.has_radiowarningspeedlimit())
    {
        ServiceCarStatus.rsu_warnlimit_spd = group_message.radiowarningspeedlimit();
    }



}

void iv::decition::BrainDecition::UpdateSate(){
    decitionGps00->isFirstRun=true;
}

void iv::decition::BrainDecition::adjuseGpsLidarPosition(){

    ServiceCarStatus.msysparam.lidarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
    ServiceCarStatus.msysparam.radarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
    ServiceCarStatus.msysparam.rearRadarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
    ServiceCarStatus.msysparam.rearLidarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
    ServiceCarStatus.msysparam.frontGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
    ServiceCarStatus.msysparam.rearGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
}