change points_filter_nan. and change map_lanetoxodr, change nds data geofit.

src/ros/catkin/src/points_filter_nan/CMakeLists.txt

@@ -81,6 +81,13 @@ include_directories(
 )
 
 ## Declare a C++ executable
-add_executable(${PROJECT_NAME}_node src/main.cpp src  )
-target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES}  ${OpenCV_LIBRARIES}    ${Protobuf_LIBRARIES})
+add_executable(points_filter_nan src/main.cpp src  )
+target_link_libraries(points_filter_nan ${catkin_LIBRARIES}  ${OpenCV_LIBRARIES}    ${Protobuf_LIBRARIES})
 
+
+install(
+  TARGETS points_filter_nan
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

src/ros/catkin/src/points_filter_nan/launch/points_filter_nan.launch

@@ -3,7 +3,7 @@
   <node pkg="points_filter_nan" type="points_filter_nan" name="points_filter_nan"  output="screen">
     <param name="points_topic" value="/points_raw"/>
     <param name="sensor_topic" value="/points_raw_sensor"/>
-    <param name="rotate" value="0"/>  
+    <param name="rotate" value="8"/>  
   </node>
 
 </launch>

src/ros/catkin/src/points_filter_nan/src/main.cpp

@@ -27,19 +27,22 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr &input)
 
     pcl::fromROSMsg(*input, point_cloud);
 
-    int pointsize = point_cloud.width;
+    int pointsize = point_cloud.points.size();
     int i;
     int nskip = 0;
 
         pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_filter(
                 new pcl::PointCloud<pcl::PointXYZI>());
     point_cloud_filter->header = point_cloud.header;
+	point_cloud_filter->height = 1;
+	point_cloud_filter->width = 0;
+ //   point_cloud_filter->header.stamp = point_cloud.header.stamp;
     for(i=0;i<pointsize;i++)
     {
         double x,y,z;
-        x = point_cloud.at(i).x;
-        y = point_cloud.at(i).y;
-        z = point_cloud.at(i).z;
+        x = point_cloud.points.at(i).x;
+        y = point_cloud.points.at(i).y;
+        z = point_cloud.points.at(i).z;
         point_cloud.points.at(i);
         if(std::isnan(x) ||(std::isnan(y))||std::isnan(z))
         {
@@ -53,10 +56,12 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr &input)
         }
 
         pcl::PointXYZI point;
+	double theta = yawrotate*M_PI/180.0;
         if(yawrotate != 0)
         {
-                double s = x*cos(yawrotate*M_PI/180.0) - y*sin(yawrotate*M_PI/180.0);
-                double t = x*sin(yawrotate*M_PI/180.0)+y*cos(yawrotate*M_PI/180.0);
+//		std::cout<<"rotate"<<std::endl;
+                double s = x*cos(theta) - y*sin(theta);
+                double t = x*sin(theta)+y*cos(theta);
                 x = s;
                 y= t;
         }
@@ -69,7 +74,7 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr &input)
     }
 
     std::cout<<" skip nan point count: "<<nskip<<" out point count: "<<point_cloud_filter->width<<std::endl;
-     point_cloud_pub.publish(point_cloud);
+     point_cloud_pub.publish(point_cloud_filter);
 }
 
 

src/tool/map_lanetoxodr/ndsdataproc.cpp

@@ -59,6 +59,7 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
             xline.lanewidth = badata[9].toDouble();
             xline.laneheading = badata[10].toDouble();
             xline.lanecurv = badata[11].toDouble();
+            xline.nlanetype = badata[23].toDouble();
             if(xline.feature >=0)xvectorline.push_back(xline);
 
         }
@@ -219,6 +220,41 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
 //        if(fdis<0.6)std::cout<<" index "<<xvectorvl[i].mvehindex<<" dis "<<fdis<<" time "<<xvectorvehicle[xvectorvl[i].mvehindex].localtime<<std::endl;
     }
 
+    for(i=0;i<nvlsize;i++)
+    {
+        iv::nds_vehicle *pv1 = &xvectorvehicle[xvectorvl[i].mvehindex];
+        double fhdg = (90-pv1->heading)*M_PI/360.0;
+        if(fhdg<0)fhdg = fhdg + 2.0*M_PI;
+        pv1->fhdg = fhdg;
+        iv::nds_line * pline1 = &xvectorline[xvectorvl[i].mvectorlineindex[0]];
+        iv::nds_line * pline2 = NULL;
+        if(xvectorvl[i].mvectorlineindex.size()>=2)
+        {
+            pline2 = &xvectorline[xvectorvl[i].mvectorlineindex[1]];
+        }
+        pv1->fx = pv1->frel_x + (pline1->lanewidth/2.0 - pline1->centerdeparture)*cos(fhdg + M_PI/2.0);
+        pv1->fy = pv1->frel_y + (pline1->lanewidth/2.0 - pline1->centerdeparture)*sin(fhdg + M_PI/2.0);
+        if(pline1->feature == 1)
+        {
+            pv1->nleftlanetype = pline1->nlanetype;
+        }
+        if(pline1->feature == 2)
+        {
+            pv1->nrightlanetype = pline1->nlanetype;
+        }
+        if(pline2 != NULL)
+        {
+            if(pline2->feature == 1)
+            {
+                pv1->nleftlanetype = pline2->nlanetype;
+            }
+            if(pline2->feature == 2)
+            {
+                pv1->nrightlanetype = pline2->nlanetype;
+            }
+        }
+    }
+
 
 
     std::cout<<" fdis "<<fS<<" min "<<fdismin<<" max "<<fdismax<<std::endl;
