change src1/decision/decision_brain, add some modulecomm.

src/tool/adciv_record/recodfile.cpp

@@ -196,7 +196,7 @@ void RecodFile::run()
         }
         else
         {
-            msleep(1);
+            msleep(5);
         }
     }
 

src1/common/modulecomm/shm/procsm_if.cpp

@@ -102,7 +102,7 @@ void procsm_if_readthread::run()
             bAttach = mpPSM->AttachMem();
             if(bAttach == false)
             {
-                msleep(1);
+                msleep(100);
                 continue;
             }
             else

src1/decision/decision_brain/ivdecision_brain.cpp

@@ -1,7 +1,9 @@
 #include "ivdecision_brain.h"
 
+#include <QCoreApplication>
 #include "common/obs_predict.h"
 #include "ivlog.h"
+#include "xmlparam.h"
 
 extern iv::Ivlog * givlog;
 
@@ -26,6 +28,7 @@ namespace iv {
 ivdecision_brain::ivdecision_brain()
 {
     mvehState = VehState::normalRun;
+    loadxml();
 }
 
 
@@ -34,6 +37,23 @@ int ivdecision_brain::getdecision(brain::decition &xdecition,iv::brain::brainsta
     static qint64 nlastdecisiontime = 0;
     static qint64 nLastMapUpdate = 0;
     iv::GPSData now_gps_ins;
+
+    std::shared_ptr<iv::hmi::hmimsg> hmi_ptr;
+    if(GetHMImsg(hmi_ptr) == true)
+    {
+        ServiceCarStatus.mbRunPause = hmi_ptr->mbpause();
+        if(hmi_ptr->mbbochemode())
+        {
+            ServiceCarStatus.bocheMode = true;
+        }
+        ServiceCarStatus.busmode = hmi_ptr->mbbusmode();
+    }
+
+    if(ServiceCarStatus.mbRunPause)
+    {
+        return 0;
+    }
+
     if(GetGPS(now_gps_ins) == false)
     {
         return -1;   //No GPS Position
@@ -66,6 +86,7 @@ int ivdecision_brain::getdecision(brain::decition &xdecition,iv::brain::brainsta
     updatev2x();
     updateultra();
     updateradar();
+    updategroupinfo();
 
     iv::decition::Decition decition= getDecideFromGPS(*now_gps_ins,mgpsMapLine,lidarptr,xvectorper,xtrafficlight);
 
@@ -103,7 +124,227 @@ int ivdecision_brain::getdecision(brain::decition &xdecition,iv::brain::brainsta
     xbs.set_decision_period((QDateTime::currentMSecsSinceEpoch() - nlastdecisiontime));
     nlastdecisiontime = QDateTime::currentMSecsSinceEpoch();
 
-    return 0;
+    return 1;
+}
+
+void ivdecision_brain::updategroupinfo()
+{
+    std::shared_ptr<iv::radio::radio_send> groupmsg_ptr;
+
+    if(false == Getp900(groupmsg_ptr))
+    {
+        return;
+    }
+
+    ServiceCarStatus.group_server_status = groupmsg_ptr->server_status();
+    ServiceCarStatus.group_comm_speed= (float)groupmsg_ptr->car_control_speed();
+    ServiceCarStatus.group_state= groupmsg_ptr->cmd_mode();
+
+    if(ServiceCarStatus.group_state!=2){
+        ServiceCarStatus.group_comm_speed=0;
+    }
+}
+
+void ivdecision_brain::updatechassistocarstatus()
+{
+    std::shared_ptr<iv::chassis> xchassis_ptr;
+    if(false == Getchassis(xchassis_ptr))
+    {
+        return;
+    }
+
+    ServiceCarStatus.epb = xchassis_ptr->epbfault();
+    ServiceCarStatus.grade = xchassis_ptr->shift();
+    ServiceCarStatus.driverMode = xchassis_ptr->drivemode();
+    if(xchassis_ptr->has_epsmode()){
+        ServiceCarStatus.epsMode = xchassis_ptr->epsmode();
+        ServiceCarStatus.receiveEps=true;
+        if(ServiceCarStatus.epsMode == 0)
+        {
+            ServiceCarStatus.mbRunPause = true;
+        }
+    }
+    if(xchassis_ptr->has_torque())
+    {
+         ServiceCarStatus.torque_st = xchassis_ptr->torque();
+        if(ServiceCarStatus.msysparam.mvehtype=="bus"){
+            ServiceCarStatus.torque_st = xchassis_ptr->torque()*0.4;
+        }
+         std::cout<<"******* xchassis.torque:"<< xchassis_ptr->torque()<<std::endl;
+        std::cout<<"******* ServiceCarStatus.torque_st:"<< ServiceCarStatus.torque_st<<std::endl;
+        givlog->warn(" ServiceCarStatus.torque_st: is %f",ServiceCarStatus.torque_st);
+
+    }
+
+    if(xchassis_ptr->has_engine_speed())
+    {
+         ServiceCarStatus.engine_speed = xchassis_ptr->engine_speed();
+
+         std::cout<<"******* xchassis.engine_speed:"<< xchassis_ptr->engine_speed()<<std::endl;
+
+
+    }
+
+}
+
+void ivdecision_brain::loadxml()
+{
+    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","yuhesen");
+
+    ServiceCarStatus.msysparam.vehWidth = atof(xp.GetParam("vehWidth","2.1").data());
+    ServiceCarStatus.msysparam.vehLenth = atof(xp.GetParam("vehLenth","4.6").data());
+
+
+    /*=============     20200113 apollo_fu  add ===========*/
+    std::string str_zhuchetime = xp.GetParam("zhuchetime","16");
+    ServiceCarStatus.msysparam.mzhuchetime =atoi(str_zhuchetime.data());
+    /*============= end ================================== */
+
+    std::string strepsoff = xp.GetParam("epsoff","0.0");
+    ServiceCarStatus.mfEPSOff = atof(strepsoff.data());
+
+    std::string strroadmode_vel = xp.GetParam("roadmode0","10");
+    ServiceCarStatus.mroadmode_vel.mfmode0 = atof(strroadmode_vel.data());
+
+    strroadmode_vel = xp.GetParam("roadmode11","30");
+    ServiceCarStatus.mroadmode_vel.mfmode11 = atof(strroadmode_vel.data());
+
+
+    strroadmode_vel = xp.GetParam("roadmode14","15");
+    ServiceCarStatus.mroadmode_vel.mfmode14 = atof(strroadmode_vel.data());
+
+    //float a = ServiceCarStatus.mroadmode_vel.mfmode14;
+    //cout<<"........"<<a<<endl;
+
+    strroadmode_vel = xp.GetParam("roadmode15","15");
+    ServiceCarStatus.mroadmode_vel.mfmode15 = atof(strroadmode_vel.data());
+
+    /*==================== 20200113    apollo_fu add =================*/
+    strroadmode_vel = xp.GetParam("roadmode5","10");
+    ServiceCarStatus.mroadmode_vel.mfmode5 = atof(strroadmode_vel.data());
+
+    strroadmode_vel = xp.GetParam("roadmode13","5");
+    ServiceCarStatus.mroadmode_vel.mfmode13 = 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());
+
+
+
+    /*=========================    end   =============================*/
+
+    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 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());
+
+
+
+    //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
+
+
+    adjuseGpsLidarPosition();
+
+    if(strexternmpc == "true")
+    {
+//        mbUseExternMPC = 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;
+    }
+
+    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;
+    }
+
+    //map
+
+ //   mstrmemmap_index = xp.GetParam("msg_mapindex","map_index");
 }
 
 void ivdecision_brain::updatev2x()
@@ -168,6 +409,16 @@ void ivdecision_brain::updatev2x()
 
 }
 
