change map_lanetoxodr. add poly3 support, but not complete.

src/tool/map_lanetoxodr/OpenDrive/OpenDriveXmlParser.cpp

@@ -656,6 +656,7 @@ bool OpenDriveXmlParser::ReadLane (LaneSection* laneSection, TiXmlElement *node,
 	TiXmlElement *subNode=node->FirstChildElement("link");
 	TiXmlElement *subSubNode;
 	if (subNode)
+    {
 		subSubNode=subNode->FirstChildElement("predecessor");
 			if (subSubNode)
 			{
@@ -663,7 +664,9 @@ bool OpenDriveXmlParser::ReadLane (LaneSection* laneSection, TiXmlElement *node,
 				if (checker==TIXML_SUCCESS)
 					lane->SetPredecessor(predecessor);
 			}
+    }
 	if (subNode)
+    {
 		subSubNode=subNode->FirstChildElement("successor");
 			if (subSubNode)
 			{
@@ -671,6 +674,7 @@ bool OpenDriveXmlParser::ReadLane (LaneSection* laneSection, TiXmlElement *node,
 				if (checker==TIXML_SUCCESS)
 					lane->SetSuccessor(successor);
 			}
+    }
 
 	//Proceed to the Road width
 	subNode=node->FirstChildElement("width");

src/tool/map_lanetoxodr/OpenDrive/RoadGeometry.cpp

@@ -519,6 +519,27 @@ void GeometryPoly3::SetAll(double s, double x, double y, double hdg, double leng
 	ComputeVars();
 }
 
+//GetA to GetD, Added by Yuchuli
+double GeometryPoly3::GetA()
+{
+    return mA;
+}
+
+double GeometryPoly3::GetB()
+{
+    return mB;
+}
+
+double GeometryPoly3::GetC()
+{
+    return mC;
+}
+
+double GeometryPoly3::GetD()
+{
+    return mD;
+}
+
 
 //***********************************************************************************
 //Cubic Polynom geometry. Has to be implemented.  Added By Yuchuli

src/tool/map_lanetoxodr/OpenDrive/RoadGeometry.h

@@ -294,6 +294,12 @@ public:
 	 */
 	void SetAll(double s, double x, double y, double hdg, double length, double a,double b,double c,double d);
 
+
+    double GetA();
+    double GetB();
+    double GetC();
+    double GetD();
+
 };
 
 //----------------------------------------------------------------------------------

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -199,6 +199,7 @@ void MainWindow::ExecPainter()
             GeometryArc * parc;
             GeometryParamPoly3 * ppp3;
             GeometrySpiral *pSpiral;
+            GeometryPoly3 *ppoly;
             double rel_x,rel_y,rel_hdg;
             pg = pgeob->GetGeometryAt(0);
 
@@ -298,6 +299,47 @@ void MainWindow::ExecPainter()
                 }
                 }
                 break;
+            case 3:
+                {
+                ppoly = (GeometryPoly3 *)pg;
+                x = pg->GetX();
+                y = pg->GetY();
+                double A,B,C,D;
+                A = ppoly->GetA();
+                B = ppoly->GetB();
+                C = ppoly->GetC();
+                D = ppoly->GetD();
+                const double steplim = 0.1;
+                double du = steplim;
+                double u = 0;
+                double v = 0;
+                double oldx,oldy;
+                oldx = x;
+                oldy = y;
+                double xstart,ystart;
+                xstart = x;
+                ystart = y;
+                double hdgstart = ppoly->GetHdg();
+                double flen = 0;
+                while(flen < ppoly->GetLength())
+                {
+                    double fdis = 0;
+                    v = A + B*u + C*u*u + D*u*u*u;
+                    x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
+                    y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
+                    fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
+                    oldx = x;
+                    oldy = y;
+                    if(fdis>(steplim*2.0))du = du/2.0;
+                    flen = flen + fdis;
+                    u = u + du;
+                    std::cout<<" x: "<<x<<" y:"<<y<<std::endl;
+                    x = x + mfViewMoveX;
+                    y = y + mfViewMoveY;
+                    painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
+                }
+                }
+                break;
             case 4:
                 {
                 ppp3 = (GeometryParamPoly3 * )pg;

src/tool/map_lanetoxodr/myview.cpp

@@ -82,6 +82,8 @@ void MyView::zoomIn()
 
     centerOn(centerx,centery);
 
+    emit beishuchange(beishu);
+
 
 
 //    QPoint x = viewport()->rect().center();
@@ -117,6 +119,8 @@ void MyView::zoomOut()
 
 
     centerOn(centerx,centery);
+
+    emit beishuchange(beishu);
 }
 
 void MyView::zoomone()
@@ -125,6 +129,8 @@ void MyView::zoomone()
     scale(1 /beishu, 1 / beishu);
     beishu  = 1.0;
 
+    emit beishuchange(beishu);
+
 }
 
 void MyView::mouseDoubleClickEvent(QMouseEvent *event)

src/tool/map_lanetoxodr/myview.h

@@ -37,6 +37,7 @@ public Q_SLOTS:
 
 signals:
     void dbclickxy(double x,double y);
+    void beishuchange(double beishu);
 
 private:
     bool bottonstatus = false;
@@ -47,6 +48,8 @@ private:
     void pinchTriggered(QPinchGesture*);
 
     bool mbInPinch = false;
+
+
 };
 
 #endif // MYVIEW_H

src/tool/map_lanetoxodr/roaddigit.cpp

@@ -22,12 +22,28 @@ void RoadDigit::UpdateSpace(double fspace)
     CalcLane();
 }
 
