change odtolanelet for Autoware.

src/map/odtolanelet/main.cpp

@@ -10,10 +10,10 @@ int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
-    odtolanelet * potl = new odtolanelet("/home/yuchuli/line.xodr");
+    odtolanelet * potl = new odtolanelet("/home/yuchuli/map/map_xiali.xodr");
 
     int64_t time1 = std::chrono::system_clock::now().time_since_epoch().count();
-    potl->ConvertToLanelet("/home/yuchuli/line.osm");
+    potl->ConvertToLanelet("/home/yuchuli/lanelet2_map.osm");
     int64_t time2 = std::chrono::system_clock::now().time_since_epoch().count();
 
     std::cout<<"use time: "<<(time2 - time1)<<std::endl;

src/map/odtolanelet/odtolanelet.cpp

@@ -4,6 +4,8 @@
 
 #include "gaussprojector.h"
 
+#include "../../../common/common/math/gnss_coordinate_convert.h"
+
 #ifdef TESTROUTING
 
 #include <lanelet2_routing/Route.h>
@@ -37,6 +39,21 @@ int odtolanelet::ConvertToLanelet(std::string strlanelet2file)
         return -1;
     }
 
+    double flon0,flat0;
+    if(pxodr->GetHeader() != NULL)
+    {
+        pxodr->GetHeader()->GetLat0Lon0(flat0,flon0);
+    }
+    else
+    {
+        flon0 = 117.0;
+        flat0 = 39.0;
+    }
+
+    mlon0 = flon0;
+    mlat0 = flat0;
+    GaussProjCal(mlon0,mlat0,&mfx0,&mfy0);
+
     lanelet::LaneletMapPtr laneletMap  = std::make_shared<lanelet::LaneletMap>();
     unsigned int nroadnum = pxodr->GetRoadCount();
     unsigned int i;
@@ -83,10 +100,22 @@ int odtolanelet::ConvertToLanelet(std::string strlanelet2file)
 #endif
 
 
-    lanelet::Origin origin2({39.1364713, 117.0866293, 0});
+ //   lanelet::Origin origin2({39.1364713, 117.0866293, 0});
+    lanelet::Origin origin2({flat0, flon0, 0});
 
     lanelet::write(strlanelet2file,*laneletMap,GaussProjector(origin2));
 
+//    lanelet::Projector * pjec;
+
+
+//    lanelet::LaneletMapPtr map;
+
+//    map->laneletLayer.size();
+//    map->pointLayer.size();
+//    map->pointLayer.begin()->x();
+
+
+
     std::cout<<"convert complete."<<std::endl;
 //    lanelet::write(strlanelet2file,*laneletMap,origin2);
 
