change vtd_pilot_if, add vtd object to fusion.

src/driver/vtd_pilot_if/main.cpp

@@ -6,7 +6,7 @@ int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
-    vtd_pilot * pvp = new vtd_pilot("1","2");
+    vtd_pilot * pvp = new vtd_pilot("rdbcommon","2");
     (void)pvp;
 
     return a.exec();

src/driver/vtd_pilot_if/vtd_pilot.cpp

@@ -16,8 +16,10 @@ static void CalcXY(const double lat0,const double lon0,
 vtd_pilot::vtd_pilot(std::string strfromvtd,std::string strtovtd)
 {
     (void)strfromvtd;
-//    mpfromvtd = iv::modulecomm::RegisterRecv(strfromvtd.data(),);
-    mptovtd = iv::modulecomm::RegisterSend(strtovtd.data(),10000,0);
+    ModuleFun funvtd =std::bind(&vtd_pilot::UpdateVTD,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpfromvtd = iv::modulecomm::RegisterRecvPlus(strfromvtd.data(),funvtd);
+
+    mptovtd = iv::modulecomm::RegisterSend(strtovtd.data(),10000,1) ;
 
     ModuleFun fungpsimu =std::bind(&vtd_pilot::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
     mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);
@@ -46,6 +48,123 @@ void vtd_pilot::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const
     mgpsimu.CopyFrom(xgpsimu);
     mbupdate_gps = true;
     mmutex.unlock();
+
+    mcv.notify_all();
+
+
+}
+
+void vtd_pilot::ConvertToObjectArray(std::vector<iv::vtd::rdbdata> &xvectorrdbdata, iv::fusion::fusionobjectarray &xfusionarray)
+{
+    int i;
+    int j;
+    for(i=0;i<(int)xvectorrdbdata.size();i++)
+    {
+        iv::vtd::rdbdata * prdbdata = &xvectorrdbdata[i];
+        for(j=0;j<prdbdata->mrdbitem_size();j++)
+        {
+            iv::vtd::rdbitem * pitem = prdbdata->mutable_mrdbitem(j);
+            if(pitem->pkgid() == RDB_PKG_ID_OBJECT_STATE)
+            {
+                RDB_OBJECT_STATE_t xobj;
+    //            std::cout<<" structure RDB_OBJECT_STATE_t size: "<<sizeof(xobj)<<"data size: "<<xrdbitem.mnpkgdatasize<<std::endl;
+                memcpy(&xobj,pitem->pkgdata().data(),sizeof(RDB_OBJECT_STATE_t));
+                std::cout<<"id:"<<xobj.base.id<<" name:"<<xobj.base.name<< " pos : x "<<xobj.base.pos.x<<" y "<<xobj.base.pos.y<<" z "<<xobj.base.pos.z<<std::endl;
+                iv::fusion::fusionobject * pobject = NULL;
+                int k;
+                for(k=0;k<xfusionarray.obj_size();k++)
+                {
+                    if(xfusionarray.mutable_obj(k)->id() == xobj.base.id)
+                    {
+                        pobject = xfusionarray.mutable_obj(k);
+                        break;
+                    }
+                }
+                if(pobject  == NULL)
+                {
+                    pobject = xfusionarray.add_obj();
+                }
+                pobject->set_id(xobj.base.id);
+                double x,y;
+                x = xobj.base.pos.x - mfX;
+                y = xobj.base.pos.y - mfY;
+                double relx,rely;
+                double beta  = mfHeading*(-1.0);
+                relx = x*cos(beta) - y*sin(beta);
+                rely = x*sin(beta) + y*sin(beta);
+                double vx,vy;
+                vx = xobj.ext.speed.x;
+                vy = xobj.ext.speed.y;
+                vx = vx - mfvx;
+                vy = vy - mfvy;
+                double relvx,relvy;
+                relvx = vx*cos(beta) - vy*sin(beta);
+                relvy = vx*sin(beta) + vy*sin(beta);
+                double fobjheading = xobj.base.pos.h;
+                fobjheading = fobjheading - mfHeading;
+                pobject->set_lifetime(100);
+                pobject->set_prob(1.0);
+                pobject->set_sensor_type(1);
+                pobject->set_yaw(fobjheading);
+                iv::fusion::PointXYZ * ppointxyz = new iv::fusion::PointXYZ;
+                ppointxyz->set_x(relx);
+                ppointxyz->set_y(rely);
+                ppointxyz->set_z(0);
+                iv::fusion::Dimension * pdim = new iv::fusion::Dimension;
+                pdim->set_x(xobj.base.geo.dimX);
+                pdim->set_y(xobj.base.geo.dimY);
+                pdim->set_z(xobj.base.geo.dimZ);
+                pobject->set_allocated_centroid(ppointxyz);
+                pobject->set_allocated_dimensions(pdim);
+                pobject->set_velocity_linear_x(sqrt(pow(relvx,2)+pow(relvy,2)));
+
+
+            }
+        }
+    }
+}
+
+void vtd_pilot::threadobject()
+{
+    std::vector<iv::vtd::rdbdata> xvectorrdbdata;
+    int64_t nlastsharetime = std::chrono::system_clock::now().time_since_epoch().count();
+//    int nmaxobjid = 2;
+    int nshareinter = 100;  //100 ms share a data.
+    bool bshare = false;
+    while(mbRun)
+    {
+        int64_t now = std::chrono::system_clock::now().time_since_epoch().count();
+        int64_t timediff = abs(now-nlastsharetime)/1000000;
+        if(timediff >= nshareinter)
+        {
+            bshare = true;
+        }
+        if(bshare)
+        {
+            //share object data.
+            bshare = false;
+            nlastsharetime = now;
+            xvectorrdbdata.clear();
+        }
+        std::unique_lock<std::mutex> lk(mmutexcvrdb);
+        if(mcvrdb.wait_for(lk,std::chrono::milliseconds(10)) == std::cv_status::timeout)
+        {
+            lk.unlock();
+            continue;
+        }
+        else
+        {
+            lk.unlock();
+
+        }
+        mmutexrdb.lock();
+        int i;
+        for(i=0;i<(int)mvectorrdbdata.size();i++)
+        {
+            xvectorrdbdata.push_back(mvectorrdbdata[i]);
+        }
+        mmutexrdb.unlock();
+    }
 }
 
 void vtd_pilot::UpdateVTD(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
@@ -57,11 +176,19 @@ void vtd_pilot::UpdateVTD(const char *strdata, const unsigned int nSize, const u
     iv::vtd::rdbdata xrdbdata;
     if(xrdbdata.ParseFromArray(strdata,nSize))
     {
+        mmutexrdb.lock();
+        mvectorrdbdata.push_back(xrdbdata);
+        mmutexrdb.unlock();
+        mcvrdb.notify_all();
         if(xrdbdata.mrdbitem_size()>0)
         {
             iv::vtd::rdbitem * pitem = xrdbdata.mutable_mrdbitem(0);
             msimFrame = pitem->simframe();
             msimTime = pitem->simtime();
+            if(pitem->pkgid() == RDB_PKG_ID_OBJECT_STATE)
+            {
+
+            }
         }
     }
 }
@@ -141,6 +268,12 @@ void vtd_pilot::threadego()
         double fheading = (90 - xgpsimu.heading())*M_PI/180.0;
         setOwnState(x,y,z,vx,vy,fheading);
 
+        mfX = x;
+        mfY = y;
+        mfHeading = fheading;
+        mfvx = vx;
+        mfvy = vy;
+
  //       RDB_OBJECT_STATE_t * pr = &sOwnObjectState;
 
         iv::modulecomm::ModuleSendMsg(mpaegostate,(char *)&sOwnObjectState,sizeof(RDB_OBJECT_STATE_t));

src/driver/vtd_pilot_if/vtd_pilot.h

@@ -12,6 +12,8 @@
 #include "gpsimu.pb.h"
 #include "modulecomm.h"
 
+#include "fusionobjectarray.pb.h"
+
 
 class vtd_pilot
 {
@@ -23,9 +25,12 @@ private:
     void UpdateVTD(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
 
     void threadego();
+    void threadobject();
 
     void setOwnState(double x, double y, double z,double vx,double vy,double fheading);
 
+    void ConvertToObjectArray(std::vector<iv::vtd::rdbdata> & xvectorrdbdata,iv::fusion::fusionobjectarray & xfusionarray);
+
 private:
     void * mpfromvtd, * mptovtd;
     void * mpagpsimu;
@@ -48,6 +53,13 @@ private:
     double msimTime;
 
     RDB_OBJECT_STATE_t sOwnObjectState;
+
+    double mfX,mfY,mfHeading,mfvx,mfvy;
+
+    std::vector<iv::vtd::rdbdata> mvectorrdbdata;
+    std::mutex mmutexrdb;
+    std::mutex mmutexcvrdb;
+    std::condition_variable mcvrdb;
 };
 
 #endif // VTD_PILOT_H

src/driver/vtd_pilot_if/vtd_pilot_if.pro

@@ -19,7 +19,9 @@ SOURCES += main.cpp \
     ../../include/msgtype/vtd.pb.cc \
     ../../include/msgtype/gpsimu.pb.cc \
     ../../common/common/math/gnss_coordinate_convert.cpp \
-    ../vtd_rdb/VTD/RDBHandler.cc
+    ../vtd_rdb/VTD/RDBHandler.cc \
+    ../../include/msgtype/fusionobject.pb.cc \
+    ../../include/msgtype/fusionobjectarray.pb.cc
 
 !include(../../../include/common.pri ) {
     error( "Couldn't find the common.pri file!" )
@@ -35,7 +37,9 @@ HEADERS += \
     ../../include/msgtype/gpsimu.pb.h \
     ../../common/common/math/gnss_coordinate_convert.h \
     ../vtd_rdb/VTD/RDBHandler.hh \
-    ../vtd_rdb/VTD/viRDBIcd.h
+    ../vtd_rdb/VTD/viRDBIcd.h \
+    ../../include/msgtype/fusionobject.pb.h \
+    ../../include/msgtype/fusionobjectarray.pb.h
 
 
 INCLUDEPATH += $$PWD/../../common/common/math