+int RoadDigit::GetSectionIndex(Road *pRoad, double s)
+{
+    int nrtn = 0;
+    int nsectioncount = pRoad->GetLaneSectionCount();
+    if(nsectioncount == 1)return 0;
+    int i;
+    for(i=0;i<(nsectioncount-1);i++)
+    {
+        if(pRoad->GetLaneSection(i+1)->GetS()>s)
+        {
+            break;
+        }
+    }
+    nrtn = i;
+    return nrtn;
+}
+
 void RoadDigit::CalcLine(double fspace)
 {
     unsigned int j;
     iv::RoadDigitUnit rdu;
 
-
     bool bLastGeo = false;
     for(j=0;j<mpRoad->GetGeometryBlockCount();j++)
     {
@@ -40,6 +56,7 @@ void RoadDigit::CalcLine(double fspace)
         GeometryArc * parc;
         GeometryParamPoly3 * ppp3;
         GeometrySpiral *pSpiral;
+        GeometryPoly3 *ppoly;
         double rel_x,rel_y,rel_hdg;
         pg = pgeob->GetGeometryAt(0);
         x = pg->GetX();
@@ -60,14 +77,34 @@ void RoadDigit::CalcLine(double fspace)
             rdu.mfHdg = pg->GetHdg();
             mvectorRDU.push_back(rdu);
             int ncount = pg->GetLength() /fspace;
+//            int nsec1 = GetSectionIndex(mpRoad,pg->GetS());
+//            int nsec2 = GetSectionIndex(mpRoad,pg->GetS() + pg->GetLength());
+//            if(nsec1 != nsec2)
+//            {
+//                if((nsec2-nsec1)>1)
+//                {
+//                    ncount = pg->GetLength()/0.5;
+//                }
+//                else
+//                {
+//                    LaneSection * psec1 = mpRoad->GetLaneSection(nsec1);
+//                    LaneSection * psec2 = mpRoad->GetLaneSection(nsec2);
+//                    if(psec1->GetLaneCount() != psec2->GetLaneCount())
+//                    {
+//                        ncount = pg->GetLength()/0.1;
+//                    }
+//                }
+//            }
+
             if(ncount<2)ncount = 2;
             double fstep;
             if(ncount > 0)fstep = pg->GetLength()/ncount;
             int i;
             if(bLastGeo )ncount = ncount+1;
+            double xtem,ytem;
             for(i=1;i<ncount;i++)
             {
-                double xtem,ytem;
+
                 xtem = x + (i*fstep)*cos(pg->GetHdg());
                 ytem = y + (i*fstep)*sin(pg->GetHdg());
 
@@ -78,6 +115,21 @@ void RoadDigit::CalcLine(double fspace)
                 mvectorRDU.push_back(rdu);
 
             }
+//            if(bLastGeo == false)
+//            {
+//                double fx = ncount*fstep -0.1;
+//                if((fx < pg->GetLength())&&(fx> ((ncount -1 )*fstep)))
+//                {
+//                    xtem = x + fx*cos(pg->GetHdg());
+//                    ytem = y + fx*sin(pg->GetHdg());
+
+//                    rdu.mS = pg->GetS() + fx;
+//                    rdu.mX = xtem;
+//                    rdu.mY = ytem;
+//                    rdu.mfHdg = pg->GetHdg();
+//                    mvectorRDU.push_back(rdu);
+//                }
+//            }
         }
             break;
 
@@ -192,6 +244,56 @@ void RoadDigit::CalcLine(double fspace)
 
 
 
+            }
+            }
+            break;
+        case 3:
+            {
+            ppoly = (GeometryPoly3 *)pg;
+            x = pg->GetX();
+            y = pg->GetY();
+            double A,B,C,D;
+            A = ppoly->GetA();
+            B = ppoly->GetB();
+            C = ppoly->GetC();
+            D = ppoly->GetD();
+            const double steplim = 0.3;
+            double du = steplim;
+            double u = 0;
+            double v = 0;
+            double oldx,oldy;
+            oldx = x;
+            oldy = y;
+            double xstart,ystart;
+            xstart = x;
+            ystart = y;
+            rdu.mS = pg->GetS();
+            rdu.mX = pg->GetX();
+            rdu.mY = pg->GetY();
+            rdu.mfHdg = pg->GetHdg();
+            mvectorRDU.push_back(rdu);
+            double hdgstart = ppoly->GetHdg();
+            double flen = 0;
+            u = u+du;
+            while(flen < ppoly->GetLength())
+            {
+                double fdis = 0;
+                v = A + B*u + C*u*u + D*u*u*u;
+                x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
+                y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
+                fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
+
+                if(fdis>(steplim*2.0))du = du/2.0;
+                flen = flen + fdis;
+                u = u + du;
+                rdu.mS = pg->GetS() + flen;
+                rdu.mX = x;
+                rdu.mY = y;
+                rdu.mfHdg = xodrfunc::CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
+
+                mvectorRDU.push_back(rdu);
+                oldx = x;
+                oldy = y;
             }
             }
             break;

src/tool/map_lanetoxodr/roaddigit.h

@@ -51,6 +51,9 @@ private:
 public:
     std::vector<iv::RoadDigitUnit> * GetRDU();
     void UpdateSpace(double fspace);
+
+private:
+    int GetSectionIndex(Road * pRoad,double s);
 };
 
 #endif // ROADDIGIT_H

src/tool/map_lanetoxodr/xodrfunc.cpp

@@ -751,6 +751,8 @@ std::vector<iv::LanePoint> xodrfunc::GetAllLanePoint(Road *pRoad,  const double
                 lp.mnlanetype = nlanetype;
                 lp.mfhdg = fhdg;
                 lp.mS = s;
+                lp.mfGeoX = x;
+                lp.mfGeoY = y;
                 xvectorlanepoint.push_back(lp);
 
             }
