change lidar_xodrroifilter. running ok.

src/driver/driver_lidar_xodrroifilter/lidarxodrroifilter.cpp

@@ -146,6 +146,7 @@ int LidarXODRROIFilter::FilterPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr po
 
     point_filter->width = 0;
     point_filter->height = 1;
+    point_filter->header = point_raw->header;
     int nPCount = point_raw->points.size();
     std::cout<<" nPCount: "<<nPCount<<std::endl;
     int nskip = 0;
@@ -173,7 +174,7 @@ int LidarXODRROIFilter::FilterPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr po
             xp.z = z;
 
             xp.intensity = intensity;
-            point_filter->push_back(xp);
+            point_filter->points.push_back(xp);
             ++point_filter->width;
         }
         else

src/driver/driver_map_xodrload/service_roi_xodr.cpp

@@ -94,25 +94,34 @@ int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::sh
 
     std::vector<iv::roi::Point> xvectorpoint;
     unsigned int nareasize = xvectorarea.size();
-    double fgpsyaw = (90-xreq_ptr->heading())*M_PI/180.0;
+    double fgpsyaw = (90-xreq_ptr->heading())*M_PI/180.0 -3.0*M_PI/2.0;
     for(i=0;i<nareasize;i++)
     {
         int ix,iy;
-        unsigned int ny = xvectorarea[i].fsteplen/fgridsize;
-        unsigned int nxl = 0;
-        unsigned int nxr = 0;
+        int ny = xvectorarea[i].fsteplen/fgridsize;
+        int nxl = 0;
+        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 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;
+//        fgpsyaw = 0;
         fxrel = frelxraw * cos(-fgpsyaw) - frelyraw * sin(-fgpsyaw);
-        fyrel = frelxraw * sin(-fgpsyaw) - frelyraw * cos(-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);
+//        iv::roi::Point xpoint;
+//        xpoint.x = fxbase;
+//        xpoint.y = fybase;
+//        xvectorpoint.push_back(xpoint);
+//        continue;
+
+        fyaw = fyaw + 3.0*M_PI/2.0;
         if((nxl == 0)&&(nxr== 0)&&(ny <= 1))
         {
             iv::roi::Point xpoint;
@@ -142,8 +151,9 @@ int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::sh
                         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);
+                        xpoint.y = fybase +fxlocal*sin(fyaw) + fylocal * cos(fyaw);
                         xvectorpoint.push_back(xpoint);
                     }
                 }

src/tool/pointcloudviewer/main.cpp

@@ -108,7 +108,10 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
     memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
     int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
     int i;
+    point_cloud->height = 1;
+    point_cloud->width = 0;
     pcl::PointXYZI * p;
+    std::cout<<" size : "<<nPCount<<std::endl;
     p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
     for(i=0;i<nPCount;i++)
     {
@@ -119,6 +122,7 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
         xp.intensity = p->intensity;
         point_cloud->push_back(xp);
         p++;
+        ++point_cloud->width;
 
   //      std::cout<<" x is "<<xp.x<<" y is "<<xp.y<<std::endl;
     }
@@ -186,6 +190,7 @@ int main(int argc, char *argv[])
     QCoreApplication a(argc, argv);
 
     snprintf(gstr_memname,255,"lidar_pc");
+//    snprintf(gstr_memname,255,"lidar_pc_filter");
 //    snprintf(gstr_memname,255,"lidarpc_center");
 
 

src/tool/tool_trace2vectormap/trace2vectormap.cpp

@@ -162,6 +162,7 @@ int trace2vectormap::convert()
         xvectorlane[i].blid = xvectorlane[i-1].lnid;
     }
 
+    bcircle = false;
     if(bcircle)
     {
         iv::vectormap::lane xlane;