Merge branch 'master' of http://127.0.0.1:3000/adc_pilot/modularization

sh/envInstall.sh

@@ -43,6 +43,7 @@ echo "nvidia" | sudo -S apt-get install python-rosinstall python-rosinstall-gene
 echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
 source ~/.bashrc
 echo "nvidia" | sudo -S apt-get install python-rosdep -y
+echo "nvidia" | sudo -S apt-get install --reinstall python3-pip
 echo "nvidia" | sudo -S pip3 install rosdepc 
 #echo "nvidia" | sudo -S sed -i '311i url="https://ghproxy.com/"+url' /usr/lib/python2.7/dist-packages/rosdep2/sources_list.py
 #echo "nvidia" | sudo -S sed -i '204i gbpdistro_url="https://ghproxy.com/"+gbpdistro_url' /usr/lib/python2.7/dist-packages/rosdep2/gbpdistro_support.py 

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/include/proto/cdadraw.proto

@@ -6,8 +6,9 @@ message cdalane
 {
 	required int32 lanemarkcolor	= 1; //车道线颜色  0 白色 1 黄色
 	required int32 lanemarktype 	= 2; //车道线类型    0 虚线 1 实线 2 双虚线  3 双实线  4 虚实线 5 实虚线  6 无
-	required double lanewidth	= 3; //车道线宽度
+	required double lanewidth	= 3; //车道宽度
 	required int32 lanetype		= 4; //车道类型 "shoulder","border","driving","stop","none","parking","biking","sidewalk","median"
+	required double lanemarkwidth	= 5; //
 };
 
 message cdageo

src/tool/map_lanetoxodr/dialogaddroadfromcda.cpp

@@ -81,6 +81,17 @@ DialogAddRoadFromCDA::~DialogAddRoadFromCDA()
 
 void DialogAddRoadFromCDA::on_pushButton_Create_clicked()
 {
+    OpenDrive * pxodr = mpxodr;
+    int nrtn = CDAProc::ProcCDA(mcdadraw,pxodr);
+
+    if(nrtn == 0)
+    {
+        this->accept();
+    }
+    else
+    {
+        QMessageBox::warning(this,tr("Warning"),tr("CDAProc ProcRoad Fail."),QMessageBox::YesAll);
+    }
 //    OpenDrive * pxodr = mpxodr;
 //    std::string strtype = ui->comboBox_Type->currentText().toStdString();
 //    std::string strradius = ui->lineEdit_radius->text().toStdString();
@@ -198,6 +209,9 @@ void DialogAddRoadFromCDA::on_pushButton_laneadd_clicked()
    plane->set_lanemarktype(ui->comboBox_lanemarktype->currentIndex());
    plane->set_lanewidth(flanewidth);
    plane->set_lanetype(ui->comboBox_lanetype->currentIndex());
+   double flanemarkwidth = ui->lineEdit_lanemarkwidth->text().toDouble();
+   if(flanemarkwidth<0.001)flanemarkwidth = 0.15;
+   plane->set_lanemarkwidth(flanemarkwidth);
 
    int nlanecount = static_cast<int>(pcdadraw->mlanes_size()) ;
    ui->comboBox_Lane->addItem(QString(tr("车道"))+QString::number(nlanecount));
@@ -270,6 +284,9 @@ void DialogAddRoadFromCDA::on_pushButton_lanechange_clicked()
     plane->set_lanemarktype(ui->comboBox_lanemarktype->currentIndex());
     plane->set_lanewidth(flanewidth);
     plane->set_lanetype(ui->comboBox_lanetype->currentIndex());
+    double flanemarkwidth = ui->lineEdit_lanemarkwidth->text().toDouble();
+    if(flanemarkwidth<0.001)flanemarkwidth = 0.15;
+    plane->set_lanemarkwidth(flanemarkwidth);
     ui->comboBox_Lane->setCurrentIndex(index);
     QMessageBox::information(this,tr("Info"),tr("Change Lane Successfully."),QMessageBox::YesAll);
 }