@@ -766,6 +768,8 @@ std::vector<iv::LanePoint> xodrfunc::GetAllLanePoint(Road *pRoad,  const double
                 lp.mflanetocenter = 0;
                 lp.mfhdg = fhdg;
                 lp.mS = s;
+                lp.mfGeoX = x;
+                lp.mfGeoY = y;
                 xvectorlanepoint.push_back(lp);
             }
         }

src/tool/map_lanetoxodr/xodrfunc.h

@@ -20,6 +20,8 @@ struct LanePoint
     int mnlanetype;  //0 driving 1 border 2 none 3 bike
     int mnlanemarktype; // -1 no 0 solid 1 broken 2 solidsolid
     int mnlanecolor;
+    double mfGeoX;
+    double mfGeoY;
 
 };
 

src/tool/map_lanetoxodr/xodrscenfunc.cpp

@@ -15,18 +15,33 @@ std::vector<QGraphicsPathItem *> xodrscenfunc::GetRoadLaneItem(RoadDigit *prd)
     {
         std::vector<iv::LanePoint> xvepre = pvectorrdu->at(i).mvectorLanePoint;
         std::vector<iv::LanePoint> xvenxt = pvectorrdu->at(i+1).mvectorLanePoint;
-        if((xvepre.size()<2)||(xvenxt.size()<2)||(xvenxt.size() != xvepre.size()))
+        if((xvepre.size()<2)||(xvenxt.size()<2))
         {
             continue;
         }
+
         unsigned int k;
         for(k=0;k<(xvepre.size()-1);k++)
         {
             QPainterPath xpath;
-            xpath.moveTo(xvepre.at(k).mfX,xvepre.at(k).mfY*(-1));
-            xpath.lineTo(xvenxt.at(k).mfX,xvenxt.at(k).mfY*(-1));
-            xpath.lineTo(xvenxt.at(k+1).mfX,xvenxt.at(k+1).mfY*(-1));
-            xpath.lineTo(xvepre.at(k+1).mfX,xvepre.at(k+1).mfY*(-1));
+            if((xvenxt.size() != xvepre.size()))
+            {
+                double fdis = sqrt(pow(xvenxt.at(0).mfGeoX-xvepre.at(0).mfGeoX,2)
+                                   +pow(xvenxt.at(0).mfGeoY - xvepre.at(0).mfGeoY,2));
+                double fhdg = xvepre.at(0).mfhdg;
+                xpath.moveTo(xvepre.at(k).mfX,xvepre.at(k).mfY*(-1));
+                xpath.lineTo(xvepre.at(k).mfX + fdis*cos(fhdg),(xvepre.at(k).mfY + fdis*sin(fhdg))*(-1));
+                xpath.lineTo(xvepre.at(k+1).mfX + fdis*cos(fhdg),(xvepre.at(k+1).mfY + fdis*sin(fhdg))*(-1));
+                xpath.lineTo(xvepre.at(k+1).mfX,xvepre.at(k+1).mfY*(-1));
+
+            }
+            else
+            {
+                xpath.moveTo(xvepre.at(k).mfX,xvepre.at(k).mfY*(-1));
+                xpath.lineTo(xvenxt.at(k).mfX,xvenxt.at(k).mfY*(-1));
+                xpath.lineTo(xvenxt.at(k+1).mfX,xvenxt.at(k+1).mfY*(-1));
+                xpath.lineTo(xvepre.at(k+1).mfX,xvepre.at(k+1).mfY*(-1));
+            }
             xpath.closeSubpath();
             QGraphicsPathItem * pitem = new QGraphicsPathItem;
             pitem->setPath(xpath);

src/tool/map_lanetoxodr/xvmainwindow.cpp

@@ -31,7 +31,7 @@ XVMainWindow::XVMainWindow(QWidget *parent) :
     myview->setGeometry(QRect(30, 30, 600, 600));
 
     connect(myview,SIGNAL(dbclickxy(double,double)),this,SLOT(onClickXY(double,double)));
-
+    connect(myview,SIGNAL(beishuchange(double)),this,SLOT(onbeishuchange(double)));
 
     myview->setCacheMode(myview->CacheBackground);
 
@@ -39,6 +39,8 @@ XVMainWindow::XVMainWindow(QWidget *parent) :
     mpscene->setBackgroundBrush(Qt::darkGreen);
 
     myview->setScene(mpscene);
+    mfViewMoveX = VIEW_WIDTH/2.0;
+    mfViewMoveY = (-1.0)*VIEW_HEIGHT/2.0;
 
     connect(&mFileDialog,SIGNAL(accepted()),this,SLOT(onFileOpen()));
 
@@ -55,6 +57,8 @@ void XVMainWindow::resizeEvent(QResizeEvent *event)
     qDebug("resize");
     QSize sizemain = ui->centralwidget->size();
     qDebug("size x = %d y=%d",sizemain.width(),sizemain.height());
+    mnViewWidth = sizemain.width();
+    mnViewHeight = sizemain.height();
     myview->setGeometry(0,30,sizemain.width(),sizemain.height()-30);
 }
 
@@ -154,7 +158,8 @@ void XVMainWindow::UpdateScene()
         for(j=0;j<ncount;j++)
         {
             QGraphicsPathItem * pitem = xvectorlanepath[j];
-            pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
+//            pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
+            pitem->setPos(mfViewMoveX,-mfViewMoveY);
             mpscene->addItem(pitem);
             mvectorviewitem.push_back(pitem);
         }
@@ -168,7 +173,8 @@ void XVMainWindow::UpdateScene()
         for(j=0;j<ncount;j++)
         {
             QGraphicsPathItem * pitem = xvectormarkpath[j];
-            pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
+ //           pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
+            pitem->setPos(mfViewMoveX,-mfViewMoveY);
             mpscene->addItem(pitem);
             mvectorviewitem.push_back(pitem);
         }
@@ -195,19 +201,35 @@ void XVMainWindow::onClickXY(double x, double y)
 {
     double selx,sely;
     double lon,lat;
-    selx = (x - VIEW_WIDTH/2);
-    sely = (y - VIEW_HEIGHT/2) * (-1);
 
+
+    selx = (x  - (1.0/mfbeishu) * VIEW_WIDTH/2);
+    sely = (y  - (1.0/mfbeishu) *VIEW_HEIGHT/2) * (-1);
+
+    qDebug("beishu is %f ",mfbeishu);
+    sely = (y  - (1.0/mfbeishu) *(mnViewHeight/2)) * (-1);
+
+
+    qDebug("selx is %f sely is %f ",selx,sely);
     mfselx = selx;
-    mfsely = sely;
+    mfsely = sely ;
+
+
+//    selx = x;
+//    sely = y;
+//    mfselx = selx;
+//    mfsely = sely;
 
 //    double x0,y0;
 //    GaussProjCal(glon0,glat0,&x0,&y0);
 //    GaussProjInvCal(x0+selx,y0+sely,&lon,&lat);
 
     double rel_x,rel_y;
-    rel_x = selx - mfViewMoveX;
-    rel_y = sely - mfViewMoveY;
+//    rel_x = selx - mfViewMoveX;
+//    rel_y = sely - mfViewMoveY;
+
+    rel_x = selx ;
+    rel_y = sely ;
 
     Road * pRoad = 0;
     GeometryBlock * pgeob;
@@ -258,7 +280,8 @@ void XVMainWindow::on_actionSet_Move_triggered()
     }
     for(i=0;i<nsize;i++)
     {
-        mvectorviewitem.at(i)->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
+//        mvectorviewitem.at(i)->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
+        mvectorviewitem.at(i)->setPos(mfViewMoveX ,-mfViewMoveY);
         mpscene->addItem(mvectorviewitem.at(i));
     }
 
@@ -280,7 +303,8 @@ void XVMainWindow::on_actionReset_Move_triggered()
 
     for(i=0;i<nsize;i++)
     {
-        mvectorviewitem.at(i)->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
+        mvectorviewitem.at(i)->setPos(mfViewMoveX ,-mfViewMoveY);
+ //       mvectorviewitem.at(i)->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
         mpscene->addItem(mvectorviewitem.at(i));
     }
 
@@ -313,3 +337,8 @@ void XVMainWindow::on_actionHelp_triggered()
                           "在屏幕上可以移动查看区域");
     QMessageBox::information(this,"Help",helpinfo,QMessageBox::Yes);
 }
