diff --git a/CMakeLists.txt b/CMakeLists.txt
index a3828b25158a081c5d3aac192b8cf39070a3ed81..2b56089fc6b3dc96462e82bb2f7652b143e7f130 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,6 @@
 # Pre-requisites about cmake itself
 CMAKE_MINIMUM_REQUIRED(VERSION 2.4)
+add_definitions(-std=c++17)
 
 if(COMMAND cmake_policy)
   cmake_policy(SET CMP0005 NEW) 
diff --git a/include/adc_circuit.h b/include/adc_circuit.h
index 965a5b081b976c6d3969c8a2a8aed273941056f6..3466d4fc581c537ccd323595f63b48aea0050297 100644
--- a/include/adc_circuit.h
+++ b/include/adc_circuit.h
@@ -14,6 +14,7 @@ class CAdcCircuit
 
     void clear_roads(void);
     void add_road(CAdcRoad &road);
+    void debug(void);
 };
 
 #endif
\ No newline at end of file
diff --git a/include/adc_geometries.h b/include/adc_geometries.h
index 4618bd365154788b9d7c611432ec78b8b8a6089b..a8aaa9961f66cb0eb1ccd6722682d065fa3ecd78 100644
--- a/include/adc_geometries.h
+++ b/include/adc_geometries.h
@@ -21,6 +21,8 @@ class CAdcGeometry
 
     bool get_world_pose(double s, double t, double heading, double &x, double &y, double &yaw);
     bool on_range(double s);
+    void debug_base(void);
+    virtual void debug(void) = 0;
 };
 
 class CAdcGeoLine: public CAdcGeometry
@@ -33,6 +35,7 @@ class CAdcGeoLine: public CAdcGeometry
     CAdcGeoLine();
     CAdcGeoLine(double min_s, double max_s, double x, double y, double heading);
     ~CAdcGeoLine();
+    void debug(void);
 };
 
 class CAdcGeoSpiral: public CAdcGeometry
@@ -50,6 +53,7 @@ class CAdcGeoSpiral: public CAdcGeometry
     CAdcGeoSpiral(double min_s, double max_s, double x, double y, double heading, double curv_start, double curv_end);
     ~CAdcGeoSpiral();
     void set_params(double curv_start, double curv_end);
+    void debug(void);
 };
 
 class CAdcGeoArc: public CAdcGeometry
@@ -66,6 +70,7 @@ class CAdcGeoArc: public CAdcGeometry
     CAdcGeoArc(double min_s, double max_s, double x, double y, double heading, double curvature);
     ~CAdcGeoArc();
     void set_params(double curvature);
+    void debug(void);
 };
 
 typedef struct 
@@ -90,6 +95,7 @@ class CAdcGeoPoly3: public CAdcGeometry
     CAdcGeoPoly3(double min_s, double max_s, double x, double y, double heading, Adc_poly3_param param);
     ~CAdcGeoPoly3();
     void set_params(Adc_poly3_param param);
+    void debug(void);
 };
 
 class CAdcGeoParamPoly3: public CAdcGeometry
@@ -108,5 +114,6 @@ class CAdcGeoParamPoly3: public CAdcGeometry
     CAdcGeoParamPoly3(double min_s, double max_s, double x, double y, double heading, Adc_poly3_param u_param, Adc_poly3_param v_param, bool normalized = true);
     ~CAdcGeoParamPoly3();
     void set_params(Adc_poly3_param u_param, Adc_poly3_param v_param, bool normalized = true);
+    void debug(void);
 };
 #endif
\ No newline at end of file
diff --git a/include/adc_road.h b/include/adc_road.h
index aa0cbdc18d6d2cd944ed986c64ba1f1d3b41cc4c..bee76224e9dc0644c6647251ac6c6cd4e1273406 100644
--- a/include/adc_road.h
+++ b/include/adc_road.h
@@ -2,19 +2,22 @@
 #define _ADC_ROAD_H
 
 #include <vector>
+#include <string>
 #include "adc_geometries.h"
 #include "adc_signals.h"
 