src/tool/map_lanetoxodr/dialogaddroadfromcda.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>900</width>
-    <height>650</height>
+    <height>690</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -40,7 +40,7 @@
    <property name="geometry">
     <rect>
      <x>358</x>
-     <y>560</y>
+     <y>617</y>
      <width>161</width>
      <height>51</height>
     </rect>
@@ -101,7 +101,7 @@
      <x>63</x>
      <y>123</y>
      <width>781</width>
-     <height>201</height>
+     <height>271</height>
     </rect>
    </property>
    <property name="title">
@@ -261,12 +261,35 @@
      </rect>
     </property>
    </widget>
+   <widget class="QLineEdit" name="lineEdit_lanemarkwidth">
+    <property name="geometry">
+     <rect>
+      <x>150</x>
+      <y>200</y>
+      <width>161</width>
+      <height>41</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_14">
+    <property name="geometry">
+     <rect>
+      <x>30</x>
+      <y>200</y>
+      <width>101</width>
+      <height>41</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>车道线宽度：</string>
+    </property>
+   </widget>
   </widget>
   <widget class="QGroupBox" name="groupBox_2">
    <property name="geometry">
     <rect>
      <x>61</x>
-     <y>338</y>
+     <y>402</y>
      <width>781</width>
      <height>181</height>
     </rect>
@@ -433,7 +456,7 @@
    <property name="geometry">
     <rect>
      <x>60</x>
-     <y>560</y>
+     <y>617</y>
      <width>161</width>
      <height>51</height>
     </rect>
@@ -446,7 +469,7 @@
    <property name="geometry">
     <rect>
      <x>690</x>
-     <y>560</y>
+     <y>617</y>
      <width>161</width>
      <height>51</height>
     </rect>

src/tool/map_lanetoxodr/function/cdaproc.cpp

@@ -3,11 +3,234 @@
 #include <iostream>
 #include <math.h>
 
+static std::string cda_lanetype_sel[9] = {"shoulder","border","driving","stop","none","parking","biking","sidewalk",
+                               "median"};
+
+static std::string cda_lanemarkcolor_sel[2] = {"standard","yellow"};
+
+static std::string cda_lanemarktype_sel[7] = {"broken","solid","broken broken","solid solid","broken solid","solid broken","none"};
+
 CDAProc::CDAProc()
 {
 
 }
 
