change roi filter, but not complete.

src/driver/driver_lidar_xodrroifilter/driver_lidar_xodrroifilter.pro

@@ -34,6 +34,11 @@ else: unix:!android: target.path = /opt/$${TARGET}/bin
     error( "Couldn't find the ivboost.pri file!" )
 }
 
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
+
+
 HEADERS += \
     ../../include/msgtype/gpsimu.pb.h \
     ../../include/msgtype/roireqest.pb.h \

src/driver/driver_lidar_xodrroifilter/lidarxodrroifilter.cpp

@@ -36,6 +36,12 @@ void LidarXODRROIFilter::threadroi()
         mwc.wait(&mwaitmutex,100);
         mwaitmutex.unlock();
 
+        //if dis extend 1 meter, roi area fail.
+        if((fabs(mgpsimu.lat() - moldgpsimu.lat()) > 0.0000100)||(fabs(mgpsimu.lon() - moldgpsimu.lon()) > 0.0000100))
+        {
+            mbroiexist = false;
+        }
+
         bool breq = false;
         if(mbroiexist == false)
         {
@@ -52,11 +58,11 @@ void LidarXODRROIFilter::threadroi()
         if(breq)
         {
             iv::roi::request xreq;
-            xreq.set_gridsize(0.2);
+            xreq.set_gridsize(mfgridsize);
             xreq.set_heading(mgpsimu.heading());
             xreq.set_lat(mgpsimu.lat());
             xreq.set_lon(mgpsimu.lon());
-            xreq.set_range(100.0);
+            xreq.set_range(mfRange);
 
             int nbytesize = xreq.ByteSize();
             std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);
@@ -64,11 +70,19 @@ void LidarXODRROIFilter::threadroi()
             int ndatasize;
             std::shared_ptr<char> pstrvt_ptr;
 
-            if(xclienROI.SendRequest(pstr_ptr,nbytesize,pstrvt_ptr,ndatasize,10000) == iv::service::Client::HAVE_RES)
+            if(xclienROI.SendRequest(pstr_ptr,nbytesize,pstrvt_ptr,ndatasize,100) == iv::service::Client::HAVE_RES)
             {
                 std::cout<<" size: "<< ndatasize<<std::endl;
-                mbroiexist = true;
-                moldgpsimu.CopyFrom(mgpsimu);
+                iv::roi::resultarray xresultarray;
+                if(xresultarray.ParseFromArray(pstrvt_ptr.get(),ndatasize))
+                {
+                    mMutex.lock();
+                    mroiarray.CopyFrom(xresultarray);
+                    mMutex.unlock();
+                    mbroiexist = true;
+                    moldgpsimu.CopyFrom(mgpsimu);
+                }
+
             }
             else
             {
@@ -82,3 +96,92 @@ void LidarXODRROIFilter::threadroi()
 
     }
 }
