change driver_map_xodrload. change roi service,not complete.

src/driver/driver_lidar_xodrroifilter/.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/driver_lidar_xodrroifilter/driver_lidar_xodrroifilter.pro

@@ -0,0 +1,41 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# You can make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+        ../../include/msgtype/gpsimu.pb.cc \
+        ../../include/msgtype/roireqest.pb.cc \
+        ../../include/msgtype/roiresult.pb.cc \
+        lidarxodrroifilter.cpp \
+        main.cpp
+
+TRANSLATIONS += \
+    driver_lidar_xodrroifilter_zh_CN.ts
+
+# 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/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+!include(../../../include/ivboost.pri ) {
+    error( "Couldn't find the ivboost.pri file!" )
+}
+
+HEADERS += \
+    ../../include/msgtype/gpsimu.pb.h \
+    ../../include/msgtype/roireqest.pb.h \
+    ../../include/msgtype/roiresult.pb.h \
+    lidarxodrroifilter.h

src/driver/driver_lidar_xodrroifilter/driver_lidar_xodrroifilter_zh_CN.ts

@@ -0,0 +1,3 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!DOCTYPE TS>
+<TS version="2.1" language="driver_lidar_xodrroifilter_zh_CN"></TS>

src/driver/driver_lidar_xodrroifilter/lidarxodrroifilter.cpp

@@ -0,0 +1,84 @@
+#include "lidarxodrroifilter.h"
+
+#include "ivservice.h"
+
+#include <iostream>
+
+#include <math.h>
+
+LidarXODRROIFilter::LidarXODRROIFilter()
+{
+    mpthread = new std::thread(&LidarXODRROIFilter::threadroi,this);
+}
+
+void LidarXODRROIFilter::SetGPSIMU(iv::gps::gpsimu &xgpsimu)
+{
+    mgpsimu.CopyFrom(xgpsimu);
+    mbGPSSet = true;
+    mwc.wakeAll();
+}
+
+void LidarXODRROIFilter::threadroi()
+{
+    iv::service::Client xclienROI("ServiceROI");
+//    std::shared_ptr<char> pstrvt_ptr;
+//    int ndatasize;
+
+    while(mbrun)
+    {
+        if(mbGPSSet == false)
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+            continue;
+        }
+
+        mwaitmutex.lock();
+        mwc.wait(&mwaitmutex,100);
+        mwaitmutex.unlock();
+
+        bool breq = false;
+        if(mbroiexist == false)
+        {
+            breq = true;
+        }
+        else
+        {
+            if((fabs(mgpsimu.lat() - moldgpsimu.lat()) > 0.0000010)||(fabs(mgpsimu.lon() - moldgpsimu.lon()) > 0.0000010))
+            {
+                breq = true;
+            }
+        }
+
+        if(breq)
+        {
+            iv::roi::request xreq;
+            xreq.set_gridsize(0.2);
+            xreq.set_heading(mgpsimu.heading());
+            xreq.set_lat(mgpsimu.lat());
+            xreq.set_lon(mgpsimu.lon());
+            xreq.set_range(100.0);
+
+            int nbytesize = xreq.ByteSize();
+            std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);
+            xreq.SerializeToArray(pstr_ptr.get(),nbytesize);
+            int ndatasize;
+            std::shared_ptr<char> pstrvt_ptr;
+
+            if(xclienROI.SendRequest(pstr_ptr,nbytesize,pstrvt_ptr,ndatasize,10000) == iv::service::Client::HAVE_RES)
+            {
+                std::cout<<" size: "<< ndatasize<<std::endl;
+                mbroiexist = true;
+                moldgpsimu.CopyFrom(mgpsimu);
+            }
+            else
+            {
+                std::cout<<" Can't Get ROI"<<std::endl;
+            }
+
+
+
+
+        }
+
+    }
+}

src/driver/driver_lidar_xodrroifilter/lidarxodrroifilter.h