+static void OffRotate(double nowx,double nowy,double nowhdg,double rel_x,double rel_y,double rel_hdg,double & a_x,double &  a_y,double & a_hdg)
+{
+    a_x = nowx + rel_x * cos(nowhdg) - rel_y * sin(nowhdg) ;
+    a_y= nowy + rel_x * sin(nowhdg) + rel_y * cos(nowhdg) ;
+    a_hdg = rel_hdg+ nowhdg;
+    if(a_hdg> 2.0*M_PI)a_hdg = a_hdg - 2.0*M_PI;
+}
+
+int CDAProc::ProcIntersectionRoad(OpenDrive * pxodr, iv::map::cdadraw * pcdadraw,int ngeo,int & nroadid,double & nowx,double & nowy, double & nowhdg)
+{
+    iv::map::cdageo * pgeo = pcdadraw->mutable_mgeos(ngeo);
+    double fRoadLen =  pgeo->geolen();
+//    double flanewidth = atof(strlanewidth.data());
+    std::vector<double> xvectorlanewidth;
+    int i;
+    for(i=0;i<pcdadraw->mlanes_size();i++)
+    {
+        xvectorlanewidth.push_back(pcdadraw->mutable_mlanes(i)->lanewidth());
+    }
+    double flanewidth11 = 0;
+    for(i=0;i<(pcdadraw->mlanes_size()-1);i++)
+    {
+        flanewidth11 = flanewidth11 + xvectorlanewidth[i];
+    }
+    int nlanecount = pcdadraw->mlanes_size();
+    if(nlanecount<=0)
+    {
+        std::cout<<" no lane "<<std::endl;
+        return -1;
+    }
+    double fdefradius = 6.0;
+    double finsectlen = fdefradius*2.0 + 2.0*flanewidth11;
+    if(fRoadLen < (finsectlen+10.0))
+    {
+        fRoadLen = finsectlen + 10.0;
+    }
+    double flinelen = (fRoadLen -finsectlen)/2.0;
+    double flinex[4],fliney[4],flinehdg[4];
+    flinex[0] = 0;fliney[0]=0;flinehdg[0] = 0;
+    flinex[1] = fRoadLen*(0.5);fliney[1] = fRoadLen*(-0.5);  flinehdg[1] = M_PI/2.0;
+    flinex[2] = fRoadLen*0.5 + finsectlen*0.5;fliney[2] = 0;  flinehdg[2] = 0;
+    flinex[3] = fRoadLen*(0.5);fliney[3] = finsectlen*0.5;  flinehdg[3] = M_PI/2.0;
+
+    double flinex_c[4],fliney_c[4],flinehdg_c[4];
+
+    double nowx_n,nowy_n,nowhdg_n;
+    for(i=0;i<4;i++)
+    {
+        flinex_c[i] = nowx + flinex[i] * cos(nowhdg) - fliney[i] * sin(nowhdg) ;
+        fliney_c[i] = nowy + flinex[i] * sin(nowhdg) + fliney[i] * cos(nowhdg) ;
+        flinehdg_c[i] = flinehdg[i] + nowhdg;
+        if(flinehdg_c[i]> 2.0*M_PI)flinehdg_c[i] = flinehdg_c[i] - 2.0*M_PI;
+
+        char strroadid[100];
+        nroadid++;
+        snprintf(strroadid,100,"%d",nroadid);
+        pxodr->AddRoad("zl",flinelen,strroadid,"-1");
+        Road * pRoad = pxodr->GetLastAddedRoad();
+        pRoad->AddGeometryBlock();
+        GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
+        pgeob->AddGeometryLine(0,flinex_c[i],fliney_c[i],flinehdg_c[i],flinelen);
+        pRoad->AddLaneSection(0);
+        LaneSection * pLS = pRoad->GetLaneSection(0);
+        pLS->AddLane(0,0,"none",false);
+        Lane * pcenterlane = pLS->GetLastAddedLane();
+        pcenterlane->AddRoadMarkRecord(0,"solid solid","standard","yellow",0.15,"none");
+
+        int j;
+        for(j=0;j<nlanecount;j++)
+        {
+            iv::map::cdalane * pcdalane = pcdadraw->mutable_mlanes(j);
+            pLS->AddLane(-1,(j+1)*(-1),cda_lanetype_sel[pcdalane->lanetype()],false,true);
+            Lane * pnewlane = pLS->GetLastAddedLane();
+
+            pnewlane->AddWidthRecord(0,pcdalane->lanewidth(),0,0,0);
+            pnewlane->AddRoadMarkRecord(0,cda_lanemarktype_sel[pcdalane->lanemarktype()],"standard",cda_lanemarkcolor_sel[pcdalane->lanemarkcolor()],pcdalane->lanemarkwidth(),"none");
+
+            pLS->AddLane(1,(j+1)*(1),cda_lanetype_sel[pcdalane->lanetype()],false,true);
+            pnewlane = pLS->GetLastAddedLane();
+            pnewlane->AddWidthRecord(0,pcdalane->lanewidth(),0,0,0);
+            pnewlane->AddRoadMarkRecord(0,cda_lanemarktype_sel[pcdalane->lanemarktype()],"standard",cda_lanemarkcolor_sel[pcdalane->lanemarkcolor()],pcdalane->lanemarkwidth(),"none");
+
+        }
+        if(i==2)
+        {
+            pRoad->GetGeometryCoords(pRoad->GetRoadLength(),nowx_n,nowy_n,nowhdg_n);
+        }
+    }
+
+    double frightlen = fdefradius * M_PI/2.0;
+    double frightx[4],frighty[4],frighthdg[4];
+    frightx[0] = flinelen;frighty[0] = flanewidth11*(-1);frighthdg[0] = 0;
+    frightx[1] = fRoadLen*0.5 + flanewidth11 ;frighty[1] = finsectlen*(-0.5);frighthdg[1] = M_PI/2.0;
+    frightx[2] = fRoadLen*0.5 + finsectlen*0.5;frighty[2] = flanewidth11*(1);frighthdg[2] = M_PI;
+    frightx[3] = fRoadLen*0.5 - flanewidth11;frighty[3] = finsectlen*0.5;frighthdg[3] = 3.0*M_PI/2.0;
+
+    for(i=0;i<4;i++)
+    {
+        flinex_c[i] = nowx + frightx[i] * cos(nowhdg) - frighty[i] * sin(nowhdg) ;
+        fliney_c[i] = nowy + frightx[i] * sin(nowhdg) + frighty[i] * cos(nowhdg) ;
+        flinehdg_c[i] = frighthdg[i] + nowhdg;
+        if(flinehdg_c[i]> 2.0*M_PI)flinehdg_c[i] = flinehdg_c[i] - 2.0*M_PI;
+
+        char strroadid[100];
+        nroadid++;
+        snprintf(strroadid,100,"%d",nroadid);
+        pxodr->AddRoad("zl",flinelen,strroadid,"-1");
+        Road * pRoad = pxodr->GetLastAddedRoad();
+        pRoad->AddGeometryBlock();
+        GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
+        pgeob->AddGeometryArc(0,flinex_c[i],fliney_c[i],flinehdg_c[i],frightlen,-1.0/fdefradius);
+        pRoad->AddLaneSection(0);
+        LaneSection * pLS = pRoad->GetLaneSection(0);
+        pLS->AddLane(0,0,"none",false);
+//        Lane * pcenterlane = pLS->GetLastAddedLane();
+ //       pcenterlane->AddRoadMarkRecord(0,"solid","standard","yellow",0.15,"none");
+
+        iv::map::cdalane * pcdalane = pcdadraw->mutable_mlanes(pcdadraw->mlanes_size()-1);
+
+        pLS->AddLane(-1,-1,cda_lanetype_sel[pcdalane->lanetype()],false,true);
+        Lane * pnewlane = pLS->GetLastAddedLane();
+        pnewlane->AddWidthRecord(0,pcdadraw->mutable_mlanes(pcdadraw->mlanes_size()-1)->lanewidth(),0,0,0);
+    }
+
+    double fleftx[4],flefty[4],flefthdg[4];
+    fleftx[0] = flinelen;flefty[0] = 0;flefthdg[0] = 0;
+    fleftx[1] = fRoadLen*0.5;flefty[1] = finsectlen*(-0.5);flefthdg[1] = M_PI/2.0;
+    fleftx[2] = flinelen+finsectlen;flefty[2] = 0;flefthdg[2] = M_PI;
+    fleftx[3] = fRoadLen*0.5;flefty[3] = finsectlen*0.5;flefthdg[3] = 3.0*M_PI/2.0;
+    for(i=0;i<4;i++)
+    {
+        double fleftlinelen = 0.5*finsectlen - fdefradius;
+        double fleftarcx = fleftx[i] + fleftlinelen * cos(flefthdg[i]);
+        double fleftarcy = flefty[i] + fleftlinelen * sin(flefthdg[i]);
+        double fleftarchdg = flefthdg[i];
+        double a_x,a_y,a_hdg;
+        char strroadid[100];
+        nroadid++;
+        snprintf(strroadid,100,"%d",nroadid);
+        pxodr->AddRoad("zl",fleftlinelen*2.0+fdefradius*M_PI/2.0,strroadid,"-1");
+        Road * pRoad = pxodr->GetLastAddedRoad();
+
+        GeometryBlock * pgeob;
+        if(fleftlinelen >0.00000000001)
+        {
+            pRoad->AddGeometryBlock();
+            GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
+            OffRotate(nowx,nowy,nowhdg,fleftx[i],flefty[i],flefthdg[i],a_x,a_y,a_hdg);
+            pgeob->AddGeometryLine(0,a_x,a_y,a_hdg,fleftlinelen);
+        }
+        pRoad->AddGeometryBlock();
+        pgeob= pRoad->GetLastAddedGeometryBlock();
+         OffRotate(nowx,nowy,nowhdg,fleftarcx,fleftarcy,fleftarchdg,a_x,a_y,a_hdg);
+        pgeob->AddGeometryArc(fleftlinelen,a_x,a_y,a_hdg,fdefradius*M_PI/2.0,1.0/fdefradius);
+        if(fleftlinelen >0.00000000001)
+        {
+            pRoad->AddGeometryBlock();
+            GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
+            OffRotate(nowx,nowy,nowhdg,fleftarcx + fdefradius*cos(flefthdg[i]) + fdefradius*cos(flefthdg[i] + M_PI/2.0) ,
+                      fleftarcy + fdefradius*sin(flefthdg[i]) + fdefradius*sin(flefthdg[i] + M_PI/2.0),flefthdg[i] +M_PI/2.0,a_x,a_y,a_hdg);
+            pgeob->AddGeometryLine(fleftlinelen + fdefradius*M_PI/2.0,a_x,a_y,a_hdg,fleftlinelen);
+        }
+        pRoad->AddLaneSection(0);
+        LaneSection * pLS = pRoad->GetLaneSection(0);
+        pLS->AddLane(0,0,"none",false);
+//        Lane * pcenterlane = pLS->GetLastAddedLane();
+ //       pcenterlane->AddRoadMarkRecord(0,"solid","standard","yellow",0.15,"none");
+
+        iv::map::cdalane * pcdalane = pcdadraw->mutable_mlanes(0);
+        pLS->AddLane(-1,-1,cda_lanetype_sel[pcdalane->lanetype()],false,true);
+        Lane * pnewlane = pLS->GetLastAddedLane();
+        pnewlane->AddWidthRecord(0,pcdadraw->mutable_mlanes(0)->lanewidth(),0,0,0);
+    }
+
+    double finlinex[4],finliney[4],finlinehdg[4];
+    finlinex[0] = flinelen;finliney[0] = 0;finlinehdg[0] = 0;
+    finlinex[1] = fRoadLen*0.5;finliney[1] = finsectlen*(-0.5);finlinehdg[1] = M_PI/2.0;
+    finlinex[2] = flinelen+finsectlen;finliney[2] = 0;finlinehdg[2] = M_PI;
+    finlinex[3] = fRoadLen*0.5;finliney[3] = finsectlen*0.5;finlinehdg[3] = 3.0*M_PI/2.0;
+
+    for(i=0;i<4;i++)
+    {
+        char strroadid[100];
+        nroadid++;
+        snprintf(strroadid,100,"%d",nroadid);
+        pxodr->AddRoad("zl",flinelen,strroadid,"-1");
+        Road * pRoad = pxodr->GetLastAddedRoad();
+        pRoad->AddGeometryBlock();
+        GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
+        double a_x,a_y,a_hdg;
+        OffRotate(nowx,nowy,nowhdg,finlinex[i],finliney[i],finlinehdg[i],a_x,a_y,a_hdg);
+        pgeob->AddGeometryLine(0,a_x,a_y,a_hdg,finsectlen);
+        pRoad->AddLaneSection(0);
+        LaneSection * pLS = pRoad->GetLaneSection(0);
+        pLS->AddLane(0,0,"none",false);
+//        Lane * pcenterlane = pLS->GetLastAddedLane();
+//        pcenterlane->AddRoadMarkRecord(0,"solid solid","standard","yellow",0.15,"none");
+
+        int j;
+        for(j=0;j<nlanecount;j++)
+        {
+            iv::map::cdalane * pcdalane = pcdadraw->mutable_mlanes(j);
+            pLS->AddLane(-1,(j+1)*(-1),cda_lanetype_sel[pcdalane->lanetype()],false,true);
+            Lane * pnewlane = pLS->GetLastAddedLane();
+            pnewlane->AddWidthRecord(0,pcdalane->lanewidth(),0,0,0);
+            pLS->AddLane(1,(j+1)*(1),cda_lanetype_sel[pcdalane->lanetype()],false,true);
+            pnewlane = pLS->GetLastAddedLane();
+            pnewlane->AddWidthRecord(0,pcdalane->lanewidth(),0,0,0);
+        }
+    }
+
+    nowx = nowx_n;
+    nowy = nowy_n;
+    nowhdg = nowhdg_n;
+
+}
 
 int CDAProc::ProcIntersectionRoad(OpenDrive * pxodr,  std::string strtype,std::string strradius,std::string strroadlen,std::string strlanewidth,
     std::string strlannecount,std::string strlanetype,std::string strlanemarkcolor,std::string strlanemarktype)