+
 class CAdcRoad
 {
   private:
     int id;
+    std::string name;
     double length;
     std::vector<CAdcGeometry*> geometries;
     std::vector<CAdcSignals> signals;
   public:
     CAdcRoad();
-    CAdcRoad(int id, double length);
+    CAdcRoad(int id, double length, std::string name);
     ~CAdcRoad();
 
     void clear_geometries(void);
@@ -25,6 +28,7 @@ class CAdcRoad
     int get_id(void);
     void set_length(double length);
     double get_length(void);
+    void debug(void);
 };
 
 #endif
diff --git a/include/adc_signals.h b/include/adc_signals.h
index 4da369d8fd043753cba175106c9a1f5a8dcb0cc2..a5739aed70532a29c3d9af7c8e9f82780c6cd3e4 100644
--- a/include/adc_signals.h
+++ b/include/adc_signals.h
@@ -38,6 +38,7 @@ class CAdcSignals
     int get_id(void);
     void get_pose(double &s, double &t, double &heading);
     void get_sign_info(std::string &type, std::string &sub_type, int &value, std::string &text);
+    void debug(void);
 };
 
 #endif
diff --git a/include/open_drive_format.h b/include/open_drive_format.h
index 0a5a472f7391469d50bc119d5287843024113b7f..063d3267f1add86132dfbe12f7d3fb56bbbfa2da 100644
--- a/include/open_drive_format.h
+++ b/include/open_drive_format.h
@@ -9,7 +9,7 @@ class COpenDriveFormat
 {
   public:
     COpenDriveFormat();
-    void load(std::string &filename);
+    void load(std::string &filename, bool debug = false);
     ~COpenDriveFormat();
    private:
     CAdcCircuit adc_circuit;
diff --git a/src/adc_circuit.cpp b/src/adc_circuit.cpp
index cebe36434c62ed013d4152fafb507ae25253728a..c2edbd1f972681a86697361d561b15425d8a7c5f 100644
--- a/src/adc_circuit.cpp
+++ b/src/adc_circuit.cpp
@@ -20,3 +20,9 @@ void CAdcCircuit::add_road(CAdcRoad &road)
 {
   this->roads.push_back(road);
 }
+
+void CAdcCircuit::debug(void)
+{
+  for (unsigned int i = 0; i < this->roads.size(); i++)
+    this->roads[i].debug();
+}
\ No newline at end of file
diff --git a/src/adc_geometries.cpp b/src/adc_geometries.cpp
index 0fed433d786c8ddd7a9c154e40151fa6a1791550..4a15e914fb08573c7ddedeb23e030011d6a5bd02 100644
--- a/src/adc_geometries.cpp
+++ b/src/adc_geometries.cpp
@@ -49,6 +49,13 @@ bool CAdcGeometry::on_range(double s)
   return false;
 }
 