+
+int LidarXODRROIFilter::FilterPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr point_raw,pcl::PointCloud<pcl::PointXYZI>::Ptr & point_filter)
+{
+    if(mbroiexist == false)
+    {
+        std::cout<<" Waring, No ROI Info."<<std::endl;
+        point_filter = point_raw;
+        return 0;
+    }
+
+    int nr = mfRange/mfgridsize;
+    int ndim = 2*(mfRange/mfgridsize) + 1;
+
+    bool *pbValid;
+    std::shared_ptr<bool> pbValid_ptr = std::shared_ptr<bool>(new bool[ndim * ndim]);
+    pbValid = pbValid_ptr.get();
+
+    int i;
+    int j;
+    for(i=0;i<ndim;i++)
+    {
+        for(j=0;j<ndim;j++)
+        {
+            pbValid[i*ndim +j] = false;
+        }
+    }
+
+    iv::roi::resultarray xresarray;
+    mMutex.lock();
+    xresarray.CopyFrom(mroiarray);
+    mMutex.unlock();
+
+    int npointsize = xresarray.res_size();
+
+    for(i=0;i<npointsize;i++)
+    {
+        double x,y;
+        x = -xresarray.mutable_res(i)->y();
+        y = xresarray.mutable_res(i)->x();
+        int pos_x = x/mfgridsize + nr;
+        int pos_y = y/mfgridsize + nr;
+        if((pos_x<0)||(pos_y<0)||(pos_x>=ndim)||(pos_y>=ndim))
+        {
+            continue;
+        }
+        pbValid[pos_y*ndim + pos_x] = true;
+    }
+
+    point_filter->width = 0;
+    point_filter->height = 1;
+    int nPCount = point_raw->points.size();
+    std::cout<<" nPCount: "<<nPCount<<std::endl;
+    int nskip = 0;
+    for(i=0;i<nPCount;i++)
+    {
+        double x,y,z,intensity;
+        x = point_raw->points.at(i).x;
+        y = point_raw->points.at(i).y;
+        z = point_raw->points.at(i).z;
+        intensity = point_raw->points.at(i).intensity;
+
+        int pos_x = x/mfgridsize + nr;
+        int pos_y = y/mfgridsize + nr;
+        if((pos_x<0)||(pos_y<0)||(pos_x>=ndim)||(pos_y>=ndim))
+        {
+            nskip++;
+            continue;
+        }
+
+        if(pbValid[pos_y*ndim + pos_x])
+        {
+            pcl::PointXYZI xp;
+            xp.x = x;
+            xp.y = y;
+            xp.z = z;
+
+            xp.intensity = intensity;
+            point_filter->push_back(xp);
+            ++point_filter->width;
+        }
+        else
+        {
+            nskip++;
+        }
+    }
+
+    std::cout<<" point filter size : "<<point_filter->width<<" skip: "<<nskip<<std::endl;
+    return 1;
+}

src/driver/driver_lidar_xodrroifilter/lidarxodrroifilter.h

@@ -11,6 +11,9 @@
 #include <QMutex>
 #include <QWaitCondition>
 
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
 class LidarXODRROIFilter
 {
 public:
@@ -20,6 +23,8 @@ public:
 public:
     void SetGPSIMU(iv::gps::gpsimu & xgpsimu);
 
+    int FilterPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr point_raw,pcl::PointCloud<pcl::PointXYZI>::Ptr & point_filter);
+
 private:
     bool mbGPSSet = false;
     bool mbroiexist = false;
@@ -33,6 +38,13 @@ private:
 
     QMutex mwaitmutex;
     QWaitCondition mwc;
+
+    QMutex mMutex;
+
+    iv::roi::resultarray mroiarray;
+
+    double mfgridsize = 0.5;
+    double mfRange = 100.0;
 };
 
 #endif // LIDARXODRROIFILTER_H

src/driver/driver_lidar_xodrroifilter/main.cpp

@@ -12,6 +12,8 @@
 
 LidarXODRROIFilter gLXRF;
 
+void * gpalidar;
+
 void ListenGPS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
 
@@ -31,6 +33,69 @@ void ListenGPS(const char * strdata,const unsigned int nSize,const unsigned int
 
 }
 