@@ -235,6 +271,10 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
 //    double fXLast;
 //    double fYLast;
 //    bool bFirst = true;
+    bool bHaveLastxyhdg = false;
+    double fLastX,fLastY,fLastHdg;
+    double fEndX,fEndY,fEndHdg;
+    double fStartX,fStartY,fStartHdg;
     while(bComplete == false)
     {
 
@@ -242,8 +282,8 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
         VectorXd y_veh(nrange);
         for(j=0;j<nrange;j++)
         {
-            x_veh[j] = xvectorvehicle[xvectorvl[j+ncurpos].mvehindex].frel_x;//mvectorrtkdata.at(j+ncurpos).mfrelx;
-            y_veh[j] = xvectorvehicle[xvectorvl[j+ncurpos].mvehindex].frel_y;//mvectorrtkdata.at(j+ncurpos).mfrely;
+            x_veh[j] = xvectorvehicle[xvectorvl[j+ncurpos].mvehindex].fx;//mvectorrtkdata.at(j+ncurpos).mfrelx;
+            y_veh[j] = xvectorvehicle[xvectorvl[j+ncurpos].mvehindex].fy;//mvectorrtkdata.at(j+ncurpos).mfrely;
         }
 
         bool bArcOk = false;
@@ -282,8 +322,6 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
             std::cout<<" a line is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
         }
 
-
-
         if((nrange <= 2)||bLineOk||bArcOk)
         {
             if(bLineOk)
@@ -306,6 +344,12 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
                 xgeo.mfY = y0;
                 xgeo.mfLen = sqrt(pow(x1-x0,2)+pow(y1-y0,2));
                 xgeo.mnType = 0;
+                fStartX = x0;
+                fStartX = y0;
+                fStartHdg = xgeo.mfHdg;
+                fEndX = x1;
+                fEndY = y1;
+                fEndHdg = fStartHdg;
 //                if(bFirst)
 //                {
 //                    xgeo.mfX = x_veh[0];
@@ -339,6 +383,12 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
                     xgeo.mfEndX = ep.x();
                     xgeo.mfEndY = ep.y();
                     xgeo.mR = fR;
+                    fStartX = xgeo.mfX;
+                    fStartY = xgeo.mfY;
+                    fStartHdg = xgeo.mfHdgStart;
+                    fEndX = xgeo.mfEndX;
+                    fEndY = xgeo.mfEndY;
+                    fEndHdg = xgeo.mfHdgEnd;
 //                    if(bFirst)
 //                    {
 //                        xgeo.mfX = x_veh[0];
@@ -360,13 +410,33 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
                 }
             }
             //        std::cout<<" a line is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
-            ncurpos = ncurpos + nrange -1;
+
+            if(bHaveLastxyhdg)
+            {
+                std::vector<geobase> xvectorgeobe = geofit::CreateBezierGeo(fLastX,fLastY,
+                                                                          fLastHdg,fStartX,fStartY,fStartHdg);
+                if(xvectorgeobe.size()>0)
+                {
+                    xvectorgeo.insert(xvectorgeo.begin()+(xvectorgeo.size()-1),xvectorgeobe[0]);
+                }
+            }
+            bHaveLastxyhdg = true;
+            fLastX = fEndX;
+            fLastY = fEndY;
+            fLastHdg = fEndHdg;
+
+
+ //           ncurpos = ncurpos + nrange -1;
+            ncurpos = ncurpos + nrange;  //Skip 1 point, use bezier
             nrange = xvectorvl.size() - ncurpos;
 
         }
         else
         {
-            nrange = nrange/2;
+            if(nrange > 30)
+                nrange = nrange/2;
+            else
+                nrange = nrange -1;
             if(nrange<2)nrange = 2;
         }
         if(ncurpos>=(xvectorvl.size()-1))bComplete = true;

src/tool/map_lanetoxodr/ndsdataproc.h

@@ -20,6 +20,13 @@ struct  nds_vehicle
     double frel_x;
     double frel_y;
 
+    double fx;
+    double fy;
+    double fhdg;
+    int nleftlanetype=1; //1 no define  2 dash 3 solid  4 dash dash 5 solid solid 6 solid dash 7 dash solid 8 luyan 9 hulan 10 zhangaiwubianjie
+    int nrightlanetype=1;
+    double flanewidth = 0;
+
 };
 
 struct nds_line
@@ -30,6 +37,7 @@ struct nds_line
     double lanewidth;
     double laneheading;
     double lanecurv;
+    int nlanetype = 1;
 };
 
 struct nds_vl