diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h index 49c920d7707284fbfbceebb5c6d561a7cac4330e..fe55f253ccf5dc22faf0e0a01ad975868b157edc 100644 --- a/include/opendrive_road_node.h +++ b/include/opendrive_road_node.h @@ -6,6 +6,7 @@ #include "opendrive_lane.h" class COpendriveLane; +class COpendriveRoadSegment; class COpendriveRoadNode { @@ -18,20 +19,23 @@ class COpendriveRoadNode TOpendriveWorldPoint start_point; std::vector<COpendriveLane *> lanes; std::vector<COpendriveGeometry *> geometries; - std::vector<unsigned int> indexes; + COpendriveRoadSegment *parent_segment; + unsigned int index; protected: void load(const planView::geometry_type &geom_info,COpendriveLane *lane); void add_link(COpendriveRoadNode *node,opendrive_mark_t mark); void set_resolution(double res); void set_scale_factor(double scale); - void set_index(unsigned int lane_index,unsigned int index); + void set_index(unsigned int index); + void set_parent_segment(COpendriveRoadSegment *segment); void add_lane(const planView::geometry_type &geom_info,COpendriveLane *lane); public: COpendriveRoadNode(); COpendriveRoadNode(const COpendriveRoadNode &object); double get_resolution(void) const; double get_scale_factor(void) const; - unsigned int get_index(unsigned int lane_index) const; + unsigned int get_index(void) const; + const COpendriveRoadSegment& get_parent_segment(void) const; TOpendriveWorldPoint get_start_pose(void) const; unsigned int get_num_links(void) const; const COpendriveLink &get_link(unsigned int index) const; diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp index 2c2b6f0b39b7cea100464e9929fd7be9aad3b294..848ea1abec92860d90f61066a719ebd5e00377b7 100644 --- a/src/opendrive_road_node.cpp +++ b/src/opendrive_road_node.cpp @@ -11,7 +11,8 @@ COpendriveRoadNode::COpendriveRoadNode() this->start_point.heading=0.0; this->lanes.clear(); this->geometries.clear(); - this->indexes.clear(); + this->index=-1; + this->parent_segment=NULL; } COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object) @@ -35,7 +36,8 @@ COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object) new_link=new COpendriveLink(*object.links[i]); this->links.push_back(new_link); } - this->indexes=object.indexes; + this->index=object.index; + this->parent_segment=object.parent_segment; } void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark) @@ -72,12 +74,14 @@ void COpendriveRoadNode::set_scale_factor(double scale) this->links[i]->set_scale_factor(scale); } -void COpendriveRoadNode::set_index(unsigned int lane_index,unsigned int index) +void COpendriveRoadNode::set_index(unsigned int index) { - if(lane_index>=0 && lane_index<this->lanes.size()) - this->indexes[lane_index]=index; - else - throw CException(_HERE_,"Invalid lane index"); + this->index=index; +} + +void COpendriveRoadNode::set_parent_segment(COpendriveRoadSegment *segment) +{ + this->parent_segment=segment; } void COpendriveRoadNode::add_lane(const planView::geometry_type &geom_info,COpendriveLane *lane) @@ -99,7 +103,6 @@ void COpendriveRoadNode::add_lane(const planView::geometry_type &geom_info,COpen geometry->load(geom_info); this->geometries.push_back(geometry); this->lanes.push_back(lane); - this->indexes.resize(this->lanes.size()); } void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendriveLane *lane) @@ -158,7 +161,6 @@ void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendriv this->links.clear(); this->lanes.clear(); this->lanes.push_back(lane); - this->indexes.resize(1); } double COpendriveRoadNode::get_resolution(void) const @@ -171,12 +173,14 @@ double COpendriveRoadNode::get_scale_factor(void) const return this->scale_factor; } -unsigned int COpendriveRoadNode::get_index(unsigned int lane_index) const +unsigned int COpendriveRoadNode::get_index(void) const { - if(lane_index>=0 && lane_index<this->indexes.size()) - return this->indexes[lane_index]; - else - throw CException(_HERE_,"Invalid lane index"); + return this->index; +} + +const COpendriveRoadSegment &COpendriveRoadNode::get_parent_segment(void) const +{ + return *this->parent_segment; } TOpendriveWorldPoint COpendriveRoadNode::get_start_pose(void) const @@ -254,12 +258,12 @@ void COpendriveRoadNode::operator=(const COpendriveRoadNode &object) new_link=new COpendriveLink(*object.links[i]); this->links.push_back(new_link); } - this->indexes=object.indexes; + this->index=object.index; } std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node) { -// out << " parent lane: " << node.lane->get_id() << std::endl; + out << " Node: " << node.get_index() << std::endl; out << " start point: x: " << node.start_point.x << " y: " << node.start_point.y << " heading: " << node.start_point.heading << std::endl; out << " Number of parent lanes: " << node.lanes.size() << std::endl; for(unsigned int i=0;i<node.lanes.size();i++) @@ -293,8 +297,11 @@ COpendriveRoadNode::~COpendriveRoadNode() this->geometries.clear(); for(unsigned int i=0;i<this->links.size();i++) { - delete this->links[i]; - this->links[i]=NULL; + if(this->links[i]->prev==this) + { + delete this->links[i]; + this->links[i]=NULL; + } } this->links.clear(); }