change odtolanelet. change mgrs code calculate, use geolib.

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/map/map_xiali.xodr");
+    odtolanelet * potl = new odtolanelet("/home/yuchuli/line.xodr");
 
     int64_t time1 = std::chrono::system_clock::now().time_since_epoch().count();
-    potl->ConvertToLanelet("/home/yuchuli/lanelet2_map.osm");
+    potl->ConvertToLanelet("/home/yuchuli/autoware_map/test/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

@@ -6,6 +6,9 @@
 
 #include "../../../common/common/math/gnss_coordinate_convert.h"
 
+#include <GeographicLib/MGRS.hpp>
+#include <GeographicLib/UTMUPS.hpp>
+
 #ifdef TESTROUTING
 
 #include <lanelet2_routing/Route.h>
@@ -16,6 +19,9 @@
 
 #endif
 
+
+
+
 namespace iv
 {
 
@@ -105,7 +111,16 @@ int odtolanelet::ConvertToLanelet(std::string strlanelet2file)
 
     lanelet::write(strlanelet2file,*laneletMap,GaussProjector(origin2));
 
-//    lanelet::Projector * pjec;
+    lanelet::Projector * pjec;
+
+    lanelet::ErrorMessages errors{};
+
+
+
+ //   lanelet::Projector xPro;
+
+
+ //   const lanelet::LaneletMapPtr map = lanelet::load("/home/yuchuli/lanelet2_map2.osm", xPro, &errors);
 
 
 //    lanelet::LaneletMapPtr map;
@@ -760,9 +775,29 @@ bool odtolanelet::IsRelation(OpenDrive * pxodr,std::string strroad1,std::string
 
 void odtolanelet::Getmgrscode(double fx,double fy,std::string & strmgrscode)
 {
+
     double flon,flat;
     GaussProjInvCal(mfx0 + fx,mfy0+ fy,&flon,&flat);
 
+    lanelet::BasicPoint3d mgrs_point{0., 0., 0};
+    lanelet::BasicPoint3d utm_point{0., 0., 0};
+    int zone{};
+    bool is_north{};
+    std::string mgrs_code;
+
+    try {
+        GeographicLib::UTMUPS::Forward(flat, flon, zone, is_north, utm_point.x(), utm_point.y());
+        GeographicLib::MGRS::Forward(
+            zone, is_north, utm_point.x(), utm_point.y(), flat, 3, mgrs_code);
+    } catch (const GeographicLib::GeographicErr & err) {
+        std::cerr << err.what() << std::endl;
+        return;
+
+    }
+    strmgrscode = mgrs_code;
+    return;
+
+
     int nlon,nlat;
     nlon = flon/6;
     nlat = flat/8;

src/map/odtolanelet/odtolanelet.pro

@@ -55,6 +55,9 @@ if(contains(DEFINES,TESTROUTING)){
 LIBS +=  -llanelet2_routing -llanelet2_traffic_rules
 }
 
+LIBS += -lGeographic
+
+
 LIBS += -lboost_system -lboost_serialization  -lboost_filesystem -lpugixml
 
 unix:INCLUDEPATH += /usr/include/eigen3