@@ -436,3 +659,122 @@ int CDAProc::ProcRoads(std::string strxlsxpath,std::string stroutpath)
     ServiceExcelAPI.Closexlsx(pexcel);
     return nr;
 }
+
+int CDAProc::ProcCDA(iv::map::cdadraw & xcdadraw,OpenDrive * pxodr)
+{
+    if(xcdadraw.mgeos_size() == 0)
+    {
+        std::cout<<" no geo. "<<std::endl;
+        return -1;
+    }
+    int i;
+    double xnow,ynow,hdgnow;
+    xnow = 0;
+    ynow = 0;
+    hdgnow = 0;
+    int nroadid = 0;
+    for(i=0;i<xcdadraw.mgeos_size();i++)
+    {
+        iv::map::cdageo * pgeo = xcdadraw.mutable_mgeos(i);
+        if(pgeo->geotype() == 0)
+        {
+            nroadid++;
+            char strroadid[256];
+            snprintf(strroadid,256,"%d",nroadid);
+            pxodr->AddRoad("zl",pgeo->geolen(),strroadid,"-1");
+            Road * pRoad = pxodr->GetLastAddedRoad();
+            pRoad->AddGeometryBlock();
+            GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
+            pgeob->AddGeometryLine(0,xnow,ynow,hdgnow,pgeo->geolen());
+            pRoad->GetGeometryCoords(pgeo->geolen(),xnow,ynow,hdgnow);
+
+            pRoad->AddLaneSection(0);
+            LaneSection * pLS = pRoad->GetLaneSection(0);
+            pLS->AddLane(0,0,"none",false);
+            Lane * pcenterlane = pLS->GetLastAddedLane();
+            pcenterlane->AddRoadMarkRecord(0,"solid solid","standard","yellow",0.15,"none");
+
+            int j;
+            iv::map::cdadraw * pcdadraw = &xcdadraw;
+            int nlanecount = pcdadraw->mlanes_size();
+            for(j=0;j<nlanecount;j++)
+            {
+                iv::map::cdalane * pcdalane = pcdadraw->mutable_mlanes(j);
+                pLS->AddLane(-1,(j+1)*(-1),cda_lanetype_sel[pcdalane->lanetype()],false,true);
+                Lane * pnewlane = pLS->GetLastAddedLane();
+
+                pnewlane->AddWidthRecord(0,pcdalane->lanewidth(),0,0,0);
+                pnewlane->AddRoadMarkRecord(0,cda_lanemarktype_sel[pcdalane->lanemarktype()],"standard",cda_lanemarkcolor_sel[pcdalane->lanemarkcolor()],pcdalane->lanemarkwidth(),"none");
+
+                pLS->AddLane(1,(j+1)*(1),cda_lanetype_sel[pcdalane->lanetype()],false,true);
+                pnewlane = pLS->GetLastAddedLane();
+                pnewlane->AddWidthRecord(0,pcdalane->lanewidth(),0,0,0);
+                pnewlane->AddRoadMarkRecord(0,cda_lanemarktype_sel[pcdalane->lanemarktype()],"standard",cda_lanemarkcolor_sel[pcdalane->lanemarkcolor()],pcdalane->lanemarkwidth(),"none");
+
+            }
+        }
+        if(pgeo->geotype() == 1)
+        {
+            nroadid++;
+            char strroadid[256];
+            snprintf(strroadid,256,"%d",nroadid);
+            pxodr->AddRoad("wd",pgeo->geolen(),strroadid,"-1");
+            Road * pRoad = pxodr->GetLastAddedRoad();
+            pRoad->AddGeometryBlock();
+            GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
+            pgeob->AddGeometryArc(0,xnow,ynow,hdgnow,pgeo->geolen(),1.0/pgeo->georadius());
+            pRoad->GetGeometryCoords(pgeo->geolen(),xnow,ynow,hdgnow);
+
+            pRoad->AddLaneSection(0);
+            LaneSection * pLS = pRoad->GetLaneSection(0);
+            pLS->AddLane(0,0,"none",false);
+            Lane * pcenterlane = pLS->GetLastAddedLane();
+            pcenterlane->AddRoadMarkRecord(0,"solid solid","standard","yellow",0.15,"none");
+
+            int j;
+            iv::map::cdadraw * pcdadraw = &xcdadraw;
+            int nlanecount = pcdadraw->mlanes_size();
+            for(j=0;j<nlanecount;j++)
+            {
+                iv::map::cdalane * pcdalane = pcdadraw->mutable_mlanes(j);
+                pLS->AddLane(-1,(j+1)*(-1),cda_lanetype_sel[pcdalane->lanetype()],false,true);
+                Lane * pnewlane = pLS->GetLastAddedLane();
+
+                pnewlane->AddWidthRecord(0,pcdalane->lanewidth(),0,0,0);
+                pnewlane->AddRoadMarkRecord(0,cda_lanemarktype_sel[pcdalane->lanemarktype()],"standard",cda_lanemarkcolor_sel[pcdalane->lanemarkcolor()],pcdalane->lanemarkwidth(),"none");
+
+                pLS->AddLane(1,(j+1)*(1),cda_lanetype_sel[pcdalane->lanetype()],false,true);
+                pnewlane = pLS->GetLastAddedLane();
+                pnewlane->AddWidthRecord(0,pcdalane->lanewidth(),0,0,0);
+                pnewlane->AddRoadMarkRecord(0,cda_lanemarktype_sel[pcdalane->lanemarktype()],"standard",cda_lanemarkcolor_sel[pcdalane->lanemarkcolor()],pcdalane->lanemarkwidth(),"none");
+
+            }
+        }
+        if(pgeo->geotype() == 2)
+        {
+            ProcIntersectionRoad(pxodr,&xcdadraw,i,nroadid,xnow,ynow,hdgnow);
+        }
+
+    }
+
+//    for(i=0;i<pxodr->GetRoadCount();i++)
+//    {
+//        Road * pRoad = pxodr->GetRoad(i);
+//        pRoad->AddLaneSection(0);
+//        LaneSection * pLS = pRoad->GetLaneSection(0);
+//        pLS->AddLane(0,0,"none",false);
+//        Lane * pcenterlane = pLS->GetLastAddedLane();
+//        pcenterlane->AddRoadMarkRecord(0,"solid","standard","yellow",0.15,"none");
+
+//        int i;
+//        for(i=0;i<2;i++)
+//        {
+//            pLS->AddLane(-1,(i+1)*(-1),"driving",false,true);
+//            Lane * pnewlane = pLS->GetLastAddedLane();
+//            pnewlane->AddWidthRecord(0,3.5,0,0,0);
+
+//                pnewlane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"none");
+//        }
+//    }
+    return 0;
+}

src/tool/map_lanetoxodr/function/cdaproc.h

@@ -7,7 +7,7 @@
 
 #include <OpenDrive/OpenDrive.h>
 
-
+#include "cdadraw.pb.h"
 
 
 //namespace iv
@@ -69,6 +69,9 @@ public:
         std::string strlannecount,std::string strlanetype,std::string strlanemarkcolor,std::string strlanemarktype);
     static int ProcIntersectionRoad(OpenDrive * pxodr,  std::string strtype,std::string strradius,std::string strroadlen,std::string strlanewidth,
         std::string strlannecount,std::string strlanetype,std::string strlanemarkcolor,std::string strlanemarktype);
+    static int ProcIntersectionRoad(OpenDrive * pxodr, iv::map::cdadraw * pcdadraw,int ngeo,int & nroadid,double & nowx,double & nowy, double & nowhdg);
+
+    static int ProcCDA(iv::map::cdadraw & xcdadraw,OpenDrive * pxodr);
 };
 
 #endif // CDAPROC_H

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);