+
+void XVMainWindow::onbeishuchange(double fbeishu)
+{
+    mfbeishu = fbeishu;
+}

src/tool/map_lanetoxodr/xvmainwindow.h

@@ -40,6 +40,8 @@ private slots:
 
     void onClickXY(double x, double y);
 
+    void onbeishuchange(double fbeishu);
+
     void on_actionSet_Move_triggered();
 
     void on_actionReset_Move_triggered();
@@ -76,6 +78,11 @@ private:
     bool mbRefresh = false;
 
     FileDialogExtern mFileDialog;
+
+    double mfbeishu = 1.0;
+
+    int mnViewWidth;
+    int mnViewHeight;
 };
 
 #endif // XVMAINWINDOW_H

src/tool/map_lanetoxodr_graphscene/mainwindow.cpp

@@ -290,6 +290,9 @@ void MainWindow::ExecPainter()
                 }
                 }
                 break;
+            case 3:
+                qDebug("poly3");
+                break;
             case 4:
                 {
                 ppp3 = (GeometryParamPoly3 * )pg;

src1/driver/driver_gps_hcp2/.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
+

src1/driver/driver_gps_hcp2/driver_gps_hcp2.pro

@@ -0,0 +1,38 @@
+QT -= gui
+
+QT += network  serialport
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any Qt feature that has 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 it uses 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 += \
+        ivdriver_gps_hcp2.cpp \
+        main.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+!include(../interface/ivgps.pri ) {
+    error( "Couldn't find the ivlidar.pri file!" )
+}
+
+HEADERS += \
+    ivdriver_gps_hcp2.h

src1/driver/driver_gps_hcp2/ivdriver_gps_hcp2.cpp

@@ -0,0 +1,194 @@
+#include "ivdriver_gps_hcp2.h"
+
+#include <math.h>
+
+namespace iv {
+
+ivdriver_gps_hcp2::ivdriver_gps_hcp2()
+{
+    mTime.start();
+}
+
+int ivdriver_gps_hcp2::decode(iv::gps::gpsimu & xgpsimu)
+{
+    int xpos = mstrBuffer.indexOf('\n');
+    QString strsen;
+    if(xpos>0)
+    {
+
+        strsen = mstrBuffer.left(xpos+1);
+        mstrBuffer.remove(0,xpos+1);
+    }
+    else
+    {
+        return 0;
+    }
+
+    QStringList strlistrmc;
+
+RETRYDECODE:
+    strlistrmc = strsen.split(",");
+
+   if(strlistrmc.size() < 23)return -1;
+   if(strlistrmc.at(0) != "$GPCHC")return -2;
+
+   if(!checknmeasen(strsen.toLatin1().data(),strsen.length()))
+   {
+       int nPos = strsen.indexOf('$',10);
+       if(nPos > 0)
+       {
+           QString strnewsen = strsen.right(strsen.size()-nPos);
+//           qDebug("new sen is %s",strnewsen.toLatin1().data());
+           strsen = strnewsen;
+           goto RETRYDECODE;
+       }
+       return -3;
+   }
+
+   double fheading,fLat,fLon,fVel,fPitch,fRoll;
+   double fHgt,gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z;
+   int nsv1,nsv2;
+   int gpsweek,gpstime;
+   int insstate,rtkstate;
+   QString strx = strlistrmc.at(3);
+   fheading = strx.toDouble();
+
+   strx = strlistrmc.at(1);
+   gpsweek = strx.toInt();
+
+   strx = strlistrmc.at(2);
+   gpstime = strx.toInt();
+
+   strx = strlistrmc.at(12);
+   fLat = strx.toDouble();
+
+   strx  = strlistrmc.at(13);
+   fLon = strx.toDouble();
+
+   strx = strlistrmc.at(14);
+   fHgt = strx.toDouble();
+
+   double ve,vn,vu;
+   strx = strlistrmc.at(15);
+   ve = strx.toDouble();
+
+   strx = strlistrmc.at(16);
+   vn = strx.toDouble();
+
+   strx = strlistrmc.at(17);
+   vu = strx.toDouble();
+
+   fVel = sqrt(ve*ve + vn* vn);
+
+   if((mTime.elapsed()-mOldTime) >= 100)
+   {
+       if(mOldTime == 0)
+       {
+           mfCalc_acc = 0;
+           mfOldVel = fVel;
+           mOldTime = mTime.elapsed();
+       }
+       else
+       {
+           mfCalc_acc = (fVel - mfOldVel)/((mTime.elapsed() - mOldTime)*0.001);
+           mfOldVel = fVel;
+           mOldTime = mTime.elapsed();
+       }
+   }
+
+   strx = strlistrmc.at(4);
+   fPitch = strx.toDouble();
+
+   strx = strlistrmc.at(5);
+   fRoll = strx.toDouble();
+
+   strx = strlistrmc.at(6);
+   gyro_x = strx.toDouble();
+
+   strx = strlistrmc.at(7);
+   gyro_y = strx.toDouble();
+
+   strx = strlistrmc.at(8);
+   gyro_z = strx.toDouble();
+
+   strx = strlistrmc.at(9);
+   acc_x = strx.toDouble();
+
+   strx = strlistrmc.at(10);
+   acc_y = strx.toDouble();
+
+   strx = strlistrmc.at(11);
+   acc_z = strx.toDouble();
+
+   strx = strlistrmc.at(19);
+   nsv1 = strx.toInt();
+
+   strx = strlistrmc.at(20);
+   nsv2 = strx.toInt();
+
+   strx = strlistrmc.at(21);
+   char strstate[3];
+   strstate[2] = 0;
+   if(strx.size() >= 2)
+   {
+
+       memcpy(strstate,strx.data(),2);
+  //     qDebug(strstate);
+       char xstate;
+       xstate = strstate[0];
+       rtkstate = 4;
+       int xs;
+       if(xstate == '0')xs = 0;
+       if(xstate == '1')xs = 3;
+       if(xstate == '2')xs = 4;
+       if(xstate == '3')xs = 8;
+       if(xstate == '4')xs = 6;
+       if(xstate == '5')xs = 5;
+       if(xstate == '6')xs = 3;
+       if(xstate == '7')xs = 4;
+       if(xstate == '8')xs = 5;
+       if(xstate == '9')xs = 5;
+
+       rtkstate = xs;
+//       if(mstate == 0)minsstate = 0;
+//       else minsstate = 4;
+
+       xstate = strstate[1];
+       if(xstate == '0')insstate = 3;
+       else insstate = 4;
+       if(rtkstate == 0)insstate = 3;
+   }
+
+   xgpsimu.set_vd(vu);
+   xgpsimu.set_ve(ve);
+   xgpsimu.set_vn(vn);
+   xgpsimu.set_lat(fLat);
+   xgpsimu.set_lon(fLon);
+   xgpsimu.set_heading(fheading);
+   xgpsimu.set_state(4);
+   xgpsimu.set_msgtime(QDateTime::currentMSecsSinceEpoch());
+   xgpsimu.set_roll(fRoll);
+   xgpsimu.set_pitch(fPitch);
+
+   xgpsimu.set_rtk_state(rtkstate);
+   xgpsimu.set_ins_state(insstate);
+
+   xgpsimu.set_height(fHgt);
+   xgpsimu.set_satnum1(nsv1);
+   xgpsimu.set_satnum2(nsv2);
+   xgpsimu.set_gpsweek(gpsweek);
+   xgpsimu.set_gpstime(gpstime);
+   xgpsimu.set_gyro_x(gyro_x);
+   xgpsimu.set_gyro_y(gyro_y);
+   xgpsimu.set_gyro_z(gyro_z);
+   xgpsimu.set_acce_x(acc_x);
+   xgpsimu.set_acce_y(acc_y);
+   xgpsimu.set_acce_z(acc_z);
+   xgpsimu.set_acc_calc(mfCalc_acc);
+
+   return 1;
+
+
+}
+
+}

src1/driver/driver_gps_hcp2/ivdriver_gps_hcp2.h

@@ -0,0 +1,24 @@
+#ifndef IVDRIVER_GPS_HCP2_H
+#define IVDRIVER_GPS_HCP2_H
+
+#include "ivdriver_gps.h"
+
+namespace iv {
+
+
+class ivdriver_gps_hcp2 : public ivdriver_gps
+{
+public:
+    ivdriver_gps_hcp2();
+    virtual int decode(iv::gps::gpsimu & xgpsimu);
+
+private:
+    double mfOldVel;
+    qint64 mOldTime;
+    double mfCalc_acc;
+    QTime mTime;
+};
+
+}
+
+#endif // IVDRIVER_GPS_HCP2_H

src1/driver/driver_gps_hcp2/main.cpp

@@ -0,0 +1,14 @@
+#include <QCoreApplication>
+
+
+#include "ivdriver_gps_hcp2.h"
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    iv::ivmodule * pivmodule = new iv::ivdriver_gps_hcp2();
+    pivmodule->start();
+
+    return a.exec();
+}

