|
@@ -0,0 +1,66 @@
|
|
|
+#include "xodrtoapollohdmap.h"
|
|
|
+
|
|
|
+#include "OpenDrive/OpenDrive.h"
|
|
|
+#include "OpenDrive/OpenDriveXmlParser.h"
|
|
|
+
|
|
|
+xodrtoapollohdmap::xodrtoapollohdmap(std::string strxodrpath,std::string strhdmappath){
|
|
|
+ mstrxodrpath = strxodrpath;
|
|
|
+ mstrhdmappath = strhdmappath;
|
|
|
+}
|
|
|
+
|
|
|
+int xodrtoapollohdmap::Convert(){
|
|
|
+
|
|
|
+ OpenDrive * pxodr = new OpenDrive(); //because add to xodr,so don't delete
|
|
|
+ OpenDriveXmlParser x(pxodr);
|
|
|
+ if(!x.ReadFile(mstrxodrpath))
|
|
|
+ {
|
|
|
+ std::cout<<"Can't load xodr file."<<std::endl;
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+
|
|
|
+ double flon0,flat0;
|
|
|
+ if(pxodr->GetHeader() != NULL)
|
|
|
+ {
|
|
|
+ pxodr->GetHeader()->GetLat0Lon0(flat0,flon0);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ flon0 = 117.0;
|
|
|
+ flat0 = 39.0;
|
|
|
+ }
|
|
|
+
|
|
|
+ apollo::hdmap::Map xHdMap;
|
|
|
+
|
|
|
+
|
|
|
+ unsigned int nroadnum = pxodr->GetRoadCount();
|
|
|
+ unsigned int i;
|
|
|
+
|
|
|
+ for(i=0;i<nroadnum;i++)
|
|
|
+ {
|
|
|
+ Road * pRoad = pxodr->GetRoad(i);
|
|
|
+
|
|
|
+ std::string * pstrroadid = new std::string;
|
|
|
+ *pstrroadid = pRoad->GetRoadId();
|
|
|
+
|
|
|
+ apollo::hdmap::Road * pHdMapRoad = xHdMap.add_road();
|
|
|
+
|
|
|
+ apollo::hdmap::Id * proadid = new apollo::hdmap::Id();
|
|
|
+ proadid->set_allocated_id(pstrroadid);
|
|
|
+
|
|
|
+ pHdMapRoad->set_allocated_id(proadid);
|
|
|
+
|
|
|
+ pHdMapRoad->set_type(apollo::hdmap::Road_Type_CITY_ROAD);
|
|
|
+
|
|
|
+ if(pRoad->GetRoadJunction() != "-1")
|
|
|
+ {
|
|
|
+ std::string * pstrjunctionid = new std::string;
|
|
|
+ *pstrjunctionid = pRoad->GetRoadJunction();
|
|
|
+ apollo::hdmap::Id * pjunctionid = new apollo::hdmap::Id();
|
|
|
+ pjunctionid->set_allocated_id(pstrjunctionid);
|
|
|
+ pHdMapRoad->set_allocated_junction_id(pjunctionid);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+ return 0;
|
|
|
+}
|