Skip to content
Snippets Groups Projects
Commit 5c59c6f9 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added a reference to the road segment hat the signals belongs to.

Added the scale factor attribute. Used to scale the position of the signal
Added a function to get the world pose of the signal.
parent 4dd90b07
No related branches found
No related tags found
No related merge requests found
......@@ -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();
};
......
#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;
}
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