diff --git a/include/open_drive_format.h b/include/open_drive_format.h index 7b66ce9b27a8e6beb07e48e280ad26cf85f5c7a5..de3c9ca072b44e9e9cd33af0c2e7ed059e639701 100644 --- a/include/open_drive_format.h +++ b/include/open_drive_format.h @@ -2,6 +2,13 @@ #define _OPEN_DRIVE_FORMAT_H #include <string> +#include <vector> + +typedef struct +{ + int id; ///< Projection polynomial of radially symmetric model. +}Road_struct; + class COpenDriveFormat { @@ -9,6 +16,8 @@ class COpenDriveFormat COpenDriveFormat(); void load(std::string &filename); ~COpenDriveFormat(); + private: + std::vector<Road_struct> road_vect; }; #endif diff --git a/src/open_drive_format.cpp b/src/open_drive_format.cpp index 8f6a2143fba9ca5bd2871f9f61cc5c7b6b430efd..9c7745cf4b55a4f0d81105f00a0d0a93e5cb309e 100644 --- a/src/open_drive_format.cpp +++ b/src/open_drive_format.cpp @@ -5,6 +5,8 @@ #include <sys/types.h> #include <sys/stat.h> #include <unistd.h> +#include <iostream> +#include <sstream> #ifdef _HAVE_XSD // #include "xml/OpenDRIVE_1.1.hxx" @@ -27,8 +29,49 @@ void COpenDriveFormat::load(std::string &filename) try{ std::auto_ptr<OpenDRIVE> open_drive(OpenDRIVE_(filename.c_str(), xml_schema::flags::dont_validate)); - // for (OpenDRIVE::road_iterator road_it (open_drive->road().begin()); road_it != open_drive->road().end(); ++road_it) - // std::cout << "Road id = " /*<< road_it->id()*/ << std::endl; + for (OpenDRIVE::road_iterator road_it(open_drive->road().begin()); road_it != open_drive->road().end(); ++road_it) + { + // Road_struct r; + // std::stringstream ss(road_it->id().get()); + // ss >> r.id; + // r.id = road_it->id().get(); + // this->road_vect.push_back(r); + + std::cout << "Road id = " << road_it->id().get() << std::endl; + if (road_it->lane_link().present()) + { + if (road_it->lane_link().get().predecessor().present()) + { + std::cout << " predecessor: type: " << (road_it->lane_link().get().predecessor().get().elementType().present() ? road_it->lane_link().get().predecessor().get().elementType().get() : ""); + std::cout << "; id = " << (road_it->lane_link().get().predecessor().get().elementId().present() ? road_it->lane_link().get().predecessor().get().elementId().get() : ""); + std::cout << "; contact point: " << (road_it->lane_link().get().predecessor().get().contactPoint().present() ? road_it->lane_link().get().predecessor().get().contactPoint().get() : "") << std::endl; + } + if (road_it->lane_link().get().successor().present()) + { + std::cout << " successor type: " << (road_it->lane_link().get().successor().get().elementType().present() ? road_it->lane_link().get().successor().get().elementType().get() : ""); + std::cout << "; id = " << (road_it->lane_link().get().successor().get().elementId().present() ? road_it->lane_link().get().successor().get().elementId().get() : ""); + std::cout << "; contact point: " << (road_it->lane_link().get().successor().get().contactPoint().present() ? road_it->lane_link().get().successor().get().contactPoint().get() : "") << std::endl; + } + } + if (road_it->signals().present()) + { + for (signals::signal_iterator signal_it(road_it->signals().get().signal().begin()); signal_it != road_it->signals().get().signal().end(); ++signal_it) + { + std::cout << " Signal id: " << (signal_it->id().present() ? signal_it->id().get() : "") << std::endl; + std::cout << " Pose: s = " << (double)(signal_it->s().present() ? signal_it->s().get() : 0.0); + std::cout << "; t = " << (double)(signal_it->t().present() ? signal_it->t().get() : 0.0); + std::cout << "; heading = " << (double)(signal_it->hOffset().present() ? signal_it->hOffset().get() : 0.0) << std::endl; + std::cout << " Orientation = " << (signal_it->orientation().present() ? signal_it->orientation().get() : "") << std::endl; + std::cout << " Type: country: " << (signal_it->country().present() ? signal_it->country().get() : ""); + std::cout << "; type: " << (signal_it->type().present() ? signal_it->type().get() : ""); + std::cout << "; subtype: " << (signal_it->subtype().present() ? signal_it->subtype().get() : "") << std::endl; + std::cout << " Value: " << (double)(signal_it->value().present() ? signal_it->value().get() : 0.0); + std::cout << " " << (signal_it->unit().present() ? signal_it->unit().get() : ""); + std::cout << "; text: " << (signal_it->text().present() ? signal_it->text().get() : "") << std::endl; + + } + } + } // road->header() std::cout << "Done." << std::endl; }catch (const xml_schema::exception& e){