@@ -0,0 +1,38 @@
+#ifndef LIDARXODRROIFILTER_H
+#define LIDARXODRROIFILTER_H
+
+#include "gpsimu.pb.h"
+
+#include "roireqest.pb.h"
+#include "roiresult.pb.h"
+
+#include <thread>
+
+#include <QMutex>
+#include <QWaitCondition>
+
+class LidarXODRROIFilter
+{
+public:
+    LidarXODRROIFilter();
+
+
+public:
+    void SetGPSIMU(iv::gps::gpsimu & xgpsimu);
+
+private:
+    bool mbGPSSet = false;
+    bool mbroiexist = false;
+    iv::gps::gpsimu mgpsimu;
+    iv::gps::gpsimu moldgpsimu;
+
+private:
+    void threadroi();
+    std::thread * mpthread;
+    bool mbrun = true;
+
+    QMutex mwaitmutex;
+    QWaitCondition mwc;
+};
+
+#endif // LIDARXODRROIFILTER_H

src/driver/driver_lidar_xodrroifilter/main.cpp

@@ -0,0 +1,42 @@
+#include <QCoreApplication>
+
+#include <iostream>
+
+#include "gpsimu.pb.h"
+#include "roireqest.pb.h"
+#include "roiresult.pb.h"
+
+#include "modulecomm.h"
+
+#include "lidarxodrroifilter.h"
+
+LidarXODRROIFilter gLXRF;
+
+void ListenGPS(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))
+    {
+        gLXRF.SetGPSIMU(xgpsimu);
+    }
+    else
+    {
+        std::cout<<" ListenGPS Parse Faile"<<std::endl;
+    }
+
+}
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    void * pa = iv::modulecomm::RegisterRecv("hcp2_gpsimu",ListenGPS);
+    (void)pa;
+
+    return a.exec();
+}

src/driver/driver_map_xodrload/main.cpp

@@ -35,6 +35,7 @@
 #include <signal.h>
 
 #include "service_roi_xodr.h"
+#include "ivservice.h"
 
 OpenDrive mxodr;
 xodrdijkstra * gpxd;
@@ -59,6 +60,8 @@ iv::Ivlog *givlog = nullptr;
 
 static QCoreApplication * gApp;
 
+service_roi_xodr gsrx;
+
 
 namespace iv {
 struct simpletrace
@@ -141,8 +144,7 @@ bool LoadXODR(std::string strpath)
     givlog->info("road name is %s",proad1->GetRoadName().data());
     std::cout<<" road name is "<<proad1->GetRoadName()<<std::endl;
 
-    service_roi_xodr * psrx = new service_roi_xodr();
-    psrx->SetXODR(&mxodr);
+    gsrx.SetXODR(&mxodr);
 }
 
 class roadx
@@ -986,6 +988,35 @@ void signal_handler(int sig)
     }
 }
 
+void ProcROIReq(std::shared_ptr<char> pstr_req,const int nreqsize,std::shared_ptr<char> & pstr_res,int & nressize)
+{
+    (void)pstr_req;
+    (void)nreqsize;
+    std::shared_ptr<iv::roi::request> pstr_roireq = std::shared_ptr<iv::roi::request>(new iv::roi::request);
+    if(pstr_roireq->ParseFromArray(pstr_req.get(),nreqsize))
+    {
+        std::shared_ptr<iv::roi::resultarray> pstr_roires;
+        gsrx.GetROI(pstr_roireq,pstr_roires);
+        int nbytesize = pstr_roires->ByteSize();
+        pstr_res = std::shared_ptr<char>(new char[nbytesize]);
+        if(pstr_roires->SerializeToArray(pstr_res.get(),nbytesize))
+        {
+            nressize = nbytesize;
+            std::cout<<"return res."<<std::endl;
+            return;
+        }
+        else
+        {
+            std::cout<<" ProcROIReq fail serizlizetoarray"<<std::endl;
+        }
+    }
+    else
+    {
+        std::cout<<" ProcROIReq parse pstr_req fail."<<std::endl;
+        return;
+    }
+}
+
 int main(int argc, char *argv[])
 {
     showversion("driver_map_xodrload");
@@ -1046,6 +1077,9 @@ int main(int argc, char *argv[])
 
     LoadXODR(strmapth);
 
+    iv::service::Server serviceroi("ServiceROI",ProcROIReq);
+    (void)serviceroi;
+
     gpmap = iv::modulecomm::RegisterSend("tracemap",20000000,1);
     gpasimple = iv::modulecomm::RegisterSend("simpletrace",100000,1);
     gpanewtrace = iv::modulecomm::RegisterSend("newtracemap",20000000,1);

src/driver/driver_map_xodrload/service_roi_xodr.cpp

@@ -15,9 +15,18 @@ void service_roi_xodr::SetXODR(OpenDrive *pxodr)
     updateroadarea();
 }
 