+void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    QTime xTime;
+    xTime.start();
+
+    if(nSize <=16)return;
+    unsigned int * pHeadSize1 = (unsigned int *)strdata;
+    if(*pHeadSize1 > nSize)
+    {
+        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize1<<"  data size is"<<nSize<<std::endl;
+    }
+
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+    int nNameSize;
+    nNameSize = *pHeadSize1 - 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 - *pHeadSize1)/sizeof(pcl::PointXYZI);
+    int i;
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize1);
+    point_cloud->width = 0;
+    point_cloud->height = 1;
+    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);
+        point_cloud->width++;
+        p++;
+    }
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_filter(
+                new pcl::PointCloud<pcl::PointXYZI>());
+
+    gLXRF.FilterPointCloud(point_cloud,point_cloud_filter);
+
+    char * strOut = new char[4+sizeof(point_cloud_filter->header.frame_id.size()) + 4+8+point_cloud_filter->width * sizeof(pcl::PointXYZI)];
+
+    int * pHeadSize = (int *)strOut;
+    *pHeadSize = 4 + point_cloud_filter->header.frame_id.size()+4+8;
+    memcpy(strOut+4,point_cloud_filter->header.frame_id.c_str(),point_cloud_filter->header.frame_id.size());
+    pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud_filter->header.frame_id.size()));
+    *pu32 = point_cloud_filter->header.seq;
+    memcpy(strOut+4+sizeof(point_cloud_filter->header.frame_id.size()) + 4,&point_cloud_filter->header.stamp,8);
+//    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)(strOut +4+sizeof(point_cloud_filter->header.frame_id.size()) + 4+8 );
+    memcpy(p,point_cloud_filter->points.data(),point_cloud_filter->width * sizeof(pcl::PointXYZI));
+
+    iv::modulecomm::ModuleSendMsg(gpalidar,strOut,4+sizeof(point_cloud_filter->header.frame_id.size()) + 4+8+point_cloud_filter->width * sizeof(pcl::PointXYZI));
+    delete strOut;
+}
+
+
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
@@ -38,5 +103,12 @@ int main(int argc, char *argv[])
     void * pa = iv::modulecomm::RegisterRecv("hcp2_gpsimu",ListenGPS);
     (void)pa;
 
+    std::string strpointname = "lidar_pc";
+
+    void * pb = iv::modulecomm::RegisterRecv(strpointname.data(),ListenPointCloud);
+    (void)pb;
+
+    gpalidar = iv::modulecomm::RegisterSend("lidar_pc_filter",6000000,1);
+
     return a.exec();
 }

src/driver/driver_map_xodrload/service_roi_xodr.cpp

@@ -90,34 +90,74 @@ int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::sh
         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;
+    double fgridsize = xreq_ptr->gridsize()/2.0;
 
     std::vector<iv::roi::Point> xvectorpoint;
-    for(i=0;i<xarea;i++)
+    unsigned int nareasize = xvectorarea.size();
+    double fgpsyaw = (90-xreq_ptr->heading())*M_PI/180.0;
+    for(i=0;i<nareasize;i++)
     {
-        for(j=0;j<yarea;j++)
+        int ix,iy;
+        unsigned int ny = xvectorarea[i].fsteplen/fgridsize;
+        unsigned int nxl = 0;
+        unsigned int nxr = 0;
+        if(xvectorarea[i].fleftwidth>0)nxl = xvectorarea[i].fleftwidth/fgridsize;
+        if(xvectorarea[i].frightwidth>0)nxr = xvectorarea[i].frightwidth/fgridsize;
+        double fyaw = xvectorarea[i].fhdg - fgpsyaw;
+        double flaneoff = xvectorarea[i].flaneoff;
+        if(fyaw < 0)fyaw = fyaw + 2.0*M_PI;
+        double frelxraw = xvectorarea[i].refPoint.x - x;
+        double frelyraw = xvectorarea[i].refPoint.y - y;
+        double fxrel,fyrel;
+        fxrel = frelxraw * cos(-fgpsyaw) - frelyraw * sin(-fgpsyaw);
+        fyrel = frelxraw * sin(-fgpsyaw) - frelyraw * cos(-fgpsyaw);
+        double fxbase = fxrel + flaneoff * cos(fyaw + M_PI/2.0);
+        double fybase = fyrel + flaneoff * sin(fyaw + M_PI/2.0);
+        if((nxl == 0)&&(nxr== 0)&&(ny <= 1))
         {
-            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 = fxbase;
+            xpoint.y = fybase;
+            xvectorpoint.push_back(xpoint);
+        }
+        else
+        {
+            if((nxl == 0)&&(nxr== 0))
             {
-                iv::roi::Point xpoint;
-                xpoint.x = rel_x;
-                xpoint.y = rel_y;
-                xvectorpoint.push_back(xpoint);
+                for(iy=(-ny);iy<=0;iy++)
+                {
+                    iv::roi::Point xpoint;
+                    xpoint.x = fxbase +(iy*fgridsize)*cos(fyaw);
+                    xpoint.y = fybase +(iy*fgridsize)*sin(fyaw);
+                    xvectorpoint.push_back(xpoint);
+                }
+            }
+            else
+            {
+                for(ix=(-nxl);ix<=nxr;ix++)
+                {
+                    for(iy=(-ny);iy<=0;iy++)
+                    {
+                        double fxlocal,fylocal;
+                        fxlocal = ix*fgridsize;
+                        fylocal = iy*fgridsize;
+                        iv::roi::Point xpoint;
+                        xpoint.x = fxbase +fxlocal*cos(fyaw) - fylocal * sin(fyaw);
+                        xpoint.y = fybase +fxlocal*sin(fyaw) - fylocal * cos(fyaw);
+                        xvectorpoint.push_back(xpoint);
+                    }
+                }
             }
         }
     }
 