+void CAdcGeometry::debug_base(void)
+{
+  std::cout << "  Geometry from " << this->min_s << " to " << this->max_s << std::endl;
+  std::cout << "           pose: x = " << this->x << ", y = " << this->y << ", yaw = " << this->heading << std::endl;
+}
+
+
 ///////////////////// CAdcGeoLine /////////////////////
 CAdcGeoLine::CAdcGeoLine()
 {
@@ -74,6 +81,12 @@ bool CAdcGeoLine::get_local_pose(double s, double t, double heading, double &u,
   return true;
 }
 
+void CAdcGeoLine::debug(void)
+{
+  debug_base();
+}
+
+
 ///////////////////// CAdcGeoSpiral /////////////////////
 CAdcGeoSpiral::CAdcGeoSpiral()
 {
@@ -115,6 +128,13 @@ bool CAdcGeoSpiral::get_local_pose(double s, double t, double heading, double &u
   return false;
 }
 
+void CAdcGeoSpiral::debug(void)
+{
+  debug_base();
+  std::cout << "           params: start_curvature = " << this->curv_start << ", end_curvature = " << this->curv_end << std::endl;
+}
+
+
 ///////////////////// CAdcGeoArc /////////////////////
 CAdcGeoArc::CAdcGeoArc()
 {
@@ -154,6 +174,13 @@ bool CAdcGeoArc::get_local_pose(double s, double t, double heading, double &u, d
   return true;
 }
 
+void CAdcGeoArc::debug(void)
+{
+  debug_base();
+  std::cout << "           params: curvature = " << this->curvature << std::endl;
+}
+
+
 ///////////////////// CAdcGeoPoly3 /////////////////////
 CAdcGeoPoly3::CAdcGeoPoly3()
 {
@@ -203,6 +230,13 @@ bool CAdcGeoPoly3::get_local_pose(double s, double t, double heading, double &u,
   return false;
 }
 
+void CAdcGeoPoly3::debug(void)
+{
+  debug_base();
+  std::cout << "           params: a = " << this->param.a << ", b = " << this->param.b  << ", c = " << this->param.c  << ", d = " << this->param.d << std::endl;
+}
+
+
 ///////////////////// CAdcGeoParamPoly3 /////////////////////
 CAdcGeoParamPoly3::CAdcGeoParamPoly3()
 {
@@ -275,4 +309,12 @@ bool CAdcGeoParamPoly3::get_local_pose(double s, double t, double heading, doubl
   v = this->v_param.a + this->v_param.b*p + this->v_param.c*p2 + this->v_param.d*p3 + t*std::cos(alpha);
   yaw = heading + alpha;
   return true;
-}
\ No newline at end of file
+}
+
+void CAdcGeoParamPoly3::debug(void)
+{
+  debug_base();
+  std::cout << "           U params: a = " << this->u_param.a << ", b = " << this->u_param.b  << ", c = " << this->u_param.c  << ", d = " << this->u_param.d << std::endl;
+  std::cout << "           V params: a = " << this->v_param.a << ", b = " << this->v_param.b  << ", c = " << this->v_param.c  << ", d = " << this->v_param.d << std::endl;
+}
+
diff --git a/src/adc_road.cpp b/src/adc_road.cpp
index aef8e337704e646aba6b87c36e8c2ea14394527c..991ecc886bb3de50cec02ac3746f91f90dab4043 100644
--- a/src/adc_road.cpp
+++ b/src/adc_road.cpp
@@ -1,33 +1,41 @@
 #include "adc_road.h"
 
+#include <iostream> 
+
 CAdcRoad::CAdcRoad()
 {
   this->id = 0;
   this->length = 0.0;
+  this->name = "";
 }
 
-CAdcRoad::CAdcRoad(int id, double length)
+CAdcRoad::CAdcRoad(int id, double length, std::string name)
 {
   this->id = id;
   this->length = length;
+  this->name = name;
 }
 
 CAdcRoad::~CAdcRoad()
 {
+  // std::cout << "road destructor " << this->id << std::endl;
   this->id = 0;
   this->length = 0.0;
+  this->name = "";
   clear_geometries();
   clear_signals();
 }
 
 void CAdcRoad::clear_geometries(void)
 {
+  // for (unsigned int i = 0; i < this->geometries.size(); i++)
+  //   delete this->geometries[i];
   std::vector<CAdcGeometry*>().swap(this->geometries);
 }
 
 void CAdcRoad::add_geometry(CAdcGeometry* geometry)
 {
-  this->geometries.push_back(geometry);
+  this->geometries.emplace_back(geometry);
 }
 
 void CAdcRoad::clear_signals(void)
@@ -59,3 +67,12 @@ double CAdcRoad::get_length(void)
 {
   return this->length;
 }
+
+void CAdcRoad::debug(void)
+{
+  std::cout << "Road " << this->name << " id = " << this->id << "; length = " << this->length << std::endl;
+  for (unsigned int i = 0; i < this->geometries.size(); i++)
+    this->geometries[i]->debug();
+  for (unsigned int i = 0; i < this->signals.size(); i++)
+    this->signals[i].debug();
+}
\ No newline at end of file
diff --git a/src/adc_signals.cpp b/src/adc_signals.cpp
index 40a0c46db3fb73dfe7cb645c0e7d7648534614d6..1de4de82b567dfcc04a9845d5128a63eacd74e10 100644
--- a/src/adc_signals.cpp
+++ b/src/adc_signals.cpp
@@ -1,5 +1,6 @@
 #include "adc_signals.h"
 #include <cmath>
+#include <iostream> 
 
 CAdcSignals::CAdcSignals()
 {
@@ -52,3 +53,10 @@ void CAdcSignals::get_sign_info(std::string &type, std::string &sub_type, int &v
   value = this->value;
   text = this->text;
 }
+
+void CAdcSignals::debug(void)
+{
+  std::cout << "  Signal id = " << this->id << "; type = " << this->type << ", subtype = " << this->sub_type << std::endl;
+  std::cout << "         value = " << this->value << "; text = " << this->text << std::endl;
+  std::cout << "         pose: s = " << this->s << ", t = " << this->t << ", heading = " << this->heading << std::endl;
+}
diff --git a/src/examples/open_drive_format_test.cpp b/src/examples/open_drive_format_test.cpp
index 80a939064dff00e998eee75ea327ae5f53a271e5..96542a3fc4bd96ac6c222663e9e72a180f301099 100644
--- a/src/examples/open_drive_format_test.cpp
+++ b/src/examples/open_drive_format_test.cpp
@@ -9,7 +9,7 @@ int main(int argc, char *argv[])
   // std::string xml_file = "../src/xml/atlatec_vires.xodr";
   try
   {
-    open_drive_format.load(xml_file);
+    open_drive_format.load(xml_file, true);
   }
   catch (CException &e)
   {
diff --git a/src/open_drive_format.cpp b/src/open_drive_format.cpp
index 317090c4b7b860c4e5e524f03c453549f5c90649..f576c15fd4609e8053be61685d9c632d6137eb7f 100644
--- a/src/open_drive_format.cpp
+++ b/src/open_drive_format.cpp
@@ -20,7 +20,7 @@ COpenDriveFormat::COpenDriveFormat()
 {
 }
 
-void COpenDriveFormat::load(std::string &filename)
+void COpenDriveFormat::load(std::string &filename, bool debug)
 {
   struct stat buffer;
 
@@ -37,38 +37,14 @@ void COpenDriveFormat::load(std::string &filename)
         int road_id;
         ss >> road_id;
         double road_length = road_it->length().get();
-        CAdcRoad road(road_id, road_length);
-        
-        std::cout << "Road: id = " << road_it->id().get();
-        std::cout << ", length = " << road_it->length().get();
-        std::cout << ", junction = " << road_it->junction().get() << std::endl;
+        std::string name = road_it->name().get();
+        CAdcRoad road(road_id, road_length, name);
 
         ///////////////////// link atributes
-        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;
-          }
-        }
 
         //////////////// Geometry atributes
         for (planView::geometry_iterator geo_it(road_it->planView().geometry().begin()); geo_it != road_it->planView().geometry().end(); ++geo_it)
         {
-          std::cout << "  Geometry: From s = " << (geo_it->s().present() ? geo_it->s().get() : 0.0); 
-          std::cout << " to s = " << (geo_it->length().present() ? geo_it->length().get() : 0.0) << std::endl;
-          std::cout << "    Origin pose: x = " << (geo_it->x().present() ? geo_it->x().get() : 0.0); 
-          std::cout << "; y = " << (geo_it->y().present() ? geo_it->y().get() : 0.0); 
-          std::cout << "; heading = " << (geo_it->hdg().present() ? geo_it->hdg().get() : 0.0) << std::endl;
-
           double min_s = (geo_it->s().present() ? geo_it->s().get() : 0.0); 
           double max_s = (geo_it->length().present() ? geo_it->length().get() : 0.0);
           double x = (geo_it->x().present() ? geo_it->x().get() : 0.0);
@@ -76,8 +52,6 @@ void COpenDriveFormat::load(std::string &filename)
           double heading = (geo_it->hdg().present() ? geo_it->hdg().get() : 0.0);
           if (geo_it->line().present())
           {
-            std::cout << "    type: Line" <<  std::endl;
-
             CAdcGeoLine line(min_s, max_s, x, y, heading);
             CAdcGeometry* geo;
             geo = &line;
@@ -85,9 +59,6 @@ void COpenDriveFormat::load(std::string &filename)
           }
           else if (geo_it->spiral().present())
           {
-            std::cout << "    type: Spiral-> curvStart = " << (geo_it->spiral().get().curvStart().present() ? geo_it->spiral().get().curvStart().get() : 0.0);
-            std::cout << ",  curvEnd = " << (geo_it->spiral().get().curvEnd().present() ? geo_it->spiral().get().curvEnd().get() : 0.0) << std::endl;
-
             double curv_start = (geo_it->spiral().get().curvStart().present() ? geo_it->spiral().get().curvStart().get() : 0.0);
             double curv_end = (geo_it->spiral().get().curvEnd().present() ? geo_it->spiral().get().curvEnd().get() : 0.0);
             CAdcGeoSpiral spi(min_s, max_s, x, y, heading, curv_start, curv_end);
@@ -97,8 +68,6 @@ void COpenDriveFormat::load(std::string &filename)
           }
           else if (geo_it->arc().present())
           {
-            std::cout << "    type: Arc-> curvature = " << (geo_it->arc().get().curvature().present() ? geo_it->arc().get().curvature().get() : 0.0) <<  std::endl;
-
             double curvature = (geo_it->arc().get().curvature().present() ? geo_it->arc().get().curvature().get() : 0.0);
             CAdcGeoArc arc(min_s, max_s, x, y, heading, curvature);
             CAdcGeometry* geo;
@@ -107,11 +76,6 @@ void COpenDriveFormat::load(std::string &filename)
           }
           else if (geo_it->poly3().present())
           {
-            std::cout << "    type: Poly3-> a = " << (geo_it->poly3().get().a().present() ? geo_it->poly3().get().a().get() : 0.0);
-            std::cout << ", b = " << (geo_it->poly3().get().b().present() ? geo_it->poly3().get().b().get() : 0.0);
-            std::cout << ", c = " << (geo_it->poly3().get().c().present() ? geo_it->poly3().get().c().get() : 0.0);
-            std::cout << ", d = " << (geo_it->poly3().get().d().present() ? geo_it->poly3().get().d().get() : 0.0) <<  std::endl;
-
             Adc_poly3_param param;
             param.a = (geo_it->poly3().get().a().present() ? geo_it->poly3().get().a().get() : 0.0);
             param.b = (geo_it->poly3().get().b().present() ? geo_it->poly3().get().b().get() : 0.0);
@@ -124,16 +88,6 @@ void COpenDriveFormat::load(std::string &filename)
           }
           else if (geo_it->paramPoly3().present())
           {
-            std::cout << "    type: ParamPoly3-> aU = " << (geo_it->paramPoly3().get().aU().present() ? geo_it->paramPoly3().get().aU().get() : 0.0);
-            std::cout << ", bU = " << (geo_it->paramPoly3().get().bU().present() ? geo_it->paramPoly3().get().bU().get() : 0.0);
-            std::cout << ", cU = " << (geo_it->paramPoly3().get().cU().present() ? geo_it->paramPoly3().get().cU().get() : 0.0);
-            std::cout << ", dU = " << (geo_it->paramPoly3().get().dU().present() ? geo_it->paramPoly3().get().dU().get() : 0.0) <<  std::endl;
-            std::cout << "                    -> aV = " << (geo_it->paramPoly3().get().aV().present() ? geo_it->paramPoly3().get().aV().get() : 0.0);
-            std::cout << ", bV = " << (geo_it->paramPoly3().get().bV().present() ? geo_it->paramPoly3().get().bV().get() : 0.0);
-            std::cout << ", cV = " << (geo_it->paramPoly3().get().cV().present() ? geo_it->paramPoly3().get().cV().get() : 0.0);
-            std::cout << ", dV = " << (geo_it->paramPoly3().get().dV().present() ? geo_it->paramPoly3().get().dV().get() : 0.0) <<  std::endl;
-            std::cout << "                    -> pRange = " << (geo_it->paramPoly3().get().pRange().present() ? geo_it->paramPoly3().get().pRange().get() : 0.0) <<  std::endl;
-
             Adc_poly3_param u_param;
             u_param.a = (geo_it->paramPoly3().get().aU().present() ? geo_it->paramPoly3().get().aU().get() : 0.0);
             u_param.b = (geo_it->paramPoly3().get().bU().present() ? geo_it->paramPoly3().get().bU().get() : 0.0);
@@ -150,6 +104,8 @@ void COpenDriveFormat::load(std::string &filename)
             CAdcGeoParamPoly3 parampoly3(min_s, max_s, x, y, heading, u_param, v_param, normalized);
             CAdcGeometry* geo;
             geo = &parampoly3;
+
+            // geo->debug();
             road.add_geometry(geo);
           }
         }
@@ -159,18 +115,6 @@ void COpenDriveFormat::load(std::string &filename)
         {
           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;
-
             std::stringstream sss(signal_it->id().get());
             int id;
             sss >> id;
@@ -189,8 +133,10 @@ void COpenDriveFormat::load(std::string &filename)
           }
         }
         this->adc_circuit.add_road(road);
-        std::cout << std::endl;
       }
+
+      if (debug)
+        adc_circuit.debug();
       std::cout << "Done." << std::endl;
     }catch (const xml_schema::exception& e){
       std::ostringstream os;
@@ -202,6 +148,190 @@ void COpenDriveFormat::load(std::string &filename)
   else
     throw CException(_HERE_,"The configuration file does not exist");
 }
+
+// void COpenDriveFormat::load(std::string &filename)
+// {
+//   struct stat buffer;
+
+//   if(stat(filename.c_str(),&buffer)==0)
+//   {
+//     // try to open the specified file
+//     try{
+//       std::auto_ptr<OpenDRIVE> open_drive(OpenDRIVE_(filename.c_str(), xml_schema::flags::dont_validate));
+
+//       ////////////////// Access road atributes
+//       for (OpenDRIVE::road_iterator road_it(open_drive->road().begin()); road_it != open_drive->road().end(); ++road_it)
+//       {
+//         std::stringstream ss(road_it->id().get());
+//         int road_id;
+//         ss >> road_id;
+//         double road_length = road_it->length().get();
+//         std::string name = road_it->name().get();
+//         CAdcRoad road(road_id, road_length, name);
+        
+//         std::cout << "Road: id = " << road_it->id().get();
+//         std::cout << ", length = " << road_it->length().get();
+//         std::cout << ", junction = " << road_it->junction().get() << std::endl;
+
+//         ///////////////////// link atributes
+//         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;
+//           }
+//         }
+
+//         //////////////// Geometry atributes
+//         for (planView::geometry_iterator geo_it(road_it->planView().geometry().begin()); geo_it != road_it->planView().geometry().end(); ++geo_it)
+//         {
+//           std::cout << "  Geometry: From s = " << (geo_it->s().present() ? geo_it->s().get() : 0.0); 
+//           std::cout << " to s = " << (geo_it->length().present() ? geo_it->length().get() : 0.0) << std::endl;
+//           std::cout << "    Origin pose: x = " << (geo_it->x().present() ? geo_it->x().get() : 0.0); 
+//           std::cout << "; y = " << (geo_it->y().present() ? geo_it->y().get() : 0.0); 
+//           std::cout << "; heading = " << (geo_it->hdg().present() ? geo_it->hdg().get() : 0.0) << std::endl;
+
+//           double min_s = (geo_it->s().present() ? geo_it->s().get() : 0.0); 
+//           double max_s = (geo_it->length().present() ? geo_it->length().get() : 0.0);
+//           double x = (geo_it->x().present() ? geo_it->x().get() : 0.0);
+//           double y = (geo_it->y().present() ? geo_it->y().get() : 0.0);
+//           double heading = (geo_it->hdg().present() ? geo_it->hdg().get() : 0.0);
+//           if (geo_it->line().present())
+//           {
+//             std::cout << "    type: Line" <<  std::endl;
+
+//             CAdcGeoLine line(min_s, max_s, x, y, heading);
+//             CAdcGeometry* geo;
+//             geo = &line;
+//             road.add_geometry(geo);
+//           }
+//           else if (geo_it->spiral().present())
+//           {
+//             std::cout << "    type: Spiral-> curvStart = " << (geo_it->spiral().get().curvStart().present() ? geo_it->spiral().get().curvStart().get() : 0.0);
+//             std::cout << ",  curvEnd = " << (geo_it->spiral().get().curvEnd().present() ? geo_it->spiral().get().curvEnd().get() : 0.0) << std::endl;
+
+//             double curv_start = (geo_it->spiral().get().curvStart().present() ? geo_it->spiral().get().curvStart().get() : 0.0);
+//             double curv_end = (geo_it->spiral().get().curvEnd().present() ? geo_it->spiral().get().curvEnd().get() : 0.0);
+//             CAdcGeoSpiral spi(min_s, max_s, x, y, heading, curv_start, curv_end);
+//             CAdcGeometry* geo;
+//             geo = &spi;
+//             road.add_geometry(geo);
+//           }
+//           else if (geo_it->arc().present())
+//           {
+//             std::cout << "    type: Arc-> curvature = " << (geo_it->arc().get().curvature().present() ? geo_it->arc().get().curvature().get() : 0.0) <<  std::endl;
+
+//             double curvature = (geo_it->arc().get().curvature().present() ? geo_it->arc().get().curvature().get() : 0.0);
+//             CAdcGeoArc arc(min_s, max_s, x, y, heading, curvature);
+//             CAdcGeometry* geo;
+//             geo = &arc;
+//             road.add_geometry(geo);
+//           }
+//           else if (geo_it->poly3().present())
+//           {
+//             std::cout << "    type: Poly3-> a = " << (geo_it->poly3().get().a().present() ? geo_it->poly3().get().a().get() : 0.0);
+//             std::cout << ", b = " << (geo_it->poly3().get().b().present() ? geo_it->poly3().get().b().get() : 0.0);
+//             std::cout << ", c = " << (geo_it->poly3().get().c().present() ? geo_it->poly3().get().c().get() : 0.0);
+//             std::cout << ", d = " << (geo_it->poly3().get().d().present() ? geo_it->poly3().get().d().get() : 0.0) <<  std::endl;
+
+//             Adc_poly3_param param;
+//             param.a = (geo_it->poly3().get().a().present() ? geo_it->poly3().get().a().get() : 0.0);
+//             param.b = (geo_it->poly3().get().b().present() ? geo_it->poly3().get().b().get() : 0.0);
+//             param.c = (geo_it->poly3().get().c().present() ? geo_it->poly3().get().c().get() : 0.0);
+//             param.d = (geo_it->poly3().get().d().present() ? geo_it->poly3().get().d().get() : 0.0);
+//             CAdcGeoPoly3 poly3(min_s, max_s, x, y, heading, param);
+//             CAdcGeometry* geo;
+//             geo = &poly3;
+//             road.add_geometry(geo);
+//           }
+//           else if (geo_it->paramPoly3().present())
+//           {
+//             std::cout << "    type: ParamPoly3-> aU = " << (geo_it->paramPoly3().get().aU().present() ? geo_it->paramPoly3().get().aU().get() : 0.0);
+//             std::cout << ", bU = " << (geo_it->paramPoly3().get().bU().present() ? geo_it->paramPoly3().get().bU().get() : 0.0);
+//             std::cout << ", cU = " << (geo_it->paramPoly3().get().cU().present() ? geo_it->paramPoly3().get().cU().get() : 0.0);
+//             std::cout << ", dU = " << (geo_it->paramPoly3().get().dU().present() ? geo_it->paramPoly3().get().dU().get() : 0.0) <<  std::endl;
+//             std::cout << "                    -> aV = " << (geo_it->paramPoly3().get().aV().present() ? geo_it->paramPoly3().get().aV().get() : 0.0);
+//             std::cout << ", bV = " << (geo_it->paramPoly3().get().bV().present() ? geo_it->paramPoly3().get().bV().get() : 0.0);
+//             std::cout << ", cV = " << (geo_it->paramPoly3().get().cV().present() ? geo_it->paramPoly3().get().cV().get() : 0.0);
+//             std::cout << ", dV = " << (geo_it->paramPoly3().get().dV().present() ? geo_it->paramPoly3().get().dV().get() : 0.0) <<  std::endl;
+//             std::cout << "                    -> pRange = " << (geo_it->paramPoly3().get().pRange().present() ? geo_it->paramPoly3().get().pRange().get() : 0.0) <<  std::endl;
+
+//             Adc_poly3_param u_param;
+//             u_param.a = (geo_it->paramPoly3().get().aU().present() ? geo_it->paramPoly3().get().aU().get() : 0.0);
+//             u_param.b = (geo_it->paramPoly3().get().bU().present() ? geo_it->paramPoly3().get().bU().get() : 0.0);
+//             u_param.c = (geo_it->paramPoly3().get().cU().present() ? geo_it->paramPoly3().get().cU().get() : 0.0);
+//             u_param.d = (geo_it->paramPoly3().get().dU().present() ? geo_it->paramPoly3().get().dU().get() : 0.0);
+//             Adc_poly3_param v_param;
+//             v_param.a = (geo_it->paramPoly3().get().aV().present() ? geo_it->paramPoly3().get().aV().get() : 0.0);
+//             v_param.b = (geo_it->paramPoly3().get().bV().present() ? geo_it->paramPoly3().get().bV().get() : 0.0);
+//             v_param.c = (geo_it->paramPoly3().get().cV().present() ? geo_it->paramPoly3().get().cV().get() : 0.0);
+//             v_param.d = (geo_it->paramPoly3().get().dV().present() ? geo_it->paramPoly3().get().dV().get() : 0.0);
+//             bool normalized = true;
+//             if (geo_it->paramPoly3().get().pRange().present() && geo_it->paramPoly3().get().pRange().get() == pRange::arcLength)
+//               normalized = false;
+//             CAdcGeoParamPoly3 parampoly3(min_s, max_s, x, y, heading, u_param, v_param, normalized);
+//             CAdcGeometry* geo;
+//             geo = &parampoly3;
+//             road.add_geometry(geo);
+//           }
+//         }
+
+//         ////////// Signals atributes
+//         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;
+
+//             std::stringstream sss(signal_it->id().get());
+//             int id;
+//             sss >> id;
+//             double s = (signal_it->s().present() ? signal_it->s().get() : 0.0);
+//             double t = (signal_it->t().present() ? signal_it->t().get() : 0.0);
+//             double heading = (signal_it->hOffset().present() ? signal_it->hOffset().get() : 0.0);
+//             std::string type = (signal_it->type().present() ? signal_it->type().get() : "");
+//             std::string sub_type = (signal_it->subtype().present() ? signal_it->subtype().get() : "");
+//             double value = (signal_it->value().present() ? signal_it->value().get() : 0.0);
+//             std::string text = (signal_it->text().present() ? signal_it->text().get() : "");
+//             bool reverse = false;
+//             if (signal_it->orientation().present() && signal_it->orientation().get() == orientation::cxx_1)
+//               reverse = true;
+//             CAdcSignals sign(id, s, t, heading, type, sub_type, value, text, reverse);
+//             road.add_signal(sign);
+//           }
+//         }
+//         this->adc_circuit.add_road(road);
+//         std::cout << std::endl;
+//       }
+//       std::cout << "Done." << std::endl;
+//     }catch (const xml_schema::exception& e){
+//       std::ostringstream os;
+//       os << e;
+//       /* handle exceptions */
+//       throw CException(_HERE_,os.str());
+//     }
+//   }
+//   else
+//     throw CException(_HERE_,"The configuration file does not exist");
+// }
  
 COpenDriveFormat::~COpenDriveFormat()
 {