Fixed bugs. Print debug info working

include/adc_geometries.h

@@ -12,30 +12,30 @@ class CAdcGeometry
     double y;
     double heading;
 
+    virtual void debug_param(void) = 0;
     virtual bool get_local_pose(double s, double t, double heading, double &u, double &v, double &yaw) = 0;
 
   public:
     CAdcGeometry();
     CAdcGeometry(double min_s, double max_s, double x, double y, double heading);
-    ~CAdcGeometry();
+    virtual ~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;
+    void debug(void);
 };
 
 class CAdcGeoLine: public CAdcGeometry
 {
   private:
   protected:
+    void debug_param(void);
     bool get_local_pose(double s, double t, double heading, double &u, double &v, double &yaw);
 
   public:
     CAdcGeoLine();
     CAdcGeoLine(double min_s, double max_s, double x, double y, double heading);
     ~CAdcGeoLine();
-    void debug(void);
 };
 
 class CAdcGeoSpiral: public CAdcGeometry
@@ -45,6 +45,7 @@ class CAdcGeoSpiral: public CAdcGeometry
     double curv_end;
 
   protected:
+    void debug_param(void);
     bool get_local_pose(double s, double t, double heading, double &u, double &v, double &yaw);
 
   public:
@@ -53,7 +54,6 @@ 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
@@ -62,6 +62,7 @@ class CAdcGeoArc: public CAdcGeometry
     double curvature;
 
   protected:
+    void debug_param(void);
     bool get_local_pose(double s, double t, double heading, double &u, double &v, double &yaw);
 
   public:
@@ -70,7 +71,6 @@ 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 
@@ -87,6 +87,7 @@ class CAdcGeoPoly3: public CAdcGeometry
     Adc_poly3_param param;
 
   protected:
+    void debug_param(void);
     bool get_local_pose(double s, double t, double heading, double &u, double &v, double &yaw);
 
   public:
@@ -95,7 +96,6 @@ 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
@@ -106,6 +106,7 @@ class CAdcGeoParamPoly3: public CAdcGeometry
     bool normalized;
 
   protected:
+    void debug_param(void);
     bool get_local_pose(double s, double t, double heading, double &u, double &v, double &yaw);
 
   public:
@@ -114,6 +115,5 @@ 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

include/adc_road.h

@@ -22,6 +22,7 @@ class CAdcRoad
 
     void clear_geometries(void);
     void add_geometry(CAdcGeometry* geometry);
+    void delete_geometries(void);
     void clear_signals(void);
     void add_signal(CAdcSignals sign);
     void set_id(int id);

src/adc_circuit.cpp

@@ -8,6 +8,8 @@ CAdcCircuit::CAdcCircuit()
 
 CAdcCircuit::~CAdcCircuit()
 {
+  for (unsigned int i = 0; i < this->roads.size(); i++)
+    this->roads[i].delete_geometries();
   clear_roads();
 }
 

src/adc_geometries.cpp

@@ -49,10 +49,11 @@ bool CAdcGeometry::on_range(double s)
   return false;
 }
 
-void CAdcGeometry::debug_base(void)
+void CAdcGeometry::debug(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;
+  debug_param();
 }
 
 
@@ -81,9 +82,9 @@ bool CAdcGeoLine::get_local_pose(double s, double t, double heading, double &u,
   return true;
 }
 
-void CAdcGeoLine::debug(void)
+void CAdcGeoLine::debug_param(void)
 {
-  debug_base();
+
 }
 
 
@@ -128,9 +129,8 @@ bool CAdcGeoSpiral::get_local_pose(double s, double t, double heading, double &u
   return false;
 }
 
-void CAdcGeoSpiral::debug(void)
+void CAdcGeoSpiral::debug_param(void)
 {
-  debug_base();
   std::cout << "           params: start_curvature = " << this->curv_start << ", end_curvature = " << this->curv_end << std::endl;
 }
 
@@ -174,9 +174,8 @@ bool CAdcGeoArc::get_local_pose(double s, double t, double heading, double &u, d
   return true;
 }
 
-void CAdcGeoArc::debug(void)
+void CAdcGeoArc::debug_param(void)
 {
-  debug_base();
   std::cout << "           params: curvature = " << this->curvature << std::endl;
 }
 
@@ -230,9 +229,8 @@ bool CAdcGeoPoly3::get_local_pose(double s, double t, double heading, double &u,
   return false;
 }
 
-void CAdcGeoPoly3::debug(void)
+void CAdcGeoPoly3::debug_param(void)
 {
-  debug_base();
   std::cout << "           params: a = " << this->param.a << ", b = " << this->param.b  << ", c = " << this->param.c  << ", d = " << this->param.d << std::endl;
 }
 
@@ -311,9 +309,8 @@ bool CAdcGeoParamPoly3::get_local_pose(double s, double t, double heading, doubl
   return true;
 }
 
-void CAdcGeoParamPoly3::debug(void)
+void CAdcGeoParamPoly3::debug_param(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;
 }

src/adc_road.cpp

@@ -33,10 +33,16 @@ void CAdcRoad::clear_geometries(void)
   std::vector<CAdcGeometry*>().swap(this->geometries);
 }
 
+void CAdcRoad::delete_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)

src/open_drive_format.cpp

@@ -46,33 +46,27 @@ void COpenDriveFormat::load(std::string &filename, bool debug)
         for (planView::geometry_iterator geo_it(road_it->planView().geometry().begin()); geo_it != road_it->planView().geometry().end(); ++geo_it)
         {
           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 max_s = min_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())
           {
-            CAdcGeoLine line(min_s, max_s, x, y, heading);
-            CAdcGeometry* geo;
-            geo = &line;
-            road.add_geometry(geo);
+            CAdcGeoLine *line = new CAdcGeoLine(min_s, max_s, x, y, heading);
+            road.add_geometry(line);
           }
           else if (geo_it->spiral().present())
           {
             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);
+            CAdcGeoSpiral *spi = new CAdcGeoSpiral(min_s, max_s, x, y, heading, curv_start, curv_end);
+            road.add_geometry(spi);
           }
           else if (geo_it->arc().present())
           {
             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);
+            CAdcGeoArc *arc = new CAdcGeoArc(min_s, max_s, x, y, heading, curvature);
+            road.add_geometry(arc);
           }
           else if (geo_it->poly3().present())
           {
@@ -81,10 +75,8 @@ void COpenDriveFormat::load(std::string &filename, bool debug)
             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);
+            CAdcGeoPoly3 *poly3 = new CAdcGeoPoly3(min_s, max_s, x, y, heading, param);
+            road.add_geometry(poly3);
           }
           else if (geo_it->paramPoly3().present())
           {
@@ -101,12 +93,8 @@ void COpenDriveFormat::load(std::string &filename, bool debug)
             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;
-
-            // geo->debug();
-            road.add_geometry(geo);
+            CAdcGeoParamPoly3 *parampoly3 = new CAdcGeoParamPoly3(min_s, max_s, x, y, heading, u_param, v_param, normalized);
+            road.add_geometry(parampoly3);
           }
         }