src1/driver/driver_lidar_rs16/ivdriver_lidar_rs16.cpp

@@ -26,6 +26,12 @@ int ivdriver_lidar_rs16::processudpData(QByteArray ba, pcl::PointCloud<pcl::Poin
 
     char * pstr = ba.data();
 
+    if(ba.size()<1206)
+    {
+        std::cout<<"packet size is small. size is "<<ba.size()<<std::endl;
+        return -1;
+    }
+
     wt1 = ((pstr[2 + Group * 100] *256) + pstr[ 3 + Group * 100]) ;
     wt = wt1/ 100.0;
 

src1/driver/driver_lidar_rs32/ivdriver_lidar_rs32.cpp

@@ -11,6 +11,107 @@ ivdriver_lidar_rs32::ivdriver_lidar_rs32()
 
 int ivdriver_lidar_rs32::processudpData(QByteArray ba, pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud)
 {
+    static double fAngle_Total = 0;
+    static double fAngle_Last = 0;
+
+    int nrtn = 0;
+
+//    float w = 0.0036;
+    float Ang = 0;
+    float Range = 0;
+    int Group = 0;
+    int pointi = 0;
+    float wt = 0;
+    int wt1 = 0;
+    //    int dx;
+    //    int dy;
+    //    float lidar_32_xdistance = 0.3;			//32线激光雷达X轴补偿
+    //    float lidar_32_ydistance = -2.3;			//32线激光雷达Y轴补偿
+
+
+    float H_BETA[32] = {
+        -8,-8,-8,-8,-8,-2.672,2.672,8,
+        -8,-2.672,2.672,8,-8,-2.672,2.672,8,
+        8,8,8,-8,8,-8,8,-8,
+        -8,-2.672,2.672,8,-8,-2.672,2.672,8
+    };
+
+    //    float V_theta[16] = {-15,-13,-11,-9,-7,-5,-3,-1,15,13,11,9,7,5,3,1};
+    float V_theta[32] = {-25,-14.638,-7.91,-5.407,-3.667,-4,-4.333,-4.667,-2.333,-2.667,-3,-3.333,-1,-1.333,-1.667,-2,
+                         -10.281,-6.424,2.333,3.333,4.667,7,10.333,15,0.333,0,-0.333,-0.667,1.667,1.333,1.0,0.667};
+
+    if(ba.size()<1206)
+    {
+        std::cout<<"packet size is small. size is "<<ba.size()<<std::endl;
+        return -1;
+    }
+    char * pstr = (char * )ba.data();
+
+    wt1 = ((pstr[2 + Group * 100] *256) + pstr[ 3 + Group * 100]) ;
+    wt = wt1/ 100.0;
+
+    double fAngX = wt;
+    if(fabs(fAngX-fAngle_Last)>300)
+    {
+        fAngle_Total = fAngle_Total + fabs(fabs(fAngX-fAngle_Last)-360);
+    }
+    else
+    {
+        fAngle_Total = fAngle_Total + fabs(fabs(fAngX-fAngle_Last));
+    }
+
+    fAngle_Last = fAngX;
+    if(fAngle_Total > 360)
+    {
+        nrtn = 1;
+        fAngle_Total = 0;
+    }
+
+    for (Group = 0; Group <= 11; Group++)
+    {
+        wt1 = ((pstr[2 + Group * 100] *256) + pstr[ 3 + Group * 100]) ;
+        wt = wt1/ 100.0;
+
+        for (pointi = 0; pointi <32; pointi++)
+        {
+            //                        Ang = (0 - wt - w * T[pointi] - H_BETA[pointi]+213) / 180.0 * Lidar_Pi;
+            Ang = (0 - wt-H_BETA[pointi]) / 180.0 * M_PI;
+            Range = ((pstr[ Group * 100 + 4 + 3 * pointi] << 8) + pstr[Group * 100 + 5 + 3 * pointi]);
+            unsigned char intensity = pstr[ Group * 100 + 6 + 3 * pointi];
+            Range=Range* 5.0/1000.0;
+
+            if(Range<150)
+            {
+                pcl::PointXYZI point;
+                point.x  = Range*cos(V_theta[pointi] / 180 * M_PI)*cos(Ang + mfrollang);
+                point.y  = Range*cos(V_theta[pointi] / 180 * M_PI)*sin(Ang + mfrollang);
+                point.z  = Range*sin(V_theta[pointi] / 180 * M_PI);
+                if(mbinclix)
+                {
+                    double y,z;
+                    y = point.y;
+                    z = point.z;
+                    point.y = y*mcos_finclinationang_xaxis +z*msin_finclinationang_xaxis;
+                    point.z = z*mcos_finclinationang_xaxis - y*msin_finclinationang_xaxis;
+                }
+                if(mbincliy)
+                {
+                    double z,x;
+                    z = point.z;
+                    x = point.x;
+                    point.z = z*mcos_finclinationang_yaxis + x*msin_finclinationang_yaxis;
+                    point.x = x*mcos_finclinationang_yaxis - z*msin_finclinationang_yaxis;
+                }
+                point.intensity = intensity;
+                point_cloud->points.push_back(point);
+
+
+                ++point_cloud->width;
+            }
+        }
+
+    }
+    return nrtn;
 
 }
 

src1/driver/interface/ivdriver_gps.cpp

@@ -0,0 +1,214 @@
+#include "ivdriver_gps.h"
+
+#include <iostream>
+
+#include <math.h>
+
+namespace iv {
+
+ivdriver_gps::ivdriver_gps()
+{
+
+}
+
+void ivdriver_gps::loadxmlparam(std::string strxmlpath)
+{
+    iv::xmlparam::Xmlparam xp(strxmlpath);
+
+    mstrportname = xp.GetParam("devname","/dev/ttyUSB0");
+    mstrmsgname = xp.GetParam("msg_gpsimu","hcp2_gpsimu");
+    mnbaudrate = xp.GetParam("baudrate",230400);
+
+}
+
+void ivdriver_gps::ShareMsg(gps::gpsimu &xgpsimu)
+{
+    char * strser;
+    bool bser;
+    int nbytesize;
+    nbytesize = xgpsimu.ByteSize();
+    strser = new char[nbytesize];
+    bser = xgpsimu.SerializeToArray(strser,nbytesize);
+    if(bser)
+        iv::modulecomm::ModuleSendMsg(mpagpsimu,strser,nbytesize);
+    else
+    {
+        std::cout<<"gpsimu serialize error."<<std::endl;
+//        givlog->error("gpsimu serialize error.");
+//        gfault->SetFaultState(1, 0, "gpsimu serialize err");
+    }
+    delete strser;
+}
+
+void ivdriver_gps::broadcastmsg(gps::gpsimu &xgpsimu)
+{
+    unsigned char strSend[72];
+    int i;
+    for(i=0;i<72;i++)strSend[i] = 0x0;
+    strSend[0] = 0xE7;
+
+    strSend[21] = xgpsimu.ins_state();
+    strSend[68] = xgpsimu.rtk_state();
+    strSend[67] = 0;
+    strSend[62] = 62;
+
+    double fV = xgpsimu.lat();
+    fV = fV/(180.0/M_PI);
+    memcpy(strSend+23,&fV,8);
+    fV = xgpsimu.lon();fV = fV/(180.0/M_PI);memcpy(strSend+31,&fV,8);
+    float fF =0;memcpy(strSend+39,&fF,4);
+
+    int iV;
+    char * piV = (char *)&iV;
+    piV++;
+    fV = xgpsimu.ve();fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+43,piV,3);
+    fV = xgpsimu.vn();fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+46,piV,3);
+    fV = xgpsimu.vd();fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+49,piV,3);
+
+    fV = xgpsimu.heading()/(180.0/M_PI);fV = fV*1000000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+52,piV,3);
+    fV = xgpsimu.pitch()/(180.0/M_PI);fV = fV*1000000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+55,piV,3);
+    fV = xgpsimu.roll()/(180.0/M_PI);fV = fV*1000000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+58,piV,3);
+
+    double facc_x,facc_y,facc_z;
+    facc_x = xgpsimu.acce_x();
+    facc_y = xgpsimu.acce_y();
+    facc_z = xgpsimu.acce_z();
+
+    fV = facc_x;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+3,piV,3);
+    fV = facc_y;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+6,piV,3);
+    fV = facc_z;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+9,piV,3);
+
+    fV = xgpsimu.gyro_x();fV = fV*100000.0*M_PI/180.0;iV = (int)fV;iV = iV*256;memcpy(strSend+12,piV,3);
+    fV = xgpsimu.gyro_y();fV = fV*100000.0*M_PI/180.0;iV = (int)fV;iV = iV*256;memcpy(strSend+15,piV,3);
+    fV = xgpsimu.gyro_z();fV = fV*100000.0*M_PI/180.0;iV = (int)fV;iV = iV*256;memcpy(strSend+18,piV,3);
+
+
+    char c;
+    c = strSend[0];
+    for(i=1;i<71;i++)c = c+strSend[i];
+    strSend[71]= c;
+
+    mudpSocketGPSIMU->writeDatagram((char *)strSend,72,QHostAddress::Broadcast, 3000);
+
+
+}
+
+void ivdriver_gps::modulerun()
+{
+    std::cout<<"register msg name is "<<mstrmsgname<<std::endl;
+    mpagpsimu = iv::modulecomm::RegisterSend(mstrmsgname.data(),10000,1);
+
+    m_serialPort_GPS = new QSerialPort();
+    m_serialPort_GPS->setPortName(mstrportname.data());
+    m_serialPort_GPS->setBaudRate(230400);
+    m_serialPort_GPS->setParity(QSerialPort::NoParity);
+    m_serialPort_GPS->setDataBits(QSerialPort::Data8);
+    m_serialPort_GPS->setStopBits(QSerialPort::OneStop);
+    m_serialPort_GPS->setFlowControl(QSerialPort::NoFlowControl);
+    m_serialPort_GPS->setReadBufferSize(0);
+
+
+    mudpSocketGPSIMU = new QUdpSocket();
+
+    mbrun = true;
+
+    bool bPortOpen = false;
+
+OPENDEVICE:
+    bPortOpen = false;
+    std::cout<<" Open Device."<<std::endl;
+    while(mbrun)
+    {
+        if(m_serialPort_GPS->open(QSerialPort::ReadWrite))
+        {
+            bPortOpen = true;
+            break;
+        }
+        else
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+        }
+        std::cout<<"retry open "<<mstrportname<<" baudrate is "<<mnbaudrate<<std::endl;
+    }
+
+    int nnotrecv = 0;
+    while(mbrun)
+    {
+        QByteArray ba = m_serialPort_GPS->readAll();
+
+        if(ba.length() > 0)
+        {
+            nnotrecv = 0;
+            mstrBuffer.push_back(ba);
+            iv::gps::gpsimu xgpsimu;
+            int nrtn = decode(xgpsimu);
+            while(1 == nrtn)
+            {
+                ShareMsg(xgpsimu);
+                broadcastmsg(xgpsimu);
+                nrtn = decode(xgpsimu);
+            }
+            //Decode
+        }
+        else
+        {
+            nnotrecv++;
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+
+        if(nnotrecv == 1000)
+        {
+            std::cout<<"not recv data."<<std::endl;
+        }
+        if(nnotrecv > 3000)
+        {
+            m_serialPort_GPS->close();
+            goto OPENDEVICE;
+        }
+    }
+
+    if(bPortOpen)m_serialPort_GPS->close();
+    delete m_serialPort_GPS;
+    iv::modulecomm::Unregister(mpagpsimu);
+
+}
+
+bool ivdriver_gps::checknmeasen(const char *strsen, const unsigned int nlen)
+{
+    if(nlen< 4)return false;
+
+    int i;
+    char check;
+    int nstarpos = -1;
+    check = strsen[1]^strsen[2];
+    for(i=3;i<nlen;i++)
+    {
+        if(strsen[i] == '*')
+        {
+            nstarpos = i;
+            break;
+        }
+        check = check^strsen[i];
+    }
+    if(nstarpos < 0)return false;
+    if(nstarpos >(nlen -3))return false;
+    char strcheck[3];
+    int ncheck = check;
+    snprintf(strcheck,3,"%02X",ncheck);
+    char strsencheck[3];
+    strsencheck[2] = 0;
+    strsencheck[0] = strsen[nstarpos + 1];
+    strsencheck[1] = strsen[nstarpos + 2];
+    if(strncmp(strcheck,strsencheck,2) == 0)
+    {
+   //     qDebug("  r sen is %s",strsen);
+        return true;
+    }
+
+//    givlog->verbose("cal check is %s, sen check is %s",strcheck,strsencheck);
+
+//    qDebug("  sen is %s",strsen);
+    return false;
+}
+
+}