+void ivdecision_brain::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;
+}
+
 void ivdecision_brain::updateultra()
 {
     std::shared_ptr<iv::ultrasonic::ultrasonic> xultra_ptr;

src1/decision/decision_brain/ivdecision_brain.h

@@ -66,6 +66,10 @@ private:
     void updatev2x();
     void updateultra();
     void updateradar();
+    void updategroupinfo();
+    void updatechassistocarstatus();
+    void loadxml();
+    void adjuseGpsLidarPosition();
 private:
     int mnRunMode = 0;
     bool mbisFirstRun = true;

src1/decision/interface/ivdecision.cpp

@@ -27,7 +27,17 @@ void ivdecision::modulerun()
     mpamobileyemsg = iv::modulecomm::RegisterRecvPlus(mstrmobileyemsgname.data(),funmobileye);
     ModuleFun funhmi = std::bind(&iv::ivdecision::UpdateHMI,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
     mpahmimsg = iv::modulecomm::RegisterRecvPlus(mstrhmimsgname.data(),funhmi);
-    mpapadmsg = iv::modulecomm::RegisterRecvPlus(mstrpadmsgname.data(),funhmi);
+    mpapadmsg = iv::modulecomm::RegisterRecvPlus(mstrpadmsgname.data(),funhmi);    
+    ModuleFun funp900 = std::bind(&iv::ivdecision::Updatep900,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpap900msg = iv::modulecomm::RegisterRecvPlus(mstrp900msgname.data(),funp900);
+    ModuleFun funv2x = std::bind(&iv::ivdecision::Updatev2x,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpav2xmsg = iv::modulecomm::RegisterRecvPlus(mstrv2xmsgname.data(),funv2x);
+    ModuleFun funultra = std::bind(&iv::ivdecision::Updateultrasonic,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpaultramsg = iv::modulecomm::RegisterRecvPlus(mstrultramsgname.data(),funultra);
+    ModuleFun funvbox = std::bind(&iv::ivdecision::UpdateVbox,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpavboxmsg = iv::modulecomm::RegisterRecvPlus(mstrvboxmsgname.data(),funvbox);
+    ModuleFun funchassis = std::bind(&iv::ivdecision::UpdatepChassis,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpachassismsg = iv::modulecomm::RegisterRecvPlus(mstrchassismsgname.data(),funchassis);
 
     mpaDecition = iv::modulecomm::RegisterSend(mstrdecisionmsgname.data(),10*sizeof(iv::decition::DecitionBasic),1);
     mpaVechicleState = iv::modulecomm::RegisterSend(mstrbrainstatemsgname.data(),10000,10);
@@ -40,6 +50,7 @@ void ivdecision::modulerun()
 
     while(mbrun)
     {
+
         if((QDateTime::currentMSecsSinceEpoch() - nLastDecTime)<nSpace)
         {
             std::this_thread::sleep_for(std::chrono::milliseconds(1));
@@ -47,8 +58,11 @@ void ivdecision::modulerun()
         }
         nLastDecTime = QDateTime::currentMSecsSinceEpoch();
         std::cout<<"running."<<std::endl;
-        getdecision(xdecition,xbs);
-        sharemsg(xdecition,xbs);
+        int nrtn = getdecision(xdecition,xbs);
+        if(nrtn == 1)
+        {
+            sharemsg(xdecition,xbs);
+        }
         //RunDecision
         //ShareMsg
     }
@@ -517,4 +531,73 @@ bool ivdecision::Getultrasonic(std::shared_ptr<ultrasonic::ultrasonic> &xultrams
     return true;
 }
 
+void ivdecision::Updatep900(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void )&index;
+    (void )&dt;
+    (void )strmemname;
+
+    std::shared_ptr<iv::radio::radio_send> xp900_ptr
+            = std::shared_ptr<iv::radio::radio_send>(new iv::radio::radio_send);
+
+    if(!xp900_ptr->ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ivdecision::Updatep900 Parse Error."<<std::endl;
+        return;
+    }
+
+    mMutexp900.lock();
+    mp900msg = xp900_ptr;
+    mnp900UpdateTime = QDateTime::currentMSecsSinceEpoch();
+    mMutexp900.unlock();
+}
+
+bool ivdecision::Getp900(std::shared_ptr<iv::radio::radio_send> & xp900msg)
+{
+    if((QDateTime::currentMSecsSinceEpoch() - mnp900UpdateTime)>mnNewThresh)
+    {
+        xp900msg = NULL;
+        return false;
+    }
+    mMutexp900.lock();
+    xp900msg = mp900msg;
+    mMutexp900.unlock();
+    return true;
+}
+
+void ivdecision::UpdatepChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void )&index;
+    (void )&dt;
+    (void )strmemname;
+
+    std::shared_ptr<iv::chassis> xchassis_ptr
+            = std::shared_ptr<iv::chassis>(new iv::chassis);
+
+
+    if(!xchassis_ptr->ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ivdecision::UpdatepChassis Parse Error."<<std::endl;
+        return;
+    }
+
+    mMutexchassis.lock();
+    mchassismsg = xchassis_ptr;
+    mnchassisUpdateTime = QDateTime::currentMSecsSinceEpoch();
+    mMutexchassis.unlock();
+}
+
+bool ivdecision::Getchassis(std::shared_ptr<iv::chassis> & xchassismsg)
+{
+    if((QDateTime::currentMSecsSinceEpoch() - mnchassisUpdateTime)>mnNewThresh)
+    {
+        xchassismsg = NULL;
+        return false;
+    }
+    mMutexchassis.lock();
+    xchassismsg = mchassismsg;
+    mMutexchassis.unlock();
+    return true;
+}
+
 }

src1/decision/interface/ivdecision.h

@@ -16,6 +16,8 @@
 #include "v2x.pb.h"
 #include "ultrasonic.pb.h"
 #include "brainstate.pb.h"
+#include "radio_send.pb.h"
+#include "chassis.pb.h"
 
 #include <QMutex>
 
@@ -44,6 +46,11 @@ private:
     std::string mstrmobileyemsgname = "mobileye";
     std::string mstrhmimsgname = "hmi";
     std::string mstrpadmsgname = "pad";
+    std::string mstrp900msgname = "p900_send";
+    std::string mstrv2xmsgname = "v2x";
+    std::string mstrultramsgname = "ultra";
+    std::string mstrvboxmsgname = "vbox";
+    std::string mstrchassismsgname = "chassis";
 
     std::string mstrdecisionmsgname = "deciton";
     std::string mstrbrainstatemsgname = "brainstate";
@@ -56,6 +63,11 @@ private:
     void * mpamobileyemsg;
     void * mpahmimsg;
     void * mpapadmsg;
+    void * mpap900msg;
+    void * mpav2xmsg;
+    void * mpaultramsg;
+    void * mpavboxmsg;
+    void * mpachassismsg;
 
     void * mpaDecition;
     void * mpaVechicleState;
@@ -70,6 +82,9 @@ private:
     void UpdateVbox(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
     void Updatev2x(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
     void Updateultrasonic(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void Updatep900(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void UpdatepChassis(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
 
 private:
 
@@ -110,6 +125,14 @@ private:
     qint64 mnultrasonicUpdateTime = 0;
     QMutex mMutexultrasonic;
 
+    std::shared_ptr<iv::radio::radio_send> mp900msg;
+    qint64 mnp900UpdateTime = 0;
+    QMutex mMutexp900;
+
+    std::shared_ptr<iv::chassis> mchassismsg;
+    qint64 mnchassisUpdateTime = 0;
+    QMutex mMutexchassis;
+
 private:
     const qint64 mnNewThresh = 1000; //1 seconds
 
@@ -126,6 +149,8 @@ public:
     bool Getvboxmsg(std::shared_ptr<iv::vbox::vbox> & xvboxmsg);
     bool Getv2xmsg(std::shared_ptr<iv::v2x::v2x> & xv2xmsg);
     bool Getultrasonic(std::shared_ptr<iv::ultrasonic::ultrasonic> & xultramsg);
+    bool Getp900(std::shared_ptr<iv::radio::radio_send> & xp900msg);
+    bool Getchassis(std::shared_ptr<iv::chassis> & xchassismsg);
 
 //    void GetLidarPtr();
 //    void GetRadar();