-int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::shared_ptr<iv::roi::resultarray> &xres_prt)
+
+#include <QTime>
+int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::shared_ptr<iv::roi::resultarray> &xres_ptr)
 {
 
+    xres_ptr = std::shared_ptr<iv::roi::resultarray>(new iv::roi::resultarray);
+    xres_ptr->set_lat(xreq_ptr->lat());
+    xres_ptr->set_lon(xreq_ptr->lon());
+    xres_ptr->set_heading(xreq_ptr->heading());
+    xres_ptr->set_range(xreq_ptr->range());
+    xres_ptr->set_gridsize(xreq_ptr->gridsize());
+
     double x0,y0;
     double x,y;
     GaussProjCal(xreq_ptr->lon(),xreq_ptr->lat(),&x,&y);
@@ -26,7 +35,13 @@ int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::sh
     y = y - y0;
     unsigned int nroadsize = mvectorarea.size();
     unsigned int i;
+    unsigned int j;
     std::vector<iv::roi::area> xvectorarea;
+
+    QTime xTime;
+    xTime.start();
+
+    //Find Near ROI area
     for(i=0;i<nroadsize;i++)
     {
         double fdis1;
@@ -37,7 +52,7 @@ int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::sh
             continue;
         }
         unsigned int nareasize = proadarea->mvectorarea.size();
-        unsigned int j;
+
         for(j=0;j<nareasize;j++)
         {
             iv::roi::area * parea = &proadarea->mvectorarea[j];
@@ -67,6 +82,46 @@ int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::sh
             }
         }
     }
+
+    std::cout<<"xTime: "<<xTime.elapsed()<<std::endl;
+
+    if(xreq_ptr->gridsize() <= 0.0)
+    {
+        return -1;  //grid size is error
+    }
+
+    unsigned int xarea = (unsigned int)(xreq_ptr->range()*2.0/xreq_ptr->gridsize()) + 1;
+    unsigned int yarea = (unsigned int)(xreq_ptr->range()*2.0/xreq_ptr->gridsize()) + 1;
+
+    std::vector<iv::roi::Point> xvectorpoint;
+    for(i=0;i<xarea;i++)
+    {
+        for(j=0;j<yarea;j++)
+        {
+            double xr = i*xreq_ptr->gridsize() - xreq_ptr->range();
+            double yr = j*xreq_ptr->gridsize() - xreq_ptr->range();
+            double hdg_o = (90-xreq_ptr->heading())*M_PI/180.0;
+            double rel_x = xr * cos(hdg_o) - yr * sin(hdg_o);
+            double rel_y = xr * sin(hdg_o) + yr * cos(hdg_o);
+            if(CheckPointInROI(rel_x,rel_y,xvectorarea))
+            {
+                iv::roi::Point xpoint;
+                xpoint.x = rel_x;
+                xpoint.y = rel_y;
+                xvectorpoint.push_back(xpoint);
+            }
+        }
+    }
+
+    std::cout<<"xTime: "<<xTime.elapsed()<<std::endl;
+
+    for(i=0;i<xvectorpoint.size();i++)
+    {
+        iv::roi::resultunit * padd = xres_ptr->add_res();
+        padd->set_x(xvectorpoint[i].x);
+        padd->set_y(xvectorpoint[i].y);
+    }
+
     return 0;
 }
 
