diff --git a/include/opendrive_signal.h b/include/opendrive_signal.h index f49a6f1923ddbee17087d857ac5c679ca92260a9..dea83187cdfdd10678cfb3ade2d830914fec6e4e 100644 --- a/include/opendrive_signal.h +++ b/include/opendrive_signal.h @@ -2,32 +2,39 @@ #define _OPENDRIVE_SIGNAL_H #include "opendrive_common.h" +#include "opendrive_road_segment.h" #ifdef _HAVE_XSD #include "../src/xml/OpenDRIVE_1.4H.hxx" #endif +class COpendriveRoadSegment; + class COpendriveSignal { friend class COpendriveRoadSegment; private: + COpendriveRoadSegment *segment; int id; ///< Signal's id. TOpendriveTrackPoint pose; std::string type; ///< Signal's OpenDrive type. std::string sub_type; ///< Signal's OpenDrive subtype. int value; ///< Signal's value. std::string text; ///< Signal's text. + double scale_factor; protected: - void load(signals::signal_type &signal_info); + void load(signals::signal_type &signal_info,COpendriveRoadSegment *segment); + void update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> &segment_refs); + void set_scale_factor(double scale); public: COpendriveSignal(); COpendriveSignal(const COpendriveSignal &object); - COpendriveSignal(int id, double s, double t, double heading, std::string type, std::string sub_type, int value, std::string text, bool reverse = false); + double get_scale_factor(void); int get_id(void) const; - TOpendriveTrackPoint get_pose(void) const; + TOpendriveTrackPoint get_track_pose(void) const; + TOpendriveWorldPoint get_world_pose(void) const; void get_type(std::string &type, std::string &sub_type) const; int get_value(void) const; std::string get_text(void) const; - void operator=(const COpendriveSignal &object); friend std::ostream& operator<<(std::ostream& out, COpendriveSignal &signal); ~COpendriveSignal(); }; diff --git a/src/opendrive_signal.cpp b/src/opendrive_signal.cpp index 20831668c5f173e962401d6a354a74f9e9fed88f..76f401590d7b3b3cb7cb9f0d569673b12d86d33b 100644 --- a/src/opendrive_signal.cpp +++ b/src/opendrive_signal.cpp @@ -1,8 +1,10 @@ #include "opendrive_signal.h" +#include "exceptions.h" #include <cmath> COpendriveSignal::COpendriveSignal() { + this->segment=NULL; this->id=-1; this->pose.s=0.0; this->pose.t=0.0; @@ -11,10 +13,12 @@ COpendriveSignal::COpendriveSignal() this->sub_type=""; this->value=0; this->text=""; + this->scale_factor=DEFAULT_SCALE_FACTOR; } COpendriveSignal::COpendriveSignal(const COpendriveSignal &object) { + this->segment=object.segment; this->id=object.id; this->pose.s=object.pose.s; this->pose.t=object.pose.t; @@ -23,21 +27,10 @@ COpendriveSignal::COpendriveSignal(const COpendriveSignal &object) this->sub_type=object.sub_type; this->value=object.value; this->text=object.text; + this->scale_factor=object.scale_factor; } -COpendriveSignal::COpendriveSignal(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->pose.s=s; - this->pose.t=t; - this->pose.heading=normalize_angle(heading + (reverse ? M_PI : 0.0)); - this->type=type; - this->sub_type=sub_type; - this->value=value; - this->text=text; -} - -void COpendriveSignal::load(signals::signal_type &signal_info) +void COpendriveSignal::load(signals::signal_type &signal_info,COpendriveRoadSegment *segment) { std::stringstream ss(signal_info.id().get()); ss >> this->id; @@ -52,6 +45,17 @@ void COpendriveSignal::load(signals::signal_type &signal_info) if (signal_info.orientation().present() && signal_info.orientation().get() == orientation::cxx_1) reverse = true; this->pose.heading = normalize_angle(this->pose.heading + (reverse ? M_PI : 0.0)); + this->segment=segment; +} + +void COpendriveSignal::update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> &segment_refs) +{ + this->segment=segment_refs[this->segment]; +} + +void COpendriveSignal::set_scale_factor(double scale) +{ + this->scale_factor=scale; } int COpendriveSignal::get_id(void) const @@ -59,9 +63,33 @@ int COpendriveSignal::get_id(void) const return this->id; } -TOpendriveTrackPoint COpendriveSignal::get_pose(void) const +double COpendriveSignal::get_scale_factor(void) +{ + return this->scale_factor; +} + +TOpendriveTrackPoint COpendriveSignal::get_track_pose(void) const +{ + TOpendriveTrackPoint track; + + track.s=this->pose.s/this->scale_factor; + track.t=this->pose.t/this->scale_factor; + track.heading=this->pose.heading; + + return track; +} + +TOpendriveWorldPoint COpendriveSignal::get_world_pose(void) const { - return this->pose; + TOpendriveWorldPoint world; + + if(this->segment!=NULL) + { + world=segment->transform_to_world_point(this->get_track_pose()); + return world; + } + else + throw CException(_HERE_,"Invalid parent segment reference"); } void COpendriveSignal::get_type(std::string &type, std::string &sub_type) const @@ -80,31 +108,23 @@ std::string COpendriveSignal::get_text(void) const return this->text; } -void COpendriveSignal::operator=(const COpendriveSignal &object) -{ - this->id=object.id; - this->pose.s=object.pose.s; - this->pose.t=object.pose.t; - this->pose.heading=object.pose.heading; - this->type=object.type; - this->sub_type=object.sub_type; - this->value=object.value; - this->text=object.text; -} - std::ostream& operator<<(std::ostream& out, COpendriveSignal &signal) { + TOpendriveTrackPoint track; + out << " Signal id = " << signal.id << std::endl; out << " type = " << signal.type << "," << signal.sub_type << std::endl; out << " value = " << signal.value << std::endl; out << " text = " << signal.text << std::endl; - out << " pose: s = " << signal.pose.s << ", t = " << signal.pose.t << ", heading = " << signal.pose.heading << std::endl; + track=signal.get_track_pose(); + out << " pose: s = " << track.s << ", t = " << track.t << ", heading = " << track.heading << std::endl; return out; } COpendriveSignal::~COpendriveSignal() { + this->segment=NULL; this->id=-1; this->pose.s=0.0; this->pose.t=0.0; @@ -113,5 +133,6 @@ COpendriveSignal::~COpendriveSignal() this->sub_type=""; this->value=0; this->text=""; + this->scale_factor=DEFAULT_SCALE_FACTOR; }