src1/driver/interface/ivdriver_gps.h

@@ -0,0 +1,50 @@
+#ifndef IVDRIVER_GPS_H
+#define IVDRIVER_GPS_H
+
+#include <QSerialPort>
+
+#include <QUdpSocket>
+#include <QNetworkDatagram>
+
+#include "ivdriver.h"
+#include "gpsimu.pb.h"
+#include "xmlparam.h"
+
+namespace iv {
+
+
+class ivdriver_gps : public ivdriver
+{
+public:
+    ivdriver_gps();
+
+public:
+    void loadxmlparam(std::string strxmlpath);
+    virtual void modulerun();
+
+    //if decode 1 sen. return 1,else return 0
+    virtual int decode(iv::gps::gpsimu & xgpsimu) = 0;
+
+public:
+    bool checknmeasen(const char * strsen,const unsigned int nlen);
+
+public:
+    QString mstrBuffer;
+
+private:
+    void ShareMsg(iv::gps::gpsimu & xgpsimu);
+    void broadcastmsg(iv::gps::gpsimu & xgpsimu);
+
+
+private:
+    int mnbaudrate = 230400;
+    std::string mstrportname = "/dev/ttyUSB0";
+    std::string mstrmsgname = "gps";
+
+    void * mpagpsimu;
+    QSerialPort *m_serialPort_GPS;
+    QUdpSocket  *mudpSocketGPSIMU;
+};
+}
+
+#endif // IVDRIVER_GPS_H