@@ -155,6 +210,7 @@ iv::roi::roadarea service_roi_xodr::GetRoadArea(Road *pRoad, const double fstep)
             xarea.refPoint.x = x;
             xarea.refPoint.y = y;
             xarea.s = snow;
+            xarea.fmaxlen = fleftwidth + frigthwidth;
             xroadarea.mvectorarea.push_back(xarea);
 //            double fleftwidth = pRoad->GetRoadLeftWidth(snow);
 //            double frigthwidth = pRoad->GetRoadRightWidth(snow);
@@ -190,3 +246,36 @@ iv::roi::roadarea service_roi_xodr::GetRoadArea(Road *pRoad, const double fstep)
     return xroadarea;
 }
 
+bool service_roi_xodr::CheckPointInArea(double x, double y, iv::roi::area &xarea)
+{
+    double a = (xarea.P2.x - xarea.P1.x)*(y - xarea.P1.y) - (xarea.P2.y-xarea.P1.y)*(x-xarea.P1.x);
+    double b = (xarea.P3.x - xarea.P2.x)*(y - xarea.P2.y) - (xarea.P3.y-xarea.P2.y)*(x-xarea.P2.x);
+    double c = (xarea.P4.x - xarea.P3.x)*(y - xarea.P3.y) - (xarea.P4.y-xarea.P3.y)*(x-xarea.P3.x);
+    double d = (xarea.P1.x - xarea.P4.x)*(y - xarea.P4.y) - (xarea.P1.y-xarea.P4.y)*(x-xarea.P4.x);
+
+    if((a > 0 && b > 0 && c > 0 && d > 0) || (a < 0 && b < 0 && c < 0 && d < 0)) {
+        return true;
+    }
+    return false;
+}
+
+bool service_roi_xodr::CheckPointInROI(double x, double y, std::vector<iv::roi::area> &xvectorroi)
+{
+    unsigned int i;
+    unsigned nsize = xvectorroi.size();
+    for(i=0;i<nsize;i++)
+    {
+        iv::roi::area xarea = xvectorroi[i];
+        double fdis = sqrt(pow(xarea.refPoint.x-x,2)+pow(xarea.refPoint.y-y,2));
+        if(fdis>(xarea.fmaxlen+0.1))
+        {
+            continue;
+        }
+        if(CheckPointInArea(x,y,xarea))
+        {
+            return true;
+        }
+    }
+    return false;
+}
+

src/driver/driver_map_xodrload/service_roi_xodr.h

@@ -26,6 +26,7 @@ struct area
     Point P2;
     Point P3;
     Point P4;
+    double fmaxlen;
 
 };
 
@@ -58,12 +59,16 @@ public:
      * @param xres_prt  result
      * @return 0 Succeed -1 No Map  -2 Not in Map
      */
-    int GetROI(std::shared_ptr<iv::roi::request> xreq_ptr,std::shared_ptr<iv::roi::resultarray> & xres_prt );
+    int GetROI(std::shared_ptr<iv::roi::request> xreq_ptr,std::shared_ptr<iv::roi::resultarray> & xres_ptr );
 
 private:
     void updateroadarea();
     iv::roi::roadarea GetRoadArea(Road * pRoad,const double fstep);
 
+    bool CheckPointInROI(double x,double y, std::vector<iv::roi::area> & xvectorroi);
+
+    bool CheckPointInArea(double x, double y,iv::roi::area & xarea);
+
 private:
     OpenDrive * mpxodr = NULL;
     std::vector<iv::roi::roadarea> mvectorarea;