complete tool_ivddecode. and change vtd_pilot_if, complete object to fusion object.

include/ivpcl.pri

@@ -6,3 +6,4 @@ win32:INCLUDEPATH += $$PWD/../thirdpartylib/pcl/pcl-1.8
 unix:INCLUDEPATH += /usr/include/eigen3
 unix:INCLUDEPATH += /usr/include/pcl-1.7
 unix:INCLUDEPATH += /usr/include/pcl-1.8
+unix:INCLUDEPATH += /usr/include/pcl-1.12

src/driver/vtd_pilot_if/vtd_pilot.cpp

@@ -26,7 +26,10 @@ vtd_pilot::vtd_pilot(std::string strfromvtd,std::string strtovtd)
 
     mpaegostate = iv::modulecomm::RegisterSend("egostate",10000,1);
 
+    mpafusion = iv::modulecomm::RegisterSend("fusion",10000000,1);
+
     mpthread = new std::thread(&vtd_pilot::threadego,this);
+    mpthreadfusion = new std::thread(&vtd_pilot::threadobject,this);
 
     memset( &sOwnObjectState, 0, sizeof( RDB_OBJECT_STATE_t ) );
 }
@@ -58,6 +61,7 @@ void vtd_pilot::ConvertToObjectArray(std::vector<iv::vtd::rdbdata> &xvectorrdbda
 {
     int i;
     int j;
+    std::cout<<" obj size: "<<xvectorrdbdata.size()<<std::endl;
     for(i=0;i<(int)xvectorrdbdata.size();i++)
     {
         iv::vtd::rdbdata * prdbdata = &xvectorrdbdata[i];
@@ -69,54 +73,65 @@ void vtd_pilot::ConvertToObjectArray(std::vector<iv::vtd::rdbdata> &xvectorrdbda
                 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++)
+   //             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;
+                if(xobj.base.id == 1)
                 {
-                    if(xfusionarray.mutable_obj(k)->id() == xobj.base.id)
-                    {
-                        pobject = xfusionarray.mutable_obj(k);
-                        break;
-                    }
+                    mfX = xobj.base.pos.x;
+                    mfY = xobj.base.pos.y;
+                    mfHeading = xobj.base.pos.h;
                 }
-                if(pobject  == NULL)
+                else
                 {
-                    pobject = xfusionarray.add_obj();
+                    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*cos(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)));
+
+                    std::cout<<" x: "<<relx<<" y: "<<rely<<std::endl;
                 }
-                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)));
 
 
             }
@@ -129,7 +144,7 @@ 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.
+    int nshareinter = 20;  //100 ms share a data.
     bool bshare = false;
     while(mbRun)
     {
@@ -141,6 +156,28 @@ void vtd_pilot::threadobject()
         }
         if(bshare)
         {
+            iv::fusion::fusionobjectarray  xfusionarray;
+            ConvertToObjectArray(xvectorrdbdata,xfusionarray);
+
+            if(xfusionarray.obj_size()>=1)
+            {
+
+            std::cout<<" now : "<<now/1000000<<std::endl;
+
+            int nbytesize = xfusionarray.ByteSizeLong();
+
+            std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);
+
+            if(xfusionarray.SerializeToArray(pstr_ptr.get(),nbytesize))
+            {
+                iv::modulecomm::ModuleSendMsg(mpafusion,pstr_ptr.get(),nbytesize);
+            }
+
+            }
+            else
+            {
+                std::cout<<"no object. "<<std::endl;
+            }
             //share object data.
             bshare = false;
             nlastsharetime = now;
@@ -163,7 +200,12 @@ void vtd_pilot::threadobject()
         {
             xvectorrdbdata.push_back(mvectorrdbdata[i]);
         }
+        mvectorrdbdata.clear();
         mmutexrdb.unlock();
+
+
+
+
     }
 }
 
@@ -180,16 +222,17 @@ void vtd_pilot::UpdateVTD(const char *strdata, const unsigned int nSize, const u
         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)
-            {
-
-            }
-        }
+ //       std::cout<<"notify"<<std::endl;
+//        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)
+//            {
+
+//            }
+//        }
     }
 }
 

src/driver/vtd_pilot_if/vtd_pilot.h

@@ -34,10 +34,12 @@ private:
 private:
     void * mpfromvtd, * mptovtd;
     void * mpagpsimu;