@@ -219,6 +248,7 @@ void odtolanelet::odtypetolettype(iv::LanePoint & xLanePoint, lanelet::LineStrin
     {
         xlinestring.setAttribute("type",lanelet::Attribute("line_thin"));
         xlinestring.setAttribute("subtype",lanelet::Attribute("solid"));
+        xlinestring.setAttribute("width",lanelet::Attribute("0.15"));
         return;
     }
     if(xLanePoint.mstrroadmarktype == "broken")
@@ -237,18 +267,21 @@ void odtolanelet::odtypetolettype(iv::LanePoint & xLanePoint, lanelet::LineStrin
     {
         xlinestring.setAttribute("type",lanelet::Attribute("line_thin"));
         xlinestring.setAttribute("subtype",lanelet::Attribute("solid_solid"));
+        xlinestring.setAttribute("width",lanelet::Attribute("0.15"));
         return;
     }
     if(xLanePoint.mstrroadmarktype == "solid broken")
     {
         xlinestring.setAttribute("type",lanelet::Attribute("line_thin"));
         xlinestring.setAttribute("subtype",lanelet::Attribute("solid_dashed"));
+        xlinestring.setAttribute("width",lanelet::Attribute("0.15"));
         return;
     }
     if(xLanePoint.mstrroadmarktype == "broken solid")
     {
         xlinestring.setAttribute("type",lanelet::Attribute("line_thin"));
         xlinestring.setAttribute("subtype",lanelet::Attribute("dashed_solid"));
+        xlinestring.setAttribute("width",lanelet::Attribute("0.15"));
         return;
     }
     if(xLanePoint.mstrroadmarktype == "curb")
@@ -274,6 +307,10 @@ void odtolanelet::LanePointToPoint3D(std::vector<iv::LanePoint> & xvectorLanePoi
     for(i=0;i<nLPsize;i++)
     {
         lanelet::Point3d xPoint(mid,xvectorLanePoint[i].mx,xvectorLanePoint[i].my,xvectorLanePoint[i].mz);
+        std::string strmgrscode;
+        Getmgrscode(xvectorLanePoint[i].mx,xvectorLanePoint[i].my,strmgrscode);
+        xPoint.setAttribute("mgrs_code",strmgrscode);
+        xPoint.setAttribute("ele","0.0");
         mid++;
         xvectorPoint3D.push_back(xPoint);
     }
@@ -303,6 +340,10 @@ void odtolanelet::LanePointToPoint3DWithLast(std::vector<iv::LanePoint> & xvecto
         if(bUseOld == false)
         {
             lanelet::Point3d xPoint(mid,xvectorLanePoint[i].mx,xvectorLanePoint[i].my,xvectorLanePoint[i].mz);
+            std::string strmgrscode;
+            Getmgrscode(xvectorLanePoint[i].mx,xvectorLanePoint[i].my,strmgrscode);
+            xPoint.setAttribute("ele","0.0");
+            xPoint.setAttribute("mgrs_code",strmgrscode);
             mid++;
             xvectorPoint3D.push_back(xPoint);
         }
@@ -326,6 +367,11 @@ void odtolanelet::linetolet(lanelet::LaneletMapPtr & laneletMap,std::vector<lane
         xlet.setAttribute("odr:s_start",lanelet::Attribute(send));
         xlet.setAttribute("odr:s_end",lanelet::Attribute(sstart));
         xlet.setAttribute("odr:lane",lanelet::Attribute(static_cast<int>(ncenterlane-i) ));
+        xlet.setAttribute("location",lanelet::Attribute("urban"));
+        xlet.setAttribute("one_way",lanelet::Attribute("yes"));
+        xlet.setAttribute("speed_limit",lanelet::Attribute("30"));
+        xlet.setAttribute("subtype",lanelet::Attribute("road"));
+        xlet.setAttribute("turn_direction",lanelet::Attribute("straight"));
         mid++;
         laneletMap->add(xlet);
     }
@@ -336,6 +382,11 @@ void odtolanelet::linetolet(lanelet::LaneletMapPtr & laneletMap,std::vector<lane
         xlet.setAttribute("odr:s_start",lanelet::Attribute(sstart));
         xlet.setAttribute("odr:s_end",lanelet::Attribute(send));
         xlet.setAttribute("odr:lane",lanelet::Attribute(static_cast<int>(i-ncenterlane+1) *(-1)));
+        xlet.setAttribute("location",lanelet::Attribute("urban"));
+        xlet.setAttribute("one_way",lanelet::Attribute("yes"));
+        xlet.setAttribute("speed_limit",lanelet::Attribute("30"));
+        xlet.setAttribute("subtype",lanelet::Attribute("road"));
+        xlet.setAttribute("turn_direction",lanelet::Attribute("straight"));
         mid++;
         laneletMap->add(xlet);
     }
@@ -347,6 +398,7 @@ int odtolanelet::InvertLine(lanelet::LineString3d & xinvert,lanelet::LineString3
     if(xraw.hasAttribute("color"))xinvert.setAttribute("color",xraw.attribute("color"));
     if(xraw.hasAttribute("subtype"))xinvert.setAttribute("subtype",xraw.attribute("subtype"));
     if(xraw.hasAttribute("type"))xinvert.setAttribute("type",xraw.attribute("type"));
+    if(xraw.hasAttribute("width"))xinvert.setAttribute("width",xraw.attribute("width"));
     xinvert.clear();
     unsigned int npointssize = static_cast<unsigned int >(xraw.size());
     unsigned int i;
@@ -704,3 +756,44 @@ bool odtolanelet::IsRelation(OpenDrive * pxodr,std::string strroad1,std::string
 
     return false;
 }
+
+
+void odtolanelet::Getmgrscode(double fx,double fy,std::string & strmgrscode)
+{
+    double flon,flat;
+    GaussProjInvCal(mfx0 + fx,mfy0+ fy,&flon,&flat);
+
+    int nlon,nlat;
+    nlon = flon/6;
+    nlat = flat/8;
+
+    int nCode = flon/6 + 31;
+
+    const char codelon[]={'A','B','C','D','E','F','G','H','J','K','L','M','N','P','Q','R','S','T','U','V','W','X','Y','Z'};
+
+    char code1 = codelon[nlat + 12];
+
+
+    double flonbase = nlon*6;
+    double flatbase = nlat*8;
+    double fxbase0,fybase0;
+    GaussProjCal(flonbase,flatbase,&fxbase0,&fybase0);
+    double fxin,fyin;
+    GaussProjCal(flon,flat,&fxin,&fyin);
+    fxin = fxin - fxbase0;
+    fyin = fyin - fybase0;
+    int n1,n2;
+    n1 = fxin/100000;
+    n2 = fyin/100000;
+    double fx2,fy2;
+    fx2 = fxin  - n1*100000.0;
+    fy2 = fyin -  n2*100000.0;
+    int n3,n4;
+    n3 = fx2/100;
+    n4 = fy2/100;
+    char strcode[30];
+    snprintf(strcode,30,"%02d%c%c%c%03d%03d",nCode,code1,codelon[n1],codelon[n2],n3,n4);
+    std::cout<<" code "<<strcode<<std::endl;
+    strmgrscode = strcode;
+
+}

src/map/odtolanelet/odtolanelet.h

@@ -33,6 +33,13 @@ private:
 
     lanelet::Id mid = 1;
 
+    double mlon0;
+    double mlat0;
+    double mfx0;
+    double mfy0;
+
+
+
 private:
     int Road2Lanelet(Road * pRoad,lanelet::LaneletMapPtr & laneletMap);
 
@@ -62,6 +69,9 @@ private:
     void InsertSameNodeID(std::vector<std::vector<iv::LaneletSameID>> & xvecotorsameID,lanelet::Id xLeftID1,lanelet::Id xRightID1,
                           lanelet::Id xLeftID2,lanelet::Id xRightID2,int npos1,int npos2);
 
+
+    void Getmgrscode(double fx,double fy,std::string & strmgrscode);
+
 };
 
 #endif // ODTOLANELET_H