src1/driver/interface/ivgps.pri

@@ -0,0 +1,36 @@
+HEADERS += \
+    $$PWD/../../../src/include/msgtype/gps.pb.h \
+    $$PWD/../../../src/include/msgtype/gpsimu.pb.h \
+    $$PWD/../../../src/include/msgtype/imu.pb.h \
+    $$PWD/../../interface/ivmodule.h \
+    $$PWD/ivdriver.h \
+    $$PWD/ivdriver_gps.h
+
+SOURCES += \
+    $$PWD/../../../src/include/msgtype/gps.pb.cc \
+    $$PWD/../../../src/include/msgtype/gpsimu.pb.cc \
+    $$PWD/../../../src/include/msgtype/imu.pb.cc \
+    $$PWD/../../interface/ivmodule.cpp \
+    $$PWD/ivdriver.cpp  \
+    $$PWD/ivdriver_gps.cpp
+
+INCLUDEPATH += $$PWD/../interface
+INCLUDEPATH += $$PWD/../../interface
+
+INCLUDEPATH += $$PWD/../../common/modulecomm
+
+INCLUDEPATH += $$PWD/../../common/modulecomm_shm
+INCLUDEPATH += $$PWD/../../common/modulecomm_fastrtps
+INCLUDEPATH += $$PWD/../../common/modulecomm_inter
+
+LIBS += -L$$PWD/../../common/build-modulecomm-Debug -lmodulecomm
+LIBS += -L$$PWD/../../common/build-modulecomm_fastrtps-Debug -lmodulecomm_fastrtps
+LIBS += -L$$PWD/../../common/build-modulecomm_shm-Debug -lmodulecomm_shm
+LIBS += -L$$PWD/../../common/build-modulecomm_inter-Debug -lmodulecomm_inter
+
+
+INCLUDEPATH += $$PWD/../../../src/common/xmlparam
+LIBS += -L$$PWD/../../../bin/ -lxmlparam
+
+INCLUDEPATH += $$PWD/../../../src/include/msgtype
+

src1/interface/ivmodule.h

@@ -20,7 +20,7 @@ public:
     void setdefaultcommtype(iv::modulecomm::ModuleComm_TYPE xtype);
     iv::modulecomm::ModuleComm_TYPE getdefefaultcommtype();
 public:
-    bool mbrun = false;
+    bool mbrun = true;
 private:
     std::thread * mpthread;
 #ifdef MODULECOMM_DEFAULT_USE_INTER