+    void * mpafusion;
 
     void * mpaegostate;
 
     std::thread * mpthread;
+    std::thread * mpthreadfusion;
 
     double mflat0 = 39.1207274;//39.1235363;
     double mflon0 = 117.0280032;//117.0272538;

src/driver/vtd_rdb/rdbconn.cpp

@@ -108,9 +108,10 @@ void RDBConn::threadconn(char *strserip, int nport)
 
     while(mbthreadconn)
     {
-        std::cout<<"start connect server."<<std::endl;
+
         if(bConnect == false)
         {
+            std::cout<<"start connect server."<<std::endl;
             if (connect( sClient, (struct sockaddr *)&server, sizeof( server ) ) == -1 )
             {
                 std::cout<<"VTD connect() failed. Server:"<<strserip<<" port:"<<nport<< std::endl;

src/tool/tool_ivddecode/mainwindow.cpp

@@ -2,14 +2,27 @@
 #include "ui_mainwindow.h"
 
 #include <QFileDialog>
+#include <QMessageBox>
 
 #include <iostream>
 
+#include "google/protobuf/io/zero_copy_stream_impl.h"
+#include "google/protobuf/text_format.h"
+
 MainWindow::MainWindow(QWidget *parent) :
     QMainWindow(parent),
     ui(new Ui::MainWindow)
 {
     ui->setupUi(this);
+
+    mpTimer = new QTimer();
+    connect(mpTimer,SIGNAL(timeout()),this,SLOT(onTimer()));
+    mpTimer->setInterval(10);
+
+    connect(this,SIGNAL(CurState(int)),this,SLOT(onCurState(int)));
+
+    ui->progressBar->setRange(0,100);
+    ui->progressBar->setValue(0);
 }
 
 MainWindow::~MainWindow()
@@ -31,14 +44,250 @@ void MainWindow::on_pushButton_decode_clicked()
     QString strivd = QFileDialog::getOpenFileName(this,tr("Open file"),"",tr("Record File(*.ivd)"));
     if(strivd.isEmpty())return;
 
-    mpthread = new std::thread(strivd.toStdString());
+    mpthread = new std::thread(&MainWindow::threadconvert,this, strivd,strdir);
     mbRunning = true;
     ui->pushButton_decode->setEnabled(false);
 
+    mpTimer->start();
+
+
+}
+
+void MainWindow::onCurState(int nstate)
+{
+    if(nstate == -1)
+    {
+        QMessageBox::warning(this,tr("Warning"),tr("Open File Fail."),QMessageBox::YesAll);
+
+    }
+
+    if(nstate == 0)
+    {
+        ui->progressBar->setValue(100);
+    }
+
+    ui->pushButton_decode->setEnabled(true);
+    mpTimer->stop();
+}
+
+void MainWindow::onTimer()
+{
+    qint64 nSize = mnFileSize;
+    if(nSize<=0)return;
+
+    int nPos = mnPos*100/mnFileSize;
+    ui->progressBar->setValue(nPos);
+}
+
+void MainWindow::threadconvert(QString strfilepath,QString strdir)
+{
+    mFile.setFileName(strfilepath);
+    if(mFile.open(QFile::ReadOnly))
+    {
+        mbOpen = true;
+        mnFileSize = mFile.size();
+        mnPos = 0;
+    }
+    else
+    {
+        mnFileSize = 0;
+        mbReplay = false;
+        mnPos = 0;
+        mbOpen = false;
+    }
+
+    if(mbOpen == false)
+    {
+        emit onCurState(-1);
+        return;
+    }
+
+    bool bx = true;
+    while(bx == true)
+    {
+        int nReadSize = 0;
+        int nDataSize;
+        char * strData;
+        char * strName;
+        bx = ReadARecord(nReadSize,&strName,&nDataSize,&strData);
+
+        mnPos = mnPos + nReadSize;
+        if(bx == true)
+        {
+            std::cout<<" read a record."<<std::endl;
+
+
+            std::cout<<" name : "<<strName<<std::endl;
+            if(strncmp(strName,"lidar_pc",256)==0)
+            {
+                QString strpath = strdir + "/" + mdtcurpos.toString("yyyy-MM-dd-hh-mm-ss-zzz") + ".pcd";
+                savepointcloud(strData,nDataSize,strpath);
+            }
+
+            if(strncmp(strName,"hcp2_gpsimu",256)==0)
+            {
+                QString strpath = strdir + "/" + mdtcurpos.toString("yyyy-MM-dd-hh-mm-ss-zzz") + ".txt";
+                savegps(strData,nDataSize,strpath);
+            }
+
+
+            delete strData;
+            delete strName;
+        }
+
+    }
+    std::cout<<"complete."<<std::endl;
+
+    emit onCurState(0);
 
 }
 
-void MainWindow::threadconvert(std::string strfilepath)
+inline QDateTime MainWindow::GetDateTimeFromRH(iv::RecordHead rh)
 {
+    QDateTime dt;
+    QDate datex;
+    QTime timex;
+    datex.setDate(rh.mYear,rh.mMon,rh.mDay);
+    timex.setHMS(rh.mHour,rh.mMin,rh.mSec,rh.mMSec);
+    dt.setDate(datex);
+    dt.setTime(timex);
 
+    return dt;
 }
+
+inline bool MainWindow::ReadARecord(int & nRecSize,char ** pstrName, int * pnDataSize, char ** pstrData)
+{
+    char strmark[10];
+    int nTotalSize,nHeadSize,nNameSize,nDataSize;
+    int nRead = mFile.read(strmark,1);
+    if(nRead == 0)return false;
+    nRead = mFile.read((char *)&nTotalSize,sizeof(int));
+    if(nRead < (int)sizeof(int))return false;
+    nRead = mFile.read((char *)&nHeadSize,sizeof(int));
+    if(nRead < (int)sizeof(int))return false;
+    nRead = mFile.read((char *)&nNameSize,sizeof(int));
+    if(nRead< (int)sizeof(int))return false;
+    nRead = mFile.read((char *)&nDataSize,sizeof(int));
+    if(nRead< (int)sizeof(int))return false;
+    if(nTotalSize !=(nHeadSize + nNameSize + nDataSize + 4*(int)sizeof(int) ))
+    {
+        return false;
+    }
+    iv::RecordHead rh;
+    char * strName = new char[1000];
+    char * strData = new char[nDataSize];
+
+    nRead = mFile.read((char *)&rh,sizeof(iv::RecordHead));
+    if(nRead < (int)sizeof(iv::RecordHead))
+    {
+        delete strData;
+        delete strName;
+        return false;
+    }
+
+    mdtcurpos = GetDateTimeFromRH(rh);
+
+    nRead = mFile.read(strName,nNameSize);
+    if(nRead < nNameSize)
+    {
+        delete strData;
+        delete strName;
+        return false;
+    }
+    strName[nNameSize] = 0;
+ //   qDebug(strName);
+//    qDebug("file pos is %d ms is %d",mFile.pos(),rh.mMSec);
+
+    nRead = mFile.read(strData,nDataSize);
+    if(nRead < nDataSize)
+    {
+        delete strData;
+        delete strName;
+        return false;
+    }
+
+    //Share Data
+
+    *pnDataSize = nDataSize;
+    *pstrName = strName;
+    *pstrData = strData;
+
+
+//    delete strData;
+    nRecSize = nTotalSize + 1;
+    return true;
+
+}
+
+void MainWindow::savepointcloud(const char *strdata, const unsigned int nSize, QString strfilepath)
+{
+    if(nSize <=16)return;
+    unsigned int * pHeadSize = (unsigned int *)strdata;
+    if(*pHeadSize > nSize)
+    {
+        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
+    }
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+    int nNameSize;
+    nNameSize = *pHeadSize - 4-4-8;
+    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
+    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
+    point_cloud->header.frame_id = strName;
+    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
+    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
+    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
+    int i;
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
+    for(i=0;i<nPCount;i++)
+    {
+        pcl::PointXYZI xp;
+        xp.x = p->x;
+        xp.y = p->y;
+        xp.z = p->z;
+        xp.intensity = p->intensity;
+        point_cloud->push_back(xp);
+        p++;
+
+  //      std::cout<<" x is "<<xp.x<<" y is "<<xp.y<<std::endl;
+    }
+
+    if(0 == pcl::io::savePCDFile(strfilepath.toLatin1().data(),*point_cloud))
+    {
+
+    }
+    else
+    {
+        std::cout<<" save pcd file fail. "<<std::endl;
+    }
+}
+
+void MainWindow::savegps(const char *strdata, const unsigned int nSize, QString strfilepath)
+{
+    iv::gps::gpsimu xgpsimu;
+    if(xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        using google::protobuf::TextFormat;
+        using google::protobuf::io::FileOutputStream;
+        using google::protobuf::io::ZeroCopyOutputStream;
+        std::string strout;
+        ZeroCopyOutputStream *output = new google::protobuf::io::StringOutputStream(&strout);//new FileOutputStream(file_descriptor);
+
+        bool success = TextFormat::Print(xgpsimu, output);
+        if(success)
+        {
+            QFile xFile;
+            xFile.setFileName(strfilepath);
+            if(xFile.open(QIODevice::ReadWrite))
+            {
+                xFile.write(strout.data(),strout.size());
+                xFile.close();
+            }
+ //           std::cout<<strout<<std::endl;
+//               qDebug(strout.data());
+        }
+    }
+}
+

src/tool/tool_ivddecode/mainwindow.h

@@ -2,9 +2,37 @@
 #define MAINWINDOW_H
 
 #include <QMainWindow>
+#include <QFile>
+#include <QMutex>
+#include <QDateTime>
+#include <QTimer>
 
 #include <thread>
 
+#include <pcl/conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+
+#include "gpsimu.pb.h"
+
+namespace iv {
+    struct RecordHead
+    {
+        int mnDataIndex;
+        unsigned short mYear;
+        unsigned char mMon;
+        unsigned char mDay;
+        unsigned char mHour;
+        unsigned char mMin;
+        unsigned char mSec;
+        unsigned short mMSec;
+        int mnTime;
+    };
+}
+
 namespace Ui {
 class MainWindow;
 }
@@ -17,8 +45,13 @@ public:
     explicit MainWindow(QWidget *parent = 0);
     ~MainWindow();
 
+signals:
+    void CurState(int);
+
 private slots:
     void on_pushButton_decode_clicked();
+    void onCurState(int );
+    void onTimer();
 
 private:
     Ui::MainWindow *ui;
@@ -27,7 +60,34 @@ private:
     bool mbRunning;
 
 private:
-    void threadconvert(std::string strfilepath);
+    QFile mFile;
+    bool mbOpen;
+    qint64 mnPos;
+    qint64 mnFileSize = 0;
+    bool mbReplay;
+    QMutex mMutex;
+
+    QDateTime mdtstart;
+    QDateTime mdtcurpos;
+    QTimer mTimerUpdate;
+    QTime mTime;
+
+    QDateTime mdtrpstart;
+
+    double mrpspeed = 1.0;
+
+    QTimer * mpTimer;
+
+private:
+    void threadconvert(QString strfilepath,QString strdir);
+
+    inline QDateTime GetDateTimeFromRH(iv::RecordHead rh);
+
+    inline bool ReadARecord(int & nRecSize,char ** pstrName, int * pnDataSize, char ** pstrData);
+
+    void savepointcloud(const char * strdata,const unsigned int nSize,QString strfilepath);
+
+    void savegps(const char * strdata,const unsigned int nSize,QString strfilepath);
 };
 
 #endif // MAINWINDOW_H

src/tool/tool_ivddecode/tool_ivddecode.pro

@@ -25,10 +25,47 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 SOURCES += \
         main.cpp \
-        mainwindow.cpp
+        mainwindow.cpp \
+    ../../include/msgtype/gpsimu.pb.cc
 
 HEADERS += \
-        mainwindow.h
+        mainwindow.h \
+    ../../include/msgtype/gpsimu.pb.h
 
 FORMS += \
         mainwindow.ui
+
+
+
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
+
+INCLUDEPATH += $$PWD/../../../src/include/msgtype
+
+
+unix:LIBS +=  -lpcl_common\
+        -lpcl_features\
+        -lpcl_filters\
+        -lpcl_io\
+        -lpcl_io_ply\
+        -lpcl_kdtree\
+        -lpcl_keypoints\
+        -lpcl_octree\
+        -lpcl_outofcore\
+        -lpcl_people\
+        -lpcl_recognition\
+        -lpcl_registration\
+        -lpcl_sample_consensus\
+        -lpcl_search\
+        -lpcl_segmentation\
+        -lpcl_surface\
+        -lpcl_tracking\
+        -lpcl_visualization
+