change src1/ivdecison. add some message callback.

src1/decision/interface/ivdecision.cpp

@@ -12,16 +12,36 @@ ivdecision::ivdecision()
 
 void ivdecision::modulerun()
 {
-
+    ModuleFun fungps = std::bind(&iv::ivdecision::UpdateGPS,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpagpsmsg = iv::modulecomm::RegisterRecvPlus(mstrgpsmsgname.data(),fungps);
+    ModuleFun funlidar = std::bind(&iv::ivdecision::UpdateLIDAR,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpalidarmsg = iv::modulecomm::RegisterRecvPlus(mstrlidarmsgname.data(),funlidar);
+    ModuleFun funradar = std::bind(&iv::ivdecision::UpdateRADAR,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mparadarmsg = iv::modulecomm::RegisterRecvPlus(mstrradarmsgname.data(),funradar);
+    ModuleFun funmap = std::bind(&iv::ivdecision::UpdateMAP,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpamapmsg = iv::modulecomm::RegisterRecvPlus(mstrmapmsgname.data(),funmap);
+    ModuleFun funmobileye = std::bind(&iv::ivdecision::UpdateMobileye,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    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);
 
     while(mbrun)
     {
         //RunDecision
         //ShareMsg
     }
+
+    iv::modulecomm::Unregister(mpapadmsg);
+    iv::modulecomm::Unregister(mpahmimsg);
+    iv::modulecomm::Unregister(mpamobileyemsg);
+    iv::modulecomm::Unregister(mpamapmsg);
+    iv::modulecomm::Unregister(mparadarmsg);
+    iv::modulecomm::Unregister(mpalidarmsg);
+    iv::modulecomm::Unregister(mpagpsmsg);
 }
 
-void ivdecision::UpdateGPSMsg(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+void ivdecision::UpdateGPS(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
 {
     (void )&index;
     (void )&dt;
@@ -83,7 +103,7 @@ bool ivdecision::GetGPS(GPSData &xGPSData)
     return true;
 }
 
-void ivdecision::UpdateRADARMsg(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+void ivdecision::UpdateRADAR(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
 {
     (void )&index;
     (void )&dt;
@@ -116,6 +136,232 @@ bool ivdecision::GetRADAR(std::shared_ptr<radar::radarobjectarray> &xRADAR)
     return true;
 }
 
+void ivdecision::UpdateLIDAR(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void )&index;
+    (void )&dt;
+    (void )strmemname;
+
+    if(nSize<1)return;
+
+    unsigned int nCount = nSize/sizeof(iv::ObstacleBasic);
+    std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs = std::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
+    lidar_obs->resize(nCount);
+    memcpy(lidar_obs->data(),strdata,nCount*sizeof(iv::ObstacleBasic));
+    mMutexLIDAR.lock();
+    mlidar_obs = lidar_obs;
+    mnLIDARUpdateTime = QDateTime::currentMSecsSinceEpoch();
+    mMutexLIDAR.unlock();
+}
+
+bool ivdecision::GetLIDARGrid(LidarGridPtr &lidargridptr)
+{
+    if((QDateTime::currentMSecsSinceEpoch() - mnLIDARUpdateTime)>mnNewThresh)
+    {
+        lidargridptr = NULL;
+        return false;
+    }
+
+    LidarGridPtr ptr = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
+    memset(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++)
+        {
+//            ptr[i * (iv::gry) + j].high = obs_grid[i][j].high;
+//            ptr[i * (iv::gry) + j].low = obs_grid[i][j].low;
+            ptr[i * (iv::gry) + j].ob = 0;
+//            ptr[i * (iv::gry) + j].obshight = obs_grid[i][j].obshight;
+//            ptr[i * (iv::gry) + j].pointcount = obs_grid[i][j].pointcount;
+        }
+    }
+
+    mMutexLIDAR.lock();
+
+    for(unsigned int i=0;i<mlidar_obs->size();i++)
+    {
+        iv::ObstacleBasic xobs = mlidar_obs->at(i);
+        int dx,dy;
+        dx = (xobs.nomal_x + gridwide * (double)centerx)/gridwide;
+        dy = (xobs.nomal_y + gridwide * (double)centery)/gridwide;
+        if((dx>=0)&&(dx<iv::grx)&&(dy>=0)&&(dy<iv::gry))
+        {
+            ptr[dx*(iv::gry) +dy].high = xobs.high;
+            ptr[dx*(iv::gry) +dy].low = xobs.low;
+            ptr[dx*(iv::gry) + dy].ob = 2;
+            ptr[dx*(iv::gry) + dy].obshight = xobs.nomal_z;
+            ptr[dx*(iv::gry) + dy].pointcount = 5;
+        }
+
+    }
+    lidargridptr = ptr;
+
+
+    mMutexLIDAR.unlock();
+
+    return true;
+}
 
+bool ivdecision::GetLIDARObs(std::shared_ptr<std::vector<ObstacleBasic> > &xlidar_obs)
+{
+    if((QDateTime::currentMSecsSinceEpoch() - mnLIDARUpdateTime)>mnNewThresh)
+    {
+        xlidar_obs = NULL;
+        return false;
+    }
+
+    mMutexLIDAR.lock();
+    xlidar_obs = mlidar_obs;
+    mMutexLIDAR.unlock();
+    return true;
+}
+
+void ivdecision::UpdateMAP(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void )&index;
+    (void )&dt;
+    (void )strmemname;
+
+    if(nSize<1)return;
+
+    //    std::cout<<"update map "<<std::endl;
+    int gpsunitsize = sizeof(iv::GPS_INS);
+    int nMapSize = nSize/gpsunitsize;
+    //    std::cout<<"map size is "<<nMapSize<<std::endl;
+
+    if(nMapSize < 1)return;
+
+    bool bUpdate = true;
+//    if(nMapSize != mnavigation_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))
+//        {
+//            //           qDebug("same map");
+//            bUpdate = false;
+//        }
+//        else
+//        {
+//            bUpdate = true;
+//        }
+//    }
+
+    if(bUpdate)
+    {
+
+        mMutexMAP.lock();
+        mnavigation_data.clear();
+        mnavigation_data.resize(nMapSize);
+        memcpy(mnavigation_data.data(),strdata,nMapSize * sizeof(iv::GPS_INS));
+        mnMAPUpdateTime = QDateTime::currentMSecsSinceEpoch();
+        mMutexMAP.unlock();
+
+    }
+    else
+    {
+
+    }
+}
+
+bool ivdecision::IsMAPUpdate(qint64 &nLastUpdateTime)
+{
+    if(mnavigation_data.size()<1)
+    {
+        return false;
+    }
+    if(nLastUpdateTime != mnMAPUpdateTime)
+    {
+        return true;
+    }
+    return false;
+}
+
+bool ivdecision::GetMAP(std::vector<GPSData> &navigation_data, qint64 &nLastUpdateTime)
+{
+    if(mnavigation_data.size()<1)
+    {
+        return false;
+    }
+    mMutexMAP.unlock();
+    nLastUpdateTime = mnMAPUpdateTime;
+    navigation_data = mnavigation_data;
+    mMutexMAP.unlock();
+    return true;
+}
+
+void ivdecision::UpdateMobileye(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void )&index;
+    (void )&dt;
+    (void )strmemname;
+
+    if(nSize<1)return;
+
+    std::shared_ptr<iv::mobileye::mobileye> xmobileye = std::shared_ptr<iv::mobileye::mobileye>(new iv::mobileye::mobileye);
+
+    if(!xmobileye->ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ivdecision::UpdateMobileye Parse Error."<<std::endl;
+        return;
+    }
+
+    mMutexmobileye.lock();
+    mmobileye = xmobileye;
+    mnmobileyeUpdateTime = QDateTime::currentMSecsSinceEpoch();
+    mMutexmobileye.unlock();
+}
+
+bool ivdecision::GetMobileye(std::shared_ptr<mobileye::mobileye> &xmobileye)
+{
+    if((QDateTime::currentMSecsSinceEpoch() - mnmobileyeUpdateTime)>mnNewThresh)
+    {
+        xmobileye = NULL;
+        return false;
+    }
+
+    mMutexmobileye.lock();
+    xmobileye = mmobileye;
+    mMutexmobileye.unlock();
+    return true;
+}
+
+void ivdecision::UpdateHMI(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void )&index;
+    (void )&dt;
+    (void )strmemname;
+
+    if(nSize<1)return;
+
+    std::shared_ptr<iv::hmi::hmimsg> xhmiptr = std::shared_ptr<iv::hmi::hmimsg>(new iv::hmi::hmimsg);
+
+    if(!xhmiptr->ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ivdecision::UpdateHMI Parse Error."<<std::endl;
+        return;
+    }
+
+    mMutexHMI.lock();
+    mHMImsg = xhmiptr;
+    mnHMIUpdateTime = QDateTime::currentMSecsSinceEpoch();
+    mMutexHMI.unlock();
+}
+
+bool ivdecision::GetHMImsg(std::shared_ptr<hmi::hmimsg> &xhmimsg)
+{
+    if((QDateTime::currentMSecsSinceEpoch() - mnHMIUpdateTime)>mnNewThresh)
+    {
+        xhmimsg = NULL;
+        return false;
+    }
+    mMutexHMI.lock();
+    mHMImsg = xhmimsg;
+    mMutexHMI.unlock();
+    return true;
+}
 
 }

src1/decision/interface/ivdecision.h

@@ -6,10 +6,11 @@
 #include "modulecomm.h"
 
 #include "gps_type.h"
-
+#include "obstacle_type.h"
 #include "gpsimu.pb.h"
-
 #include "radarobjectarray.pb.h"
+#include "mobileye.pb.h"
+#include "hmi.pb.h"
 
 #include <QMutex>
 
@@ -31,11 +32,13 @@ public:
     virtual void sharemsg() = 0;
 
 private:
-    std::string strgpsmsgname = "hcp2_gpsimu";
-    std::string strlidarmsgname = "lidar_obs";
-    std::string strradarmsgname = "radar";
-    std::string strmapmsgname = "tracemap";
-    std::string strmobileyemsgname = "mobileye";
+    std::string mstrgpsmsgname = "hcp2_gpsimu";
+    std::string mstrlidarmsgname = "lidar_obs";
+    std::string mstrradarmsgname = "radar";
+    std::string mstrmapmsgname = "tracemap";
+    std::string mstrmobileyemsgname = "mobileye";
+    std::string mstrhmimsgname = "hmi";
+    std::string mstrpadmsgname = "pad";
 
 private:
     void * mpagpsmsg;
@@ -43,10 +46,17 @@ private:
     void * mparadarmsg;
     void * mpamapmsg;
     void * mpamobileyemsg;
+    void * mpahmimsg;
+    void * mpapadmsg;
 
 private:
-    void UpdateGPSMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-    void UpdateRADARMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void UpdateGPS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void UpdateRADAR(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void UpdateLIDAR(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void UpdateMAP(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void UpdateMobileye(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void UpdateHMI(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
 
 private:
 
@@ -59,7 +69,21 @@ private:
     qint64 mnRADARUpdateTime = 0;
     QMutex mMutexRADAR;
 
+    std::shared_ptr<std::vector<iv::ObstacleBasic> > mlidar_obs;
+    qint64 mnLIDARUpdateTime = 0;
+    QMutex mMutexLIDAR;
 
+    std::vector<iv::GPSData> mnavigation_data;	//导航数据，GPS结构体数组
+    qint64 mnMAPUpdateTime = 0;
+    QMutex mMutexMAP;
+
+    std::shared_ptr<iv::mobileye::mobileye> mmobileye;
+    qint64 mnmobileyeUpdateTime = 0;
+    QMutex mMutexmobileye;
+
+    std::shared_ptr<iv::hmi::hmimsg> mHMImsg;
+    qint64 mnHMIUpdateTime = 0;
+    QMutex mMutexHMI;
 
 private:
     const qint64 mnNewThresh = 1000; //3 seconds
@@ -68,6 +92,13 @@ private:
 public:
     bool GetGPS(iv::GPSData & xGPSData);
     bool GetRADAR(std::shared_ptr<iv::radar::radarobjectarray> & xRADAR);
+    bool GetLIDARGrid(iv::LidarGridPtr & lidargridptr);
+    bool GetLIDARObs(std::shared_ptr<std::vector<iv::ObstacleBasic> > & xlidar_obs);
+    bool IsMAPUpdate(qint64 & nLastUpdateTime);
+    bool GetMAP(std::vector<iv::GPSData> & navigation_data,qint64 & nLastUpdateTime);
+    bool GetMobileye(std::shared_ptr<iv::mobileye::mobileye> & xmobileye);
+    bool GetHMImsg(std::shared_ptr<iv::hmi::hmimsg> & xhmimsg);
+
 //    void GetLidarPtr();
 //    void GetRadar();
 //    void GetMap();

src1/types/obstacle_type.h

@@ -3,7 +3,7 @@
 #define _IV_COMMON_OBSTACLE_TYPE_
 #include <vector>
 #ifndef Android
-#include <common/boost.h>
+#include <boost.h>
 #endif
 /**
 *障碍物类型