change driver_map_xodrload. fix plan error.

src/driver/driver_lidar_leishen32/leishen32.cpp

@@ -28,7 +28,7 @@ void leishen32::threaddecode()
 
 }
 
-int leishen32::DecodeUDPPac(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,iv::lidarudppac & xpac)
+int leishen32::DecodeUDPPac(iv::lidarudppac & xpac)
 {
     if(xpac.mndatasize != 1206)
     {
@@ -62,6 +62,21 @@ int leishen32::DecodeUDPPac(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,i
 
     char * pstr =  static_cast<char * >(xpac.mdata_ptr.get()) ;
 
+    static double gAngleTotal = 0;
+    static double gAngleOld = 0;
+    static unsigned int g_seq = 0;
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+
+    point_cloud->header.frame_id = "velodyne";
+    point_cloud->height = 1;
+    point_cloud->header.stamp = static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count()/1000);
+    point_cloud->width = 0;
+    point_cloud->header.seq =g_seq;
+    g_seq++;
+    mpoint_cloud_temp = point_cloud;
+
     int i;
     for(i=0;i<12;i++)
     {
@@ -72,6 +87,33 @@ int leishen32::DecodeUDPPac(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,i
         fazu = fazu * M_PI/180.0;
         pcl::PointXYZI point;
 
+
+        if(fabs(fazu-gAngleOld)>=300)
+        {
+            gAngleTotal = gAngleTotal + fabs(fabs(fazu-gAngleOld)-360);
+        }
+        else
+        {
+            gAngleTotal = gAngleTotal + fabs(fabs(fazu-gAngleOld));
+        }
+        gAngleOld = fazu;
+
+        if(gAngleTotal>=360.0)
+        {
+            //share point cloud.
+
+            pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                        new pcl::PointCloud<pcl::PointXYZI>());
+
+            point_cloud->header.frame_id = "velodyne";
+            point_cloud->height = 1;
+            point_cloud->header.stamp = static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count()/1000);
+            point_cloud->width = 0;
+            point_cloud->header.seq =g_seq;
+            g_seq++;
+            mpoint_cloud_temp = point_cloud;
+        }
+
         int j;
         for(j=0;j<32;j++)
         {

src/driver/driver_lidar_leishen32/leishen32.h

@@ -47,7 +47,7 @@ private:
 
 private:
     void threaddecode( );
-    int DecodeUDPPac(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,iv::lidarudppac & xpac);
+    int DecodeUDPPac(iv::lidarudppac & xpac);
     int DecodeDevinfo(iv::lidarudppac & xpac);
 
 

src/driver/driver_map_xodrload/globalplan.cpp

@@ -704,9 +704,10 @@ int nmode = 0; //0 same direciion dis zero   1 oposite dis = 0 2 same direction
 int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDrive * pxodr,std::vector<iv::nearroad> & xvectornear,
                          const double nearthresh,bool bhdgvalid = true)
 {
+    std::cout<<" near thresh "<<nearthresh<<std::endl;
     int i;
     double frels;
-    int nminmode = 5;
+    int nminmode = 6;
     for(i=0;i<pxodr->GetRoadCount();i++)
     {
         int j;
@@ -722,10 +723,11 @@ int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDriv
         double locals = 0;
         double localfrels = 0;
         GeometryBlock * pgeolocal;
+        localdismin = std::numeric_limits<double>::infinity();
 
         for(j=0;j<proad->GetGeometryBlockCount();j++)
         {
-            localdismin = std::numeric_limits<double>::infinity();
+
             GeometryBlock * pgb = proad->GetGeometryBlock(j);
             double dis;
             RoadGeometry * pg;
@@ -769,6 +771,7 @@ int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDriv
 
         }
 
+        std::cout<<" local dismin "<<localdismin<<std::endl;
         if(localdismin < nearthresh)
         {
             iv::nearroad xnear;
@@ -908,15 +911,34 @@ int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDriv
                 }
             }
             if(xnear.nmode < nminmode)nminmode = xnear.nmode;
+
+            if(xnear.pObjRoad->GetLaneSectionCount()>0)
+            {
+                LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
+                if((pLS->GetRightLaneCount()>0)&& ((pLS->GetLeftLaneCount()==0)) && (xnear.lr == 1))
+                {
+                    xnear.lr = 2;
+                }
+                if((pLS->GetRightLaneCount() == 0)&& ((pLS->GetLeftLaneCount()>0)) && (xnear.lr == 2))
+                {
+                    xnear.lr = 1;
+                }
+            }
+
             xvectornear.push_back(xnear);
             }
         }
 
     }
 
-    if(xvectornear.size() == 0)return -1;
+    if(xvectornear.size() == 0)
+    {
+        std::cout<<" no near. "<<std::endl;
+        return -1;
+    }
 
 
+//    std::cout<<" near size: "<<xvectornear.size()<<std::endl;
     if(xvectornear.size() > 1)
     {
         int i;
@@ -929,6 +951,7 @@ int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDriv
             }
         }
     }
+//    std::cout<<" after size: "<<xvectornear.size()<<std::endl;
     if((xvectornear.size()>1)&&(nminmode>=4))  //if dis > 5 select small dis
     {
         int i;
@@ -2864,6 +2887,9 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
         return -2;
     }
 
+    std::cout<<" start size: "<<xvectornearstart.size()<<std::endl;
+    std::cout<<" end size: "<<xvectornearend.size()<<std::endl;
+
     std::vector<std::vector<PlanPoint>> xvectorplans;
     std::vector<double> xvectorroutedis;
 
@@ -2882,6 +2908,8 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
             lr_start = pnearstart->lr;
             lr_end = pnearend->lr;
 
+            std::cout<<" lr start: "<<lr_start<<" lr end "<<lr_end<<std::endl;
+
             nearx = pnearstart->nearx;
             neary = pnearstart->neary;
 

src/driver/driver_map_xodrload/main.cpp

@@ -1240,7 +1240,7 @@ int main(int argc, char *argv[])
 //    SetPlan(xo);
 
 
-    void * pivexit = iv::ivexit::RegIVExitCall(ExitFunc);
+ //   void * pivexit = iv::ivexit::RegIVExitCall(ExitFunc);
 
     signal(SIGINT,signal_handler);
 

src/tool/tool_xodrobj/main.cpp

@@ -20,7 +20,7 @@ void ExitFunc()
 
 int main(int argc, char *argv[])
 {
-    iv::ivexit::RegIVExitCall(ExitFunc);
+//    iv::ivexit::RegIVExitCall(ExitFunc);
     RegisterIVBackTrace();
     showversion("tool_xodrobj");
     QApplication a(argc, argv);