-    std::cout<<"xTime: "<<xTime.elapsed()<<std::endl;
+
+    std::cout<<"xTime: "<<xTime.elapsed()<<" size : "<<xvectorpoint.size()<< std::endl;
 
     for(i=0;i<xvectorpoint.size();i++)
     {
         iv::roi::resultunit * padd = xres_ptr->add_res();
+//        std::cout<<" x: "<<xvectorpoint[i].x<<" y: "<<xvectorpoint[i].y<<std::endl;
         padd->set_x(xvectorpoint[i].x);
         padd->set_y(xvectorpoint[i].y);
     }
@@ -131,7 +171,7 @@ void service_roi_xodr::updateroadarea()
     mvectorarea.clear();
     unsigned int nroadcount = pxodr->GetRoadCount();
     unsigned int i;
-    const double fstep = 0.1;
+    double fstep = GEOSAMPLESTEP;
     for(i=0;i<nroadcount;i++)
     {
         Road * pRoad = pxodr->GetRoad(i);
@@ -211,6 +251,11 @@ iv::roi::roadarea service_roi_xodr::GetRoadArea(Road *pRoad, const double fstep)
             xarea.refPoint.y = y;
             xarea.s = snow;
             xarea.fmaxlen = fleftwidth + frigthwidth;
+            xarea.fhdg = yaw;
+            xarea.fleftwidth = fleftwidth;
+            xarea.frightwidth = frigthwidth;
+            xarea.flaneoff = flaneoff;
+            xarea.fsteplen = fstep;
             xroadarea.mvectorarea.push_back(xarea);
 //            double fleftwidth = pRoad->GetRoadLeftWidth(snow);
 //            double frigthwidth = pRoad->GetRoadRightWidth(snow);
@@ -221,8 +266,9 @@ iv::roi::roadarea service_roi_xodr::GetRoadArea(Road *pRoad, const double fstep)
             snow = snow + fstep;
 
         }
-        if(snow < (endS - 0.000001))
+        if((snow-fstep) < (endS - 0.000001))
         {
+            double foldsnow = snow - fstep;
             snow = endS - 0.000001;
             fleftwidth = pRoad->GetRoadLeftWidth(snow);
             frigthwidth = pRoad->GetRoadRightWidth(snow);
@@ -240,6 +286,11 @@ iv::roi::roadarea service_roi_xodr::GetRoadArea(Road *pRoad, const double fstep)
             xarea.refPoint.x = x;
             xarea.refPoint.y = y;
             xarea.s = snow;
+            xarea.fhdg = yaw;
+            xarea.fleftwidth = fleftwidth;
+            xarea.frightwidth = frigthwidth;
+            xarea.flaneoff = flaneoff;
+            xarea.fsteplen = endS - foldsnow;
             xroadarea.mvectorarea.push_back(xarea);
         }
     }

src/driver/driver_map_xodrload/service_roi_xodr.h

@@ -26,6 +26,11 @@ struct area
     Point P2;
     Point P3;
     Point P4;
+    double fhdg;
+    double fleftwidth;
+    double frightwidth;
+    double fsteplen;
+    double flaneoff;
     double fmaxlen;
 
 };
@@ -74,6 +79,7 @@ private:
     std::vector<iv::roi::roadarea> mvectorarea;
     double mlon0;
     double mlat0;
+    const double GEOSAMPLESTEP = 0.5;  // Get Road Unit at 0.5m
 
 };