diff --git a/include/adc_circuit.h b/include/adc_circuit.h new file mode 100644 index 0000000000000000000000000000000000000000..965a5b081b976c6d3969c8a2a8aed273941056f6 --- /dev/null +++ b/include/adc_circuit.h @@ -0,0 +1,19 @@ +#ifndef _ADC_CIRCUIT_H +#define _ADC_CIRCUIT_H + +#include "adc_road.h" +#include <vector> + +class CAdcCircuit +{ + private: + std::vector<CAdcRoad> roads; + public: + CAdcCircuit(); + ~CAdcCircuit(); + + void clear_roads(void); + void add_road(CAdcRoad &road); +}; + +#endif \ No newline at end of file diff --git a/include/adc_geometries.h b/include/adc_geometries.h new file mode 100644 index 0000000000000000000000000000000000000000..4618bd365154788b9d7c611432ec78b8b8a6089b --- /dev/null +++ b/include/adc_geometries.h @@ -0,0 +1,112 @@ +#ifndef _ADC_GEOMETRIES_H +#define _ADC_GEOMETRIES_H + + +class CAdcGeometry +{ + private: + protected: + double min_s; + double max_s; + double x; + double y; + double heading; + + 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(); + + bool get_world_pose(double s, double t, double heading, double &x, double &y, double &yaw); + bool on_range(double s); +}; + +class CAdcGeoLine: public CAdcGeometry +{ + private: + protected: + 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(); +}; + +class CAdcGeoSpiral: public CAdcGeometry +{ + private: + double curv_start; + double curv_end; + + protected: + bool get_local_pose(double s, double t, double heading, double &u, double &v, double &yaw); + + public: + CAdcGeoSpiral(); + CAdcGeoSpiral(double min_s, double max_s, double x, double y, double heading); + 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); +}; + +class CAdcGeoArc: public CAdcGeometry +{ + private: + double curvature; + + protected: + bool get_local_pose(double s, double t, double heading, double &u, double &v, double &yaw); + + public: + CAdcGeoArc(); + CAdcGeoArc(double min_s, double max_s, double x, double y, double heading); + CAdcGeoArc(double min_s, double max_s, double x, double y, double heading, double curvature); + ~CAdcGeoArc(); + void set_params(double curvature); +}; + +typedef struct +{ + double a; + double b; + double c; + double d; +}Adc_poly3_param; + +class CAdcGeoPoly3: public CAdcGeometry +{ + private: + Adc_poly3_param param; + + protected: + bool get_local_pose(double s, double t, double heading, double &u, double &v, double &yaw); + + public: + CAdcGeoPoly3(); + CAdcGeoPoly3(double min_s, double max_s, double x, double y, double heading); + 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); +}; + +class CAdcGeoParamPoly3: public CAdcGeometry +{ + private: + Adc_poly3_param u_param; + Adc_poly3_param v_param; + bool normalized; + + protected: + bool get_local_pose(double s, double t, double heading, double &u, double &v, double &yaw); + + public: + CAdcGeoParamPoly3(); + CAdcGeoParamPoly3(double min_s, double max_s, double x, double y, double heading); + 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); +}; +#endif \ No newline at end of file diff --git a/include/adc_road.h b/include/adc_road.h new file mode 100644 index 0000000000000000000000000000000000000000..aa0cbdc18d6d2cd944ed986c64ba1f1d3b41cc4c --- /dev/null +++ b/include/adc_road.h @@ -0,0 +1,30 @@ +#ifndef _ADC_ROAD_H +#define _ADC_ROAD_H + +#include <vector> +#include "adc_geometries.h" +#include "adc_signals.h" + +class CAdcRoad +{ + private: + int id; + double length; + std::vector<CAdcGeometry*> geometries; + std::vector<CAdcSignals> signals; + public: + CAdcRoad(); + CAdcRoad(int id, double length); + ~CAdcRoad(); + + void clear_geometries(void); + void add_geometry(CAdcGeometry* geometry); + void clear_signals(void); + void add_signal(CAdcSignals sign); + void set_id(int id); + int get_id(void); + void set_length(double length); + double get_length(void); +}; + +#endif diff --git a/include/adc_signals.h b/include/adc_signals.h new file mode 100644 index 0000000000000000000000000000000000000000..4da369d8fd043753cba175106c9a1f5a8dcb0cc2 --- /dev/null +++ b/include/adc_signals.h @@ -0,0 +1,43 @@ +#ifndef _ADC_SIGNALS_H +#define _ADC_SIGNALS_H +#include <string> + +// UNMARKEDINTERSECTION 102 +// STOPANDGIVEWAY 206 +// PARKINGAREA ??? +// HAVEWAY ??? +// AHEADONLY 209 +// AHEADONLYSUB 30 +// GIVEWAY 205 +// PEDESTRIANCROSSING ??? +// ROUNDABOUT ??? +// NOOVERTAKING 276 +// NOENTRYVEHICULARTRAFFIC 267 +// TESTCOURSEA9 ??? +// ONEWAYSTREET 220 +// ROADWORKS 123 +// KMH50 274 +// KMH100 274 + +class CAdcSignals +{ + private: + int id; + double s; + double t; + double heading; + std::string type; + std::string sub_type; + int value; + std::string text; + public: + CAdcSignals(); + CAdcSignals(int id, double s, double t, double heading, std::string type, std::string sub_type, int value, std::string text, bool reverse = false); + ~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); +}; + +#endif diff --git a/include/open_drive_format.h b/include/open_drive_format.h index 05cbb245d32d1c4394565cd42db26083007161e7..0a5a472f7391469d50bc119d5287843024113b7f 100644 --- a/include/open_drive_format.h +++ b/include/open_drive_format.h @@ -3,28 +3,7 @@ #include <string> #include <vector> - -typedef struct -{ - int id; ///< Projection polynomial of radially symmetric model. -}Road_struct; - -// UNMARKEDINTERSECTION 102 -// STOPANDGIVEWAY 206 -// PARKINGAREA ??? -// HAVEWAY ??? -// AHEADONLY 209 -// AHEADONLYSUB 30 -// GIVEWAY 205 -// PEDESTRIANCROSSING ??? -// ROUNDABOUT ??? -// NOOVERTAKING 276 -// NOENTRYVEHICULARTRAFFIC 267 -// TESTCOURSEA9 ??? -// ONEWAYSTREET 220 -// ROADWORKS 123 -// KMH50 274 -// KMH100 274 +#include "adc_circuit.h" class COpenDriveFormat { @@ -33,7 +12,7 @@ class COpenDriveFormat void load(std::string &filename); ~COpenDriveFormat(); private: - std::vector<Road_struct> road_vect; + CAdcCircuit adc_circuit; }; #endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 1fdb9a7b3a8fcff6e05999181779695d52b39b3d..f3550e5b03dcf3adabbc792bc4e8af4f8afe3a23 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,7 +1,7 @@ # driver source files -SET(sources open_drive_format.cpp) +SET(sources open_drive_format.cpp adc_circuit.cpp adc_road.cpp adc_geometries.cpp adc_signals.cpp) # application header files -SET(headers ../include/open_drive_format.h) +SET(headers ../include/open_drive_format.h ../include/adc_circuit.h ../include/adc_road.h ../include/adc_geometries.h ../include/adc_signals.h) # locate the necessary dependencies FIND_PACKAGE(iriutils REQUIRED) diff --git a/src/adc_circuit.cpp b/src/adc_circuit.cpp new file mode 100644 index 0000000000000000000000000000000000000000..cebe36434c62ed013d4152fafb507ae25253728a --- /dev/null +++ b/src/adc_circuit.cpp @@ -0,0 +1,22 @@ +#include "adc_circuit.h" + + +CAdcCircuit::CAdcCircuit() +{ + +} + +CAdcCircuit::~CAdcCircuit() +{ + clear_roads(); +} + +void CAdcCircuit::clear_roads(void) +{ + std::vector<CAdcRoad>().swap(this->roads); +} + +void CAdcCircuit::add_road(CAdcRoad &road) +{ + this->roads.push_back(road); +} diff --git a/src/adc_geometries.cpp b/src/adc_geometries.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0fed433d786c8ddd7a9c154e40151fa6a1791550 --- /dev/null +++ b/src/adc_geometries.cpp @@ -0,0 +1,278 @@ +#include "adc_geometries.h" +#include <cmath> +#include <iostream> + +///////////////////// CAdcGeometry ///////////////////// +CAdcGeometry::CAdcGeometry() +{ + this->min_s = 0.0; + this->max_s = 0.0; + this->x = 0.0; + this->y = 0.0; + this->heading = 0.0; +} + +CAdcGeometry::~CAdcGeometry() +{ + this->min_s = 0.0; + this->max_s = 0.0; + this->x = 0.0; + this->y = 0.0; + this->heading = 0.0; +} +CAdcGeometry::CAdcGeometry(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; +} + +bool CAdcGeometry::get_world_pose(double s, double t, double heading, double &x, double &y, double &yaw) +{ + double local_u, local_v, local_yaw; + if (get_local_pose(s, t, heading, local_u, local_v, local_yaw)) + { + yaw = this->heading + local_yaw; + x = local_u*std::cos(this->heading) - local_v*std::sin(this->heading) + this->x; + y = local_u*std::sin(this->heading) + local_v*std::cos(this->heading) + this->y; + return true; + } + return false; +} + +bool CAdcGeometry::on_range(double s) +{ + if ((s < this->max_s) && (s > this->min_s)) + return true; + return false; +} + +///////////////////// CAdcGeoLine ///////////////////// +CAdcGeoLine::CAdcGeoLine() +{ + +} + +CAdcGeoLine::~CAdcGeoLine() +{ + +} + +CAdcGeoLine::CAdcGeoLine(double min_s, double max_s, double x, double y, double heading): +CAdcGeometry(min_s, max_s, x, y, heading) +{ + +} + +bool CAdcGeoLine::get_local_pose(double s, double t, double heading, double &u, double &v, double &yaw) +{ + u = s - this->min_s; + v = t; + yaw = heading; + return true; +} + +///////////////////// CAdcGeoSpiral ///////////////////// +CAdcGeoSpiral::CAdcGeoSpiral() +{ + this->curv_start = 0.0; + this->curv_end = 0.0; +} + +CAdcGeoSpiral::~CAdcGeoSpiral() +{ + this->curv_start = 0.0; + this->curv_end = 0.0; +} + +CAdcGeoSpiral::CAdcGeoSpiral(double min_s, double max_s, double x, double y, double heading): +CAdcGeometry(min_s, max_s, x, y, heading) +{ + +} + +CAdcGeoSpiral::CAdcGeoSpiral(double min_s, double max_s, double x, double y, double heading, double curv_start, double curv_end): +CAdcGeometry(min_s, max_s, x, y, heading) +{ + this->curv_start = curv_start; + this->curv_end = curv_end; +} + +void CAdcGeoSpiral::set_params(double curv_start, double curv_end) +{ + this->curv_start = curv_start; + this->curv_end = curv_end; +} + +bool CAdcGeoSpiral::get_local_pose(double s, double t, double heading, double &u, double &v, double &yaw) +{ + u = 0.0; + v = 0.0; + yaw = 0.0; + std::cout << "not supported" << std::endl; + return false; +} + +///////////////////// CAdcGeoArc ///////////////////// +CAdcGeoArc::CAdcGeoArc() +{ + this->curvature = 0.0; +} + +CAdcGeoArc::~CAdcGeoArc() +{ + this->curvature = 0.0; +} + +CAdcGeoArc::CAdcGeoArc(double min_s, double max_s, double x, double y, double heading): +CAdcGeometry(min_s, max_s, x, y, heading) +{ + +} + +CAdcGeoArc::CAdcGeoArc(double min_s, double max_s, double x, double y, double heading, double curvature): +CAdcGeometry(min_s, max_s, x, y, heading) +{ + this->curvature = curvature; +} + +void CAdcGeoArc::set_params(double curvature) +{ + this->curvature = curvature; +} + +bool CAdcGeoArc::get_local_pose(double s, double t, double heading, double &u, double &v, double &yaw) +{ + s -= this->min_s; + double alpha = std::fabs(s*this->curvature); + bool pos_arc = (this->curvature < 0.0 ? false : true); + u = std::sin(alpha)/this->curvature - t*std::sin(alpha)*(pos_arc ? 1 : -1); + v = (1 - std::cos(alpha))*(pos_arc ? 1 : -1)/this->curvature + t*std::cos(alpha); + yaw = heading + alpha*(pos_arc ? 1 : -1); + return true; +} + +///////////////////// CAdcGeoPoly3 ///////////////////// +CAdcGeoPoly3::CAdcGeoPoly3() +{ + this->param.a = 0.0; + this->param.b = 0.0; + this->param.c = 0.0; + this->param.d = 0.0; +} + +CAdcGeoPoly3::~CAdcGeoPoly3() +{ + this->param.a = 0.0; + this->param.b = 0.0; + this->param.c = 0.0; + this->param.d = 0.0; +} + +CAdcGeoPoly3::CAdcGeoPoly3(double min_s, double max_s, double x, double y, double heading): +CAdcGeometry(min_s, max_s, x, y, heading) +{ + +} + +CAdcGeoPoly3::CAdcGeoPoly3(double min_s, double max_s, double x, double y, double heading, Adc_poly3_param param): +CAdcGeometry(min_s, max_s, x, y, heading) +{ + this->param.a = param.a; + this->param.b = param.b; + this->param.c = param.c; + this->param.d = param.d; +} + +void CAdcGeoPoly3::set_params(Adc_poly3_param param) +{ + this->param.a = param.a; + this->param.b = param.b; + this->param.c = param.c; + this->param.d = param.d; +} + +bool CAdcGeoPoly3::get_local_pose(double s, double t, double heading, double &u, double &v, double &yaw) +{ + u = 0.0; + v = 0.0; + yaw = 0.0; + std::cout << "not supported" << std::endl; + return false; +} + +///////////////////// CAdcGeoParamPoly3 ///////////////////// +CAdcGeoParamPoly3::CAdcGeoParamPoly3() +{ + this->u_param.a = 0.0; + this->u_param.b = 0.0; + this->u_param.c = 0.0; + this->u_param.d = 0.0; + this->v_param.a = 0.0; + this->v_param.b = 0.0; + this->v_param.c = 0.0; + this->v_param.d = 0.0; + this->normalized = true; +} + +CAdcGeoParamPoly3::~CAdcGeoParamPoly3() +{ + this->u_param.a = 0.0; + this->u_param.b = 0.0; + this->u_param.c = 0.0; + this->u_param.d = 0.0; + this->v_param.a = 0.0; + this->v_param.b = 0.0; + this->v_param.c = 0.0; + this->v_param.d = 0.0; + this->normalized = true; +} + +CAdcGeoParamPoly3::CAdcGeoParamPoly3(double min_s, double max_s, double x, double y, double heading): +CAdcGeometry(min_s, max_s, x, y, heading) +{ + +} + +CAdcGeoParamPoly3::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): +CAdcGeometry(min_s, max_s, x, y, heading) +{ + this->u_param.a = u_param.a; + this->u_param.b = u_param.b; + this->u_param.c = u_param.c; + this->u_param.d = u_param.d; + this->v_param.a = v_param.a; + this->v_param.b = v_param.b; + this->v_param.c = v_param.c; + this->v_param.d = v_param.d; + this->normalized = normalized; +} + +void CAdcGeoParamPoly3::set_params(Adc_poly3_param u_param, Adc_poly3_param v_param, bool normalized) +{ + this->u_param.a = u_param.a; + this->u_param.b = u_param.b; + this->u_param.c = u_param.c; + this->u_param.d = u_param.d; + this->v_param.a = v_param.a; + this->v_param.b = v_param.b; + this->v_param.c = v_param.c; + this->v_param.d = v_param.d; + this->normalized = normalized; +} + +bool CAdcGeoParamPoly3::get_local_pose(double s, double t, double heading, double &u, double &v, double &yaw) +{ + double p = (this->normalized ? (s - this->min_s)/(this->max_s - this->min_s): (s - this->min_s)); + double p2 = p*p; + double p3 = p2*p; + double du = this->u_param.b + 2*this->u_param.c*p + 3*this->u_param.d*p2; + double dv = this->v_param.b + 2*this->v_param.c*p + 3*this->v_param.d*p2; + double alpha = std::atan2(dv, du); + u = this->u_param.a + this->u_param.b*p + this->u_param.c*p2 + this->u_param.d*p3 - t*std::sin(alpha); + 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 diff --git a/src/adc_road.cpp b/src/adc_road.cpp new file mode 100644 index 0000000000000000000000000000000000000000..aef8e337704e646aba6b87c36e8c2ea14394527c --- /dev/null +++ b/src/adc_road.cpp @@ -0,0 +1,61 @@ +#include "adc_road.h" + +CAdcRoad::CAdcRoad() +{ + this->id = 0; + this->length = 0.0; +} + +CAdcRoad::CAdcRoad(int id, double length) +{ + this->id = id; + this->length = length; +} + +CAdcRoad::~CAdcRoad() +{ + this->id = 0; + this->length = 0.0; + clear_geometries(); + clear_signals(); +} + +void CAdcRoad::clear_geometries(void) +{ + std::vector<CAdcGeometry*>().swap(this->geometries); +} + +void CAdcRoad::add_geometry(CAdcGeometry* geometry) +{ + this->geometries.push_back(geometry); +} + +void CAdcRoad::clear_signals(void) +{ + std::vector<CAdcSignals>().swap(this->signals); +} + +void CAdcRoad::add_signal(CAdcSignals sign) +{ + this->signals.push_back(sign); +} + +void CAdcRoad::set_id(int id) +{ + this->id = id; +} + +int CAdcRoad::get_id(void) +{ + return this->id; +} + +void CAdcRoad::set_length(double length) +{ + this->length = length; +} + +double CAdcRoad::get_length(void) +{ + return this->length; +} diff --git a/src/adc_signals.cpp b/src/adc_signals.cpp new file mode 100644 index 0000000000000000000000000000000000000000..40a0c46db3fb73dfe7cb645c0e7d7648534614d6 --- /dev/null +++ b/src/adc_signals.cpp @@ -0,0 +1,54 @@ +#include "adc_signals.h" +#include <cmath> + +CAdcSignals::CAdcSignals() +{ + this->id = 0; + this->s = 0; + this->t = 0; + this->heading = 0; + this->type = ""; + this->sub_type = ""; + this->value = 0; + this->text = ""; +} + +CAdcSignals::~CAdcSignals() +{ + this->id = 0; + this->s = 0; + this->t = 0; + this->heading = 0; + this->type = ""; + this->sub_type = ""; + this->value = 0; + this->text = ""; +} + +CAdcSignals::CAdcSignals(int id, double s, double t, double heading, std::string type, std::string sub_type, int value, std::string text, bool reverse) +{ + this->id = id; + this->s = s; + this->t = t; + this->heading = heading + (reverse ? 0.0 : M_PI); + this->type = type; + this->sub_type = sub_type; + this->value = value; + this->text = text; +} + +void CAdcSignals::get_pose(double &s, double &t, double &heading) +{ + id = this->id; + s = this->s; + t = this->t; + heading = this->heading; +} + +void CAdcSignals::get_sign_info(std::string &type, std::string &sub_type, int &value, std::string &text) +{ + type = this->type; + sub_type = this->sub_type; + value = this->value; + text = this->text; +} diff --git a/src/open_drive_format.cpp b/src/open_drive_format.cpp index 44c78dfb181fbe605312c802465212783c818c2a..317090c4b7b860c4e5e524f03c453549f5c90649 100644 --- a/src/open_drive_format.cpp +++ b/src/open_drive_format.cpp @@ -1,6 +1,5 @@ #include "open_drive_format.h" #include "exceptions.h" -#include <iostream> #include <sys/types.h> #include <sys/stat.h> @@ -8,7 +7,12 @@ #include <iostream> #include <sstream> +#include "adc_road.h" +#include "adc_geometries.h" +#include "adc_signals.h" + #ifdef _HAVE_XSD + #include "xml/OpenDRIVE_1.4H.hxx" #endif @@ -29,12 +33,11 @@ void COpenDriveFormat::load(std::string &filename) ////////////////// Access road atributes 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::stringstream ss(road_it->id().get()); + 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(); @@ -66,18 +69,41 @@ void COpenDriveFormat::load(std::string &filename) 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()) { @@ -85,6 +111,16 @@ void COpenDriveFormat::load(std::string &filename) 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()) { @@ -97,6 +133,24 @@ void COpenDriveFormat::load(std::string &filename) 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 = ¶mpoly3; + road.add_geometry(geo); } } @@ -117,11 +171,26 @@ void COpenDriveFormat::load(std::string &filename) 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; } - // road->header() std::cout << "Done." << std::endl; }catch (const xml_schema::exception& e){ std::ostringstream os;