add vtd_pilot_if.

src/decition/decition_brain_sf/decition/brain.cpp

@@ -127,6 +127,8 @@ iv::decition::BrainDecition::BrainDecition()
 
     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);

src/driver/vtd_pilot_if/.gitignore

@@ -0,0 +1,73 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/driver/vtd_pilot_if/main.cpp

@@ -0,0 +1,13 @@
+#include <QCoreApplication>
+
+#include "vtd_pilot.h"
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    vtd_pilot * pvp = new vtd_pilot("1","2");
+    (void)pvp;
+
+    return a.exec();
+}

src/driver/vtd_pilot_if/vtd_pilot.cpp

@@ -0,0 +1,151 @@
+#include "vtd_pilot.h"
+
+
+static void CalcXY(const double lat0,const double lon0,
+              const double lat,const double lon,
+              double & x,double & y)
+{
+
+    double x0,y0;
+    GaussProjCal(lon0,lat0,&x0,&y0);
+    GaussProjCal(lon,lat,&x,&y);
+    x = x - x0;
+    y = y-  y0;
+}
+
+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 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);
+
+    mpaegostate = iv::modulecomm::RegisterSend("egostate",10000,1);
+
+    mpthread = new std::thread(&vtd_pilot::threadego,this);
+
+    memset( &sOwnObjectState, 0, sizeof( RDB_OBJECT_STATE_t ) );
+}
+
+void vtd_pilot::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    (void)index;
+    (void)dt;
+    (void)strmemname;
+    iv::gps::gpsimu  xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"parse gpsimu fail"<<std::endl;
+        return;
+    }
+
+    mmutex.lock();
+    mgpsimu.CopyFrom(xgpsimu);
+    mbupdate_gps = true;
+    mmutex.unlock();
+}
+
+void vtd_pilot::UpdateVTD(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void)index;
+    (void)dt;
+    (void)strmemname;
+
+    iv::vtd::rdbdata xrdbdata;
+    if(xrdbdata.ParseFromArray(strdata,nSize))
+    {
+        if(xrdbdata.mrdbitem_size()>0)
+        {
+            iv::vtd::rdbitem * pitem = xrdbdata.mutable_mrdbitem(0);
+            msimFrame = pitem->simframe();
+            msimTime = pitem->simtime();
+        }
+    }
+}
+
+void vtd_pilot::setOwnState(double x, double y, double z)
+{
+    sOwnObjectState.base.id       = 1;
+    sOwnObjectState.base.category = RDB_OBJECT_CATEGORY_PLAYER;
+    sOwnObjectState.base.type     = RDB_OBJECT_TYPE_PLAYER_CAR;
+    strcpy( sOwnObjectState.base.name, "Ego" );
+
+    // dimensions of own vehicle
+    sOwnObjectState.base.geo.dimX = 4.60 ;//* dimFactor;
+    sOwnObjectState.base.geo.dimY = 1.86 ;//* dimFactor;
+    sOwnObjectState.base.geo.dimZ = 1.60 ;//* dimFactor;
+
+
+    // offset between reference point and center of geometry
+    sOwnObjectState.base.geo.offX = 0.80;
+    sOwnObjectState.base.geo.offY = 0.00;
+    sOwnObjectState.base.geo.offZ = 0.30;
+
+    sOwnObjectState.base.pos.x     = x;
+    sOwnObjectState.base.pos.y     = y;
+    sOwnObjectState.base.pos.z     = z;
+    sOwnObjectState.base.pos.h     = 0.0;
+    sOwnObjectState.base.pos.p     = 0.0;
+    sOwnObjectState.base.pos.r     = 0.0;
+    sOwnObjectState.base.pos.flags = RDB_COORD_FLAG_POINT_VALID | RDB_COORD_FLAG_ANGLES_VALID;
+
+    sOwnObjectState.ext.speed.x     = 0;
+    sOwnObjectState.ext.speed.y     = 0;
+    sOwnObjectState.ext.speed.z     = 0;
+    sOwnObjectState.ext.speed.h     = 0.0;
+    sOwnObjectState.ext.speed.p     = 0.0;
+    sOwnObjectState.ext.speed.r     = 0.0;
+    sOwnObjectState.ext.speed.flags = RDB_COORD_FLAG_POINT_VALID | RDB_COORD_FLAG_ANGLES_VALID;
+
+    sOwnObjectState.ext.accel.x     = 0.0;
+    sOwnObjectState.ext.accel.y     = 0.0;
+    sOwnObjectState.ext.accel.z     = 0.0;
+    sOwnObjectState.ext.accel.flags = RDB_COORD_FLAG_POINT_VALID;
+
+    sOwnObjectState.base.visMask    =  RDB_OBJECT_VIS_FLAG_TRAFFIC | RDB_OBJECT_VIS_FLAG_RECORDER;
+}
+
+void vtd_pilot::threadego()
+{
+    while(mbRun)
+    {
+        if(mbupdate_gps == false)
+        {
+            std::unique_lock<std::mutex> lk(mmutexcv);
+            if(mcv.wait_for(lk,std::chrono::milliseconds(100)) == std::cv_status::timeout)
+            {
+                lk.unlock();
+                continue;
+            }
+            else
+            {
+                lk.unlock();
+                continue;
+            }
+        }
+
+        iv::gps::gpsimu xgpsimu;
+        mmutex.lock();
+        xgpsimu.CopyFrom(mgpsimu);
+        mbupdate_gps = false;
+        mmutex.unlock();
+
+        double x,y,z;
+        CalcXY(xgpsimu.lat(),xgpsimu.lon(),mflat0,mflon0,x,y);
+        z = 0;
+        setOwnState(x,y,z);
+
+        RDB_OBJECT_STATE_t * pr = &sOwnObjectState;
+
+        iv::modulecomm::ModuleSendMsg(mpaegostate,(char *)&sOwnObjectState,sizeof(RDB_OBJECT_STATE_t));
+
+
+
+        //convert to x,y,z
+
+        //send data.
+    }
+}

src/driver/vtd_pilot_if/vtd_pilot.h

@@ -0,0 +1,53 @@
+#ifndef VTD_PILOT_H
+#define VTD_PILOT_H
+
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+#include <iostream>
+
+#include "RDBHandler.hh"
+#include "gnss_coordinate_convert.h"
+#include "vtd.pb.h"
+#include "gpsimu.pb.h"
+#include "modulecomm.h"
+
+
+class vtd_pilot
+{
+public:
+    vtd_pilot(std::string strfromvtd,std::string strtovtd);
+
+private:
+    void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void UpdateVTD(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
+    void threadego();
+
+    void setOwnState(double x, double y, double z);
+
+private:
+    void * mpfromvtd, * mptovtd;
+    void * mpagpsimu;
+
+    void * mpaegostate;
+
+    std::thread * mpthread;
+
+    double mflat0 = 39.12017769;
+    double mflon0 = 117.0280226;
+
+    iv::gps::gpsimu mgpsimu;
+    bool mbupdate_gps = false;
+    std::mutex mmutex;
+    std::mutex mmutexcv;
+    std::condition_variable mcv;
+    bool mbRun = true;
+
+    int msimFrame;
+    double msimTime;
+
+    RDB_OBJECT_STATE_t sOwnObjectState;
+};
+
+#endif // VTD_PILOT_H

src/driver/vtd_pilot_if/vtd_pilot_if.pro

@@ -0,0 +1,43 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += main.cpp \
+    vtd_pilot.cpp \
+    ../../include/msgtype/vtd.pb.cc \
+    ../../include/msgtype/gpsimu.pb.cc \
+    ../../common/common/math/gnss_coordinate_convert.cpp \
+    ../vtd_rdb/VTD/RDBHandler.cc
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+HEADERS += \
+    vtd_pilot.h \
+    ../../include/msgtype/vtd.pb.h \
+    ../../include/msgtype/gpsimu.pb.h \
+    ../../common/common/math/gnss_coordinate_convert.h \
+    ../vtd_rdb/VTD/RDBHandler.hh \
+    ../vtd_rdb/VTD/viRDBIcd.h
+
+
+INCLUDEPATH += $$PWD/../../common/common/math
+
+INCLUDEPATH += $$PWD/../vtd_rdb/VTD

src/driver/vtd_rdb/rdbconn.cpp

@@ -83,6 +83,40 @@ void RDBConn::threadconn(char *strserip, int nport)
 
         std::cout<<" ret: "<<ret<<std::endl;
 
+        if(mbEgoUpdate)
+        {
+            Framework::RDBHandler myHandler;
+
+            // start a new message
+            myHandler.initMsg();
+
+            // begin with an SOF identifier
+            myHandler.addPackage( simTime, simFrame, RDB_PKG_ID_START_OF_FRAME );
+
+            // add extended package for the object state
+            RDB_OBJECT_STATE_t *objState = ( RDB_OBJECT_STATE_t* ) myHandler.addPackage( simTime, simFrame, RDB_PKG_ID_OBJECT_STATE, 1, true );
+
+            if ( !objState )
+            {
+                fprintf( stderr, "sendOwnObjectState: could not create object state\n" );
+                return;
+            }
+
+            // copy contents of internally held object state to output structure
+            memcpy( objState, &sOwnObjectState, sizeof( RDB_OBJECT_STATE_t ) );
+
+            fprintf( stderr, "sendOwnObjectState: sending pos x/y/z = %.3lf/%.3lf/%.3lf,\n", objState->base.pos.x, objState->base.pos.y, objState->base.pos.z );
+
+            // terminate with an EOF identifier
+            myHandler.addPackage( simTime, simFrame, RDB_PKG_ID_END_OF_FRAME );
+
+            int retVal = send( sClient, ( const char* ) ( myHandler.getMsg() ), myHandler.getMsgTotalSize(), 0 );
+
+            if ( !retVal )
+                fprintf( stderr, "sendOwnObjectState: could not send object state\n" );
+            mbEgoUpdate = false;
+        }
+
         if ( ret == -1 )
         {
             printf( "recv() failed: %s\n", strerror( errno ) );
@@ -161,6 +195,8 @@ void RDBConn::parseRDBMessage(RDB_MSG_t *msg)
 
     while ( remainingBytes )
     {
+        simTime =  msg->hdr.simTime;
+        simFrame = msg->hdr.frameNo;
         parseRDBMessageEntry( msg->hdr.simTime, msg->hdr.frameNo, entry );
 
 
@@ -205,6 +241,10 @@ void RDBConn::parseRDBMessageEntry(const double &simTime, const unsigned int &si
     while ( noElements-- )
     {
         iv::rdbitem xrdbitem;
+        int64_t nchronotime = std::chrono::system_clock::now().time_since_epoch().count()/1000;
+        double fchronotime = nchronotime;
+        fchronotime = fchronotime * 0.000001;
+        xrdbitem.mfMsgTime = fchronotime;
         xrdbitem.simFrame = simFrame;
         xrdbitem.simTime = simTime;
         xrdbitem.mnpkgdatasize = entryHdr->elementSize;

src/driver/vtd_rdb/rdbconn.h

@@ -26,6 +26,7 @@
 namespace iv {
 struct rdbitem
 {
+    double mfMsgTime; //when recv this meesage,second
     double simTime;
     unsigned int simFrame;
     uint16_t  pkgid;
@@ -63,6 +64,12 @@ private:
 
 public:
     int ConsumeBuf(iv::rdbitem & xrdbitem,int nwaitms = 0);
+
+public:
+    RDB_OBJECT_STATE_t sOwnObjectState;
+    bool mbEgoUpdate = false;
+    double simTime;
+    unsigned int simFrame;
 };
 
 #endif // RDBCONN_H

src/driver/vtd_rdb/rdbmodulecomm.cpp

@@ -15,6 +15,9 @@ rdbmodulecomm::rdbmodulecomm(std::string strcommonmsg,std::string strpicturemsg,
     int ninterval = 10;
     mpthreadCommonSend = new std::thread(&rdbmodulecomm::threadCommonSend,this,ninterval);
 
+    ModuleFun funego =std::bind(&rdbmodulecomm::UpdateEGO,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpaegostate = iv::modulecomm::RegisterRecvPlus("egostate",funego);
+
 }
 
 rdbmodulecomm::~rdbmodulecomm()
@@ -39,7 +42,7 @@ void rdbmodulecomm::threadCommon(RDBConn * pconn)
         mmutexcommon.lock();
         mvectorcommonitem.push_back(xrdbitem);
         mmutexcommon.unlock();
-        continue;
+  //      continue;
         std::cout<<"pkg id: "<<xrdbitem.pkgid<<std::endl;
         switch (xrdbitem.pkgid) {
         case RDB_PKG_ID_OBJECT_STATE:
@@ -83,7 +86,7 @@ void rdbmodulecomm::threadCommonSend(int nmsinterval)
     while(mbRun)
     {
         int64_t timenow =  std::chrono::system_clock::now().time_since_epoch().count();
-        if(abs(timenow - lastsend) < nmsinterval )
+        if(abs(timenow - lastsend)/1000000 < nmsinterval )
         {
             std::this_thread::sleep_for(std::chrono::milliseconds(1));
             continue;
@@ -97,6 +100,7 @@ void rdbmodulecomm::threadCommonSend(int nmsinterval)
         {
             iv::vtd::rdbitem * pitem = xrdbdata.add_mrdbitem();
             iv::rdbitem * prdbitem = &mvectorcommonitem[i];
+            pitem->set_fmsgtime(prdbitem->mfMsgTime);
             pitem->set_simframe(prdbitem->simFrame);
             pitem->set_simtime(prdbitem->simTime);
             pitem->set_pkgid(prdbitem->pkgid);
@@ -105,6 +109,8 @@ void rdbmodulecomm::threadCommonSend(int nmsinterval)
         mvectorcommonitem.clear();
         mmutexcommon.unlock();
 
+        lastsend = timenow;
+
         int ndatasize = xrdbdata.ByteSize();
         std::shared_ptr<char> pstrdata_ptr = std::shared_ptr<char>(new char[ndatasize]);
         if(xrdbdata.SerializeToArray(pstrdata_ptr.get(),ndatasize))
@@ -118,3 +124,12 @@ void rdbmodulecomm::threadCommonSend(int nmsinterval)
 
     }
 }
+
+void rdbmodulecomm::UpdateEGO(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    memcpy((char*)&mpConnCommon->sOwnObjectState,strdata,nSize);
+
+    RDB_OBJECT_STATE_t * pr = &mpConnCommon->sOwnObjectState;
+
+    mpConnCommon->mbEgoUpdate = true;
+}

src/driver/vtd_rdb/rdbmodulecomm.h

@@ -8,6 +8,8 @@
 #include "rdbconn.h"
 #include <iostream>
 
+#include "RDBHandler.hh"
+
 #include "vtd.pb.h"
 
 class rdbmodulecomm
@@ -34,6 +36,8 @@ private:
     void * mpacommon;
     void * mpapicture;
 
+    void * mpaegostate;
+
     bool mbRun = true;
 
     std::thread * mpthreadcommon;
@@ -47,6 +51,8 @@ private:
 
     void threadCommonSend(int nmsinterval);
 
+    void UpdateEGO(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
 };
 
 #endif // RDBMODULECOMM_H