Skip to content
Snippets Groups Projects
Commit c23e5ead authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Now each class extracts its useful information from .xodr files

parent ce92e0af
No related branches found
No related tags found
No related merge requests found
...@@ -7,6 +7,10 @@ ...@@ -7,6 +7,10 @@
#include <vector> #include <vector>
#ifdef _HAVE_XSD
#include "../src/xml/OpenDRIVE_1.4H.hxx"
#endif
/** /**
* \class CAdcCircuit * \class CAdcCircuit
* *
...@@ -20,6 +24,20 @@ class CAdcCircuit ...@@ -20,6 +24,20 @@ class CAdcCircuit
private: private:
std::vector<CAdcRoad*> roads;///< Variable to store an access each road data. std::vector<CAdcRoad*> roads;///< Variable to store an access each road data.
/**
* \brief Function to add a road.
*
* \param road The road to add.
*
*/
inline void add_road(CAdcRoad* &road);
/**
* \brief Function to clear roads.
*
*/
inline void clear_roads(void);
public: public:
/** /**
* \brief Default constructor. * \brief Default constructor.
...@@ -35,20 +53,6 @@ class CAdcCircuit ...@@ -35,20 +53,6 @@ class CAdcCircuit
*/ */
~CAdcCircuit(); ~CAdcCircuit();
/**
* \brief Function to clear roads.
*
*/
void clear_roads(void);
/**
* \brief Function to add a road.
*
* \param road The road to add.
*
*/
void add_road(CAdcRoad* &road);
/** /**
* \brief Function to print each road info. * \brief Function to print each road info.
* *
...@@ -89,6 +93,14 @@ class CAdcCircuit ...@@ -89,6 +93,14 @@ class CAdcCircuit
* \throws CException If the output file can't be opened. * \throws CException If the output file can't be opened.
*/ */
void generate_spwan_launch_file(std::string &filename); void generate_spwan_launch_file(std::string &filename);
/**
* \brief Function to load the .xodr info.
*
* \param open_drive The objects generated with XSD.
*
*/
void load(std::auto_ptr<OpenDRIVE> &open_drive);
}; };
#endif #endif
\ No newline at end of file
#ifndef _ADC_GEOMETRIES_H #ifndef _ADC_GEOMETRIES_H
#define _ADC_GEOMETRIES_H #define _ADC_GEOMETRIES_H
#ifdef _HAVE_XSD
#include "../src/xml/OpenDRIVE_1.4H.hxx"
#endif
/** /**
* \class CAdcGeometry * \class CAdcGeometry
* *
...@@ -45,6 +49,14 @@ class CAdcGeometry ...@@ -45,6 +49,14 @@ class CAdcGeometry
*/ */
virtual bool get_local_pose(double s, double t, double heading, double &u, double &v, double &yaw) = 0; virtual bool get_local_pose(double s, double t, double heading, double &u, double &v, double &yaw) = 0;
/**
* \brief Function to load the .xodr geometry specific info.
*
* \param geometry_info The objects generated with XSD.
*
*/
virtual void load_param(std::auto_ptr<planView::geometry_type> &geometry_info) = 0;
public: public:
/** /**
* \brief Default constructor. * \brief Default constructor.
...@@ -133,6 +145,30 @@ class CAdcGeometry ...@@ -133,6 +145,30 @@ class CAdcGeometry
* *
*/ */
void debug(void); void debug(void);
/**
* \brief Function to load the .xodr info.
*
* \param geometry_info The objects generated with XSD.
*
*/
void load(std::auto_ptr<planView::geometry_type> &geometry_info);
/**
* \brief Function to set the common geometry parameters.
*
* \param min_s Geometry's min_s.
*
* \param max_s Geometry's max_s.
*
* \param x Geometry's origin x coordenate.
*
* \param y Geometry's origin y coordenate.
*
* \param heading Geometry's origins heading.
*
*/
// set_base_paremeters(double min_s, double max_s, double x, double y, double heading);
}; };
...@@ -225,6 +261,14 @@ class CAdcGeoLine: public CAdcGeometry ...@@ -225,6 +261,14 @@ class CAdcGeoLine: public CAdcGeometry
* *
*/ */
CAdcGeometry* clone() const; CAdcGeometry* clone() const;
/**
* \brief Function to load the .xodr info.
*
* \param geometry_info The objects generated with XSD.
*
*/
void load_param(std::auto_ptr<planView::geometry_type> &geometry_info);
}; };
...@@ -351,7 +395,15 @@ class CAdcGeoSpiral: public CAdcGeometry ...@@ -351,7 +395,15 @@ class CAdcGeoSpiral: public CAdcGeometry
* *
*/ */
void set_params(double curv_start, double curv_end); void set_params(double curv_start, double curv_end);
};
/**
* \brief Function to load the .xodr info.
*
* \param geometry_info The objects generated with XSD.
*
*/
void load_param(std::auto_ptr<planView::geometry_type> &geometry_info);
};
/** /**
...@@ -469,6 +521,14 @@ class CAdcGeoArc: public CAdcGeometry ...@@ -469,6 +521,14 @@ class CAdcGeoArc: public CAdcGeometry
* *
*/ */
void set_params(double curvature); void set_params(double curvature);
/**
* \brief Function to load the .xodr info.
*
* \param geometry_info The objects generated with XSD.
*
*/
void load_param(std::auto_ptr<planView::geometry_type> &geometry_info);
}; };
...@@ -602,6 +662,14 @@ class CAdcGeoPoly3: public CAdcGeometry ...@@ -602,6 +662,14 @@ class CAdcGeoPoly3: public CAdcGeometry
* *
*/ */
void set_params(Adc_poly3_param param); void set_params(Adc_poly3_param param);
/**
* \brief Function to load the .xodr info.
*
* \param geometry_info The objects generated with XSD.
*
*/
void load_param(std::auto_ptr<planView::geometry_type> &geometry_info);
}; };
...@@ -731,5 +799,13 @@ class CAdcGeoParamPoly3: public CAdcGeometry ...@@ -731,5 +799,13 @@ class CAdcGeoParamPoly3: public CAdcGeometry
* *
*/ */
void set_params(Adc_poly3_param u_param, Adc_poly3_param v_param, bool normalized = true); void set_params(Adc_poly3_param u_param, Adc_poly3_param v_param, bool normalized = true);
/**
* \brief Function to load the .xodr info.
*
* \param geometry_info The objects generated with XSD.
*
*/
void load_param(std::auto_ptr<planView::geometry_type> &geometry_info);
}; };
#endif #endif
\ No newline at end of file
...@@ -6,7 +6,9 @@ ...@@ -6,7 +6,9 @@
#include "adc_geometries.h" #include "adc_geometries.h"
#include "adc_signals.h" #include "adc_signals.h"
#ifdef _HAVE_XSD
#include "../src/xml/OpenDRIVE_1.4H.hxx"
#endif
/** /**
* \class CAdcRoad * \class CAdcRoad
* *
...@@ -24,6 +26,34 @@ class CAdcRoad ...@@ -24,6 +26,34 @@ class CAdcRoad
std::vector<CAdcGeometry*> geometries;///< Road's geometries. std::vector<CAdcGeometry*> geometries;///< Road's geometries.
std::vector<CAdcSignals*> signals; ///< Road's signals. std::vector<CAdcSignals*> signals; ///< Road's signals.
/**
* \brief Function to clear geometries.
*
*/
inline void clear_geometries(void);
/**
* \brief Function to add a geometry.
*
* \param geometry The road's geometry to add.
*
*/
inline void add_geometry(CAdcGeometry* geometry);
/**
* \brief Function to clear signals.
*
*/
inline void clear_signals(void);
/**
* \brief Function to add a sign.
*
* \param sign The road's sign to add.
*
*/
inline void add_signal(CAdcSignals* sign);
public: public:
/** /**
* \brief Default constructor. * \brief Default constructor.
...@@ -61,34 +91,6 @@ class CAdcRoad ...@@ -61,34 +91,6 @@ class CAdcRoad
*/ */
void operator = (const CAdcRoad &road); void operator = (const CAdcRoad &road);
/**
* \brief Function to clear geometries.
*
*/
void clear_geometries(void);
/**
* \brief Function to add a geometry.
*
* \param geometry The road's geometry to add.
*
*/
void add_geometry(CAdcGeometry* geometry);
/**
* \brief Function to clear signals.
*
*/
void clear_signals(void);
/**
* \brief Function to add a sign.
*
* \param sign The road's sign to add.
*
*/
void add_signal(CAdcSignals* sign);
/** /**
* \brief Function to set road's id. * \brief Function to set road's id.
* *
...@@ -157,6 +159,14 @@ class CAdcRoad ...@@ -157,6 +159,14 @@ class CAdcRoad
* *
*/ */
void append_signs_spawn(std::string &filename); void append_signs_spawn(std::string &filename);
/**
* \brief Function to load the .xodr info.
*
* \param road_info The objects generated with XSD.
*
*/
void load(std::auto_ptr<OpenDRIVE::road_type> &road_info);
}; };
#endif #endif
...@@ -3,22 +3,26 @@ ...@@ -3,22 +3,26 @@
#include <string> #include <string>
#include <map> #include <map>
#ifdef _HAVE_XSD
#include "../src/xml/OpenDRIVE_1.4H.hxx"
#endif
#define UNMARKEDINTERSECTION_TYPE "102" #define UNMARKEDINTERSECTION_TYPE "102"
#define UNMARKEDINTERSECTION_MARKER "alvar0" #define UNMARKEDINTERSECTION_MARKER "alvar0"
#define STOPANDGIVEWAY_TYPE "206" #define STOPANDGIVEWAY_TYPE "206"
#define STOPANDGIVEWAY_MARKER "alvar1" #define STOPANDGIVEWAY_MARKER "alvar1"
#define PARKINGAREA_TYPE "???" #define PARKINGAREA_TYPE "314"
#define PARKINGAREA_MARKER "alvar2" #define PARKINGAREA_MARKER "alvar2"
#define HAVEWAY_TYPE "???" #define HAVEWAY_TYPE "301"
#define HAVEWAY_MARKER "alvar3" #define HAVEWAY_MARKER "alvar3"
#define AHEADONLY_TYPE "209" #define AHEADONLY_TYPE "209"
#define AHEADONLY_SUB_TYPE "30" #define AHEADONLY_SUB_TYPE "30"
#define AHEADONLY_MARKER "alvar4" #define AHEADONLY_MARKER "alvar4"
#define GIVEWAY_TYPE "205" #define GIVEWAY_TYPE "205"
#define GIVEWAY_MARKER "alvar5" #define GIVEWAY_MARKER "alvar5"
#define PEDESTRIANCROSSING_TYPE "???" #define PEDESTRIANCROSSING_TYPE "350"
#define PEDESTRIANCROSSING_MARKER "alvar6" #define PEDESTRIANCROSSING_MARKER "alvar6"
#define ROUNDABOUT_TYPE "???" #define ROUNDABOUT_TYPE "215"
#define ROUNDABOUT_MARKER "alvar7" #define ROUNDABOUT_MARKER "alvar7"
#define NOOVERTAKING_TYPE "276" #define NOOVERTAKING_TYPE "276"
#define NOOVERTAKING_MARKER "alvar8" #define NOOVERTAKING_MARKER "alvar8"
...@@ -194,6 +198,14 @@ class CAdcSignals ...@@ -194,6 +198,14 @@ class CAdcSignals
* *
*/ */
void append_sign_spawn(std::string &filename); void append_sign_spawn(std::string &filename);
/**
* \brief Function to load the .xodr info.
*
* \param signal_info The objects generated with XSD.
*
*/
void load(std::auto_ptr<signals::signal_type> &signal_info);
}; };
#endif #endif
...@@ -75,5 +75,16 @@ void CAdcCircuit::generate_spwan_launch_file(std::string &filename) ...@@ -75,5 +75,16 @@ void CAdcCircuit::generate_spwan_launch_file(std::string &filename)
throw CException(_HERE_,"CAdcCircuit::generate_spwan_launch_file -> The output file can't be open"); throw CException(_HERE_,"CAdcCircuit::generate_spwan_launch_file -> The output file can't be open");
out_file << oss.str() << std::endl; out_file << oss.str() << std::endl;
out_file.close(); out_file.close();
}
void CAdcCircuit::load(std::auto_ptr<OpenDRIVE> &open_drive)
{
////////////////// Access road atributes
for (OpenDRIVE::road_iterator road_it(open_drive->road().begin()); road_it != open_drive->road().end(); ++road_it)
{
CAdcRoad *road = new CAdcRoad();
std::auto_ptr<OpenDRIVE::road_type> road_info(new OpenDRIVE::road_type(*road_it));
road->load(road_info);
add_road(road);
}
} }
\ No newline at end of file
...@@ -75,6 +75,25 @@ void CAdcGeometry::debug(void) ...@@ -75,6 +75,25 @@ void CAdcGeometry::debug(void)
debug_param(); debug_param();
} }
void CAdcGeometry::load(std::auto_ptr<planView::geometry_type> &geometry_info)
{
this->min_s = (geometry_info->s().present() ? geometry_info->s().get() : 0.0);
this->max_s = min_s + (geometry_info->length().present() ? geometry_info->length().get() : 0.0);
this->x = (geometry_info->x().present() ? geometry_info->x().get() : 0.0);
this->y = (geometry_info->y().present() ? geometry_info->y().get() : 0.0);
this->heading = (geometry_info->hdg().present() ? geometry_info->hdg().get() : 0.0);
load_param(geometry_info);
}
// CAdcGeometry::set_base_paremeters(double min_s, double max_s, double x, double y, double heading)
// {
// this->min_s = min_s;
// this->max_s = max_s;
// this->x = x;
// this->y = y;
// this->heading = heading;
// }
///////////////////// CAdcGeoLine ///////////////////// ///////////////////// CAdcGeoLine /////////////////////
CAdcGeoLine::CAdcGeoLine() CAdcGeoLine::CAdcGeoLine()
...@@ -122,6 +141,11 @@ void CAdcGeoLine::debug_param(void) ...@@ -122,6 +141,11 @@ void CAdcGeoLine::debug_param(void)
} }
void CAdcGeoLine::load_param(std::auto_ptr<planView::geometry_type> &geometry_info)
{
}
///////////////////// CAdcGeoSpiral ///////////////////// ///////////////////// CAdcGeoSpiral /////////////////////
CAdcGeoSpiral::CAdcGeoSpiral() CAdcGeoSpiral::CAdcGeoSpiral()
...@@ -188,6 +212,12 @@ void CAdcGeoSpiral::debug_param(void) ...@@ -188,6 +212,12 @@ void CAdcGeoSpiral::debug_param(void)
std::cout << " params: start_curvature = " << this->curv_start << ", end_curvature = " << this->curv_end << std::endl; std::cout << " params: start_curvature = " << this->curv_start << ", end_curvature = " << this->curv_end << std::endl;
} }
void CAdcGeoSpiral::load_param(std::auto_ptr<planView::geometry_type> &geometry_info)
{
this->curv_start = (geometry_info->spiral().get().curvStart().present() ? geometry_info->spiral().get().curvStart().get() : 0.0);
this->curv_end = (geometry_info->spiral().get().curvEnd().present() ? geometry_info->spiral().get().curvEnd().get() : 0.0);
}
///////////////////// CAdcGeoArc ///////////////////// ///////////////////// CAdcGeoArc /////////////////////
CAdcGeoArc::CAdcGeoArc() CAdcGeoArc::CAdcGeoArc()
...@@ -250,6 +280,11 @@ void CAdcGeoArc::debug_param(void) ...@@ -250,6 +280,11 @@ void CAdcGeoArc::debug_param(void)
std::cout << " params: curvature = " << this->curvature << std::endl; std::cout << " params: curvature = " << this->curvature << std::endl;
} }
void CAdcGeoArc::load_param(std::auto_ptr<planView::geometry_type> &geometry_info)
{
this->curvature = (geometry_info->arc().get().curvature().present() ? geometry_info->arc().get().curvature().get() : 0.0);
}
///////////////////// CAdcGeoPoly3 ///////////////////// ///////////////////// CAdcGeoPoly3 /////////////////////
CAdcGeoPoly3::CAdcGeoPoly3() CAdcGeoPoly3::CAdcGeoPoly3()
...@@ -327,6 +362,14 @@ void CAdcGeoPoly3::debug_param(void) ...@@ -327,6 +362,14 @@ void CAdcGeoPoly3::debug_param(void)
{ {
std::cout << " params: a = " << this->param.a << ", b = " << this->param.b << ", c = " << this->param.c << ", d = " << this->param.d << std::endl; std::cout << " params: a = " << this->param.a << ", b = " << this->param.b << ", c = " << this->param.c << ", d = " << this->param.d << std::endl;
} }
void CAdcGeoPoly3::load_param(std::auto_ptr<planView::geometry_type> &geometry_info)
{
this->param.a = (geometry_info->poly3().get().a().present() ? geometry_info->poly3().get().a().get() : 0.0);
this->param.b = (geometry_info->poly3().get().b().present() ? geometry_info->poly3().get().b().get() : 0.0);
this->param.c = (geometry_info->poly3().get().c().present() ? geometry_info->poly3().get().c().get() : 0.0);
this->param.d = (geometry_info->poly3().get().d().present() ? geometry_info->poly3().get().d().get() : 0.0);
}
///////////////////// CAdcGeoParamPoly3 ///////////////////// ///////////////////// CAdcGeoParamPoly3 /////////////////////
...@@ -442,3 +485,17 @@ void CAdcGeoParamPoly3::debug_param(void) ...@@ -442,3 +485,17 @@ void CAdcGeoParamPoly3::debug_param(void)
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; 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;
} }
void CAdcGeoParamPoly3::load_param(std::auto_ptr<planView::geometry_type> &geometry_info)
{
this->u_param.a = (geometry_info->paramPoly3().get().aU().present() ? geometry_info->paramPoly3().get().aU().get() : 0.0);
this->u_param.b = (geometry_info->paramPoly3().get().bU().present() ? geometry_info->paramPoly3().get().bU().get() : 0.0);
this->u_param.c = (geometry_info->paramPoly3().get().cU().present() ? geometry_info->paramPoly3().get().cU().get() : 0.0);
this->u_param.d = (geometry_info->paramPoly3().get().dU().present() ? geometry_info->paramPoly3().get().dU().get() : 0.0);
this->v_param.a = (geometry_info->paramPoly3().get().aV().present() ? geometry_info->paramPoly3().get().aV().get() : 0.0);
this->v_param.b = (geometry_info->paramPoly3().get().bV().present() ? geometry_info->paramPoly3().get().bV().get() : 0.0);
this->v_param.c = (geometry_info->paramPoly3().get().cV().present() ? geometry_info->paramPoly3().get().cV().get() : 0.0);
this->v_param.d = (geometry_info->paramPoly3().get().dV().present() ? geometry_info->paramPoly3().get().dV().get() : 0.0);
this->normalized = true;
if (geometry_info->paramPoly3().get().pRange().present() && geometry_info->paramPoly3().get().pRange().get() == pRange::arcLength)
this->normalized = false;
}
...@@ -137,4 +137,64 @@ void CAdcRoad::append_signs_spawn(std::string &filename) ...@@ -137,4 +137,64 @@ void CAdcRoad::append_signs_spawn(std::string &filename)
{ {
for (unsigned int i = 0; i < this->signals.size(); i++) for (unsigned int i = 0; i < this->signals.size(); i++)
this->signals[i]->append_sign_spawn(filename); this->signals[i]->append_sign_spawn(filename);
}
void CAdcRoad::load(std::auto_ptr<OpenDRIVE::road_type> &road_info)
{
std::stringstream ss(road_info->id().get());
ss >> this->id;
this->length = road_info->length().get();
this->name = road_info->name().get();
//////////////// Geometry atributes
for (planView::geometry_iterator geo_it(road_info->planView().geometry().begin()); geo_it != road_info->planView().geometry().end(); ++geo_it)
{
if (geo_it->line().present())
{
CAdcGeoLine *line = new CAdcGeoLine();
std::auto_ptr<planView::geometry_type> geo_info(new planView::geometry_type(*geo_it));
line->load(geo_info);
add_geometry(line);
}
else if (geo_it->spiral().present())
{
CAdcGeoSpiral *spi = new CAdcGeoSpiral();
std::auto_ptr<planView::geometry_type> geo_info(new planView::geometry_type(*geo_it));
spi->load(geo_info);
add_geometry(spi);
}
else if (geo_it->arc().present())
{
CAdcGeoArc *arc = new CAdcGeoArc();
std::auto_ptr<planView::geometry_type> geo_info(new planView::geometry_type(*geo_it));
arc->load(geo_info);
add_geometry(arc);
}
else if (geo_it->poly3().present())
{
CAdcGeoPoly3 *poly3 = new CAdcGeoPoly3();
std::auto_ptr<planView::geometry_type> geo_info(new planView::geometry_type(*geo_it));
poly3->load(geo_info);
add_geometry(poly3);
}
else if (geo_it->paramPoly3().present())
{
CAdcGeoParamPoly3 *parampoly3 = new CAdcGeoParamPoly3();
std::auto_ptr<planView::geometry_type> geo_info(new planView::geometry_type(*geo_it));
parampoly3->load(geo_info);
add_geometry(parampoly3);
}
}
////////// Signals atributes
if (road_info->signals().present())
{
for (signals::signal_iterator signal_it(road_info->signals().get().signal().begin()); signal_it != road_info->signals().get().signal().end(); ++signal_it)
{
CAdcSignals *sign = new CAdcSignals();
std::auto_ptr<signals::signal_type> signal_info(new signals::signal_type(*signal_it));
sign->load(signal_info);
add_signal(sign);
}
}
} }
\ No newline at end of file
...@@ -217,5 +217,21 @@ void CAdcSignals::append_sign_spawn(std::string &filename) ...@@ -217,5 +217,21 @@ void CAdcSignals::append_sign_spawn(std::string &filename)
throw CException(_HERE_,"CAdcCircuit::generate_spwan_launch_file -> The output file can't be open"); throw CException(_HERE_,"CAdcCircuit::generate_spwan_launch_file -> The output file can't be open");
out_file << oss.str() << std::endl; out_file << oss.str() << std::endl;
out_file.close(); out_file.close();
}
void CAdcSignals::load(std::auto_ptr<signals::signal_type> &signal_info)
{
std::stringstream ss(signal_info->id().get());
ss >> this->id;
this->s = (signal_info->s().present() ? signal_info->s().get() : 0.0);
this->t = (signal_info->t().present() ? signal_info->t().get() : 0.0);
this->heading = (signal_info->hOffset().present() ? signal_info->hOffset().get() : 0.0);
this->type = (signal_info->type().present() ? signal_info->type().get() : "");
this->sub_type = (signal_info->subtype().present() ? signal_info->subtype().get() : "");
this->value = (signal_info->value().present() ? signal_info->value().get() : 0.0);
this->text = (signal_info->text().present() ? signal_info->text().get() : "");
bool reverse = false;
if (signal_info->orientation().present() && signal_info->orientation().get() == orientation::cxx_1)
reverse = true;
this->heading += (reverse ? M_PI : 0.0);
} }
\ No newline at end of file
...@@ -13,7 +13,7 @@ int main(int argc, char *argv[]) ...@@ -13,7 +13,7 @@ int main(int argc, char *argv[])
try try
{ {
open_drive_format.load(xml_file, false); open_drive_format.load(xml_file, false);
open_drive_format.calculate_signals_pose(false); open_drive_format.calculate_signals_pose(true);
std::vector<CAdcSignals*> signals; std::vector<CAdcSignals*> signals;
// open_drive_format.get_signals(signals); // open_drive_format.get_signals(signals);
......
...@@ -31,98 +31,7 @@ void COpenDriveFormat::load(std::string &filename, bool debug) ...@@ -31,98 +31,7 @@ void COpenDriveFormat::load(std::string &filename, bool debug)
try{ try{
std::auto_ptr<OpenDRIVE> open_drive(OpenDRIVE_(filename.c_str(), xml_schema::flags::dont_validate)); std::auto_ptr<OpenDRIVE> open_drive(OpenDRIVE_(filename.c_str(), xml_schema::flags::dont_validate));
////////////////// Access road atributes this->adc_circuit.load(open_drive);
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 = new CAdcRoad(road_id, road_length, name);
///////////////////// link atributes
//////////////// Geometry atributes
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 = 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 = 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 = 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 = new CAdcGeoArc(min_s, max_s, x, y, heading, curvature);
road->add_geometry(arc);
}
else if (geo_it->poly3().present())
{
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 = new CAdcGeoPoly3(min_s, max_s, x, y, heading, param);
road->add_geometry(poly3);
}
else if (geo_it->paramPoly3().present())
{
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 = new CAdcGeoParamPoly3(min_s, max_s, x, y, heading, u_param, v_param, normalized);
road->add_geometry(parampoly3);
}
}
////////// 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::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 = new CAdcSignals(id, s, t, heading, type, sub_type, value, text, reverse);
road->add_signal(sign);
}
}
this->adc_circuit.add_road(road);
}
std::cout << "Loading .xodr info done." << std::endl; std::cout << "Loading .xodr info done." << std::endl;
if (debug) if (debug)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment