diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index 6699c2bfdd1e0e255a396ab0352abc8ebad2ab01..1cc2e20a2e15b46e22b788ab4cb19f0f83f001cb 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -51,38 +51,84 @@ COpendriveRoadSegment &COpendriveRoad::operator[](std::string &key)
void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
{
+ std::string predecessor_id,successor_id;
+
for(OpenDRIVE::road_iterator road_it(open_drive.road().begin());road_it!=open_drive.road().end();++road_it)
{
- COpendriveRoadSegment &segment=(*this)[road_it->id().get()];
- if(std::stoi(road_it->junction().get())==-1)// process only non junction road segments
+ // get current segment
+ COpendriveRoadSegment &road=(*this)[road_it->id().get()];
+ // get predecessor and successor
+ if(road_it->lane_link().present())
{
- // get predecessor
- if(road_it->lane_link().present())
+ if(road_it->lane_link().get().predecessor().present())// predecessor present
{
- if(road_it->lane_link().get().predecessor().present())// predecessor present
- {
- if(road_it->lane_link().get().predecessor().get().elementType().get()=="road")
- {
- COpendriveRoadSegment &link_segment=(*this)[road_it->lane_link().get().predecessor().get().elementId().get()];
- link_segment.link_segment(segment);
- }
- // else ignore juntions
- }
- if(road_it->lane_link().get().successor().present())// predecessor present
+ if(road_it->lane_link().get().predecessor().get().elementType().get()=="road")// previous segment is a road
+ predecessor_id=road_it->lane_link().get().predecessor().get().elementId().get();
+ }
+ if(road_it->lane_link().get().successor().present())// successor present
+ {
+ if(road_it->lane_link().get().successor().get().elementType().get()=="road")
+ successor_id=road_it->lane_link().get().successor().get().elementId().get();
+ }
+ }
+ if(std::stoi(road_it->junction().get())==-1)// non junction road segments
+ {
+ if(!predecessor_id.empty())
+ {
+ COpendriveRoadSegment &prev_road=(*this)[predecessor_id];
+ prev_road.link_segment(road);
+ predecessor_id.clear();
+ }
+ if(!successor_id.empty())
+ {
+ COpendriveRoadSegment &next_road=(*this)[successor_id];
+ road.link_segment(next_road);
+ successor_id.clear();
+ }
+ }
+ else// junction segment
+ {
+ for(OpenDRIVE::junction_iterator junction_it(open_drive.junction().begin());junction_it!=open_drive.junction().end();++junction_it)
+ {
+ for(junction::connection_iterator connection_it(junction_it->connection().begin()); connection_it!=junction_it->connection().end();++connection_it)
{
- if(road_it->lane_link().get().successor().get().elementType().get()=="road")
+ std::string incoming_road_id;
+ std::string connecting_road_id;
+ if(connection_it->incomingRoad().present())
+ incoming_road_id=connection_it->incomingRoad().get();
+ else
+ throw CException(_HERE_,"Connectivity information missing");
+ if(connection_it->connectingRoad().present())
+ connecting_road_id=connection_it->connectingRoad().get();
+ else
+ throw CException(_HERE_,"Connectivity information missing");
+ if(predecessor_id.compare(incoming_road_id)==0 && successor_id.compare(connecting_road_id)==0)// this is the connection
{
- COpendriveRoadSegment &link_segment=(*this)[road_it->lane_link().get().successor().get().elementId().get()];
- segment.link_segment(link_segment);
+ COpendriveRoadSegment &prev_road=(*this)[predecessor_id];
+ COpendriveRoadSegment &next_road=(*this)[successor_id];
+ for(connection::laneLink_iterator lane_link_it(connection_it->laneLink().begin()); lane_link_it!=connection_it->laneLink().end();++lane_link_it)
+ {
+ int from_lane_id;
+ int to_lane_id;
+ if(lane_link_it->from().present())
+ from_lane_id=lane_link_it->from().get();
+ else
+ throw CException(_HERE_,"Connectivity information missing");
+ if(lane_link_it->to().present())
+ to_lane_id=lane_link_it->to().get();
+ else
+ throw CException(_HERE_,"Connectivity information missing");
+ prev_road.link_segment(road,from_lane_id,-1);
+ road.link_segment(next_road,-1,to_lane_id);
+ }
}
- // else ignore juntions
}
}
}
}
}
-void COpendriveRoad::add_node(COpendriveRoadNode *node)
+unsigned int COpendriveRoad::add_node(COpendriveRoadNode *node)
{
for(unsigned int i=0;i<this->nodes.size();i++)
{
@@ -90,6 +136,8 @@ void COpendriveRoad::add_node(COpendriveRoadNode *node)
throw CException(_HERE_,"Node already present");
}
this->nodes.push_back(node);
+
+ return this->nodes.size()-1;
}
bool COpendriveRoad::node_exists_at(const TOpendriveWorldPoint &pose)
@@ -120,41 +168,6 @@ COpendriveRoadNode* COpendriveRoad::get_node_at(const TOpendriveWorldPoint &pose
return NULL;
}
-std::string COpendriveRoad::get_junction_road_id(OpenDRIVE &open_drive,std::string &incoming,std::string &connecting)
-{
- bool predecessor_match,successor_match;
- for (OpenDRIVE::road_iterator road_it(open_drive.road().begin()); road_it != open_drive.road().end(); ++road_it)
- {
- predecessor_match=false;
- successor_match=false;
- if(std::stoi(road_it->junction().get())!=-1)// process only junction road segments
- {
- // get predecessor
- if(road_it->lane_link().present())
- {
- if(road_it->lane_link().get().predecessor().present())// predecessor present
- {
- if(road_it->lane_link().get().predecessor().get().elementType().get()=="road")
- if(road_it->lane_link().get().predecessor().get().elementId().get()==incoming)
- predecessor_match=true;
- }
- if(road_it->lane_link().get().successor().present())// predecessor present
- {
- if(road_it->lane_link().get().successor().get().elementType().get()=="road")
- {
- if(road_it->lane_link().get().successor().get().elementId().get()==connecting)
- successor_match=true;
- }
- }
- }
- }
- if(predecessor_match && successor_match)
- return road_it->id().get();
- }
-
- return std::string("");
-}
-
void COpendriveRoad::load(const std::string &filename)
{
struct stat buffer;
@@ -185,50 +198,6 @@ void COpendriveRoad::load(const std::string &filename)
// link segments
this->link_segments(*open_drive);
// process junctions
- for(OpenDRIVE::junction_iterator junction_it(open_drive->junction().begin());junction_it!=open_drive->junction().end();++junction_it)
- {
- for(junction::connection_iterator connection_it(junction_it->connection().begin()); connection_it!=junction_it->connection().end();++connection_it)
- {
- std::string incoming_road_id;
- std::string connecting_road_id;
- std::string contact_point;
- if(connection_it->incomingRoad().present())
- incoming_road_id=connection_it->incomingRoad().get();
- else
- throw CException(_HERE_,"Connectivity information missing");
- if(connection_it->connectingRoad().present())
- connecting_road_id=connection_it->connectingRoad().get();
- else
- throw CException(_HERE_,"Connectivity information missing");
- if(connection_it->contactPoint().present())
- contact_point=connection_it->contactPoint().get();
- else
- throw CException(_HERE_,"Connectivity information missing");
- for(connection::laneLink_iterator lane_link_it(connection_it->laneLink().begin()); lane_link_it!=connection_it->laneLink().end();++lane_link_it)
- {
- int from_lane_id;
- int to_lane_id;
- if(lane_link_it->from().present())
- from_lane_id=lane_link_it->from().get();
- else
- throw CException(_HERE_,"Connectivity information missing");
- if(lane_link_it->to().present())
- to_lane_id=lane_link_it->to().get();
- else
- throw CException(_HERE_,"Connectivity information missing");
- // search the road segment starting at incoming_road_id and ending at connecting_road_id
- std::string road_id=this->get_junction_road_id(*open_drive,incoming_road_id,connecting_road_id);
- if(!road_id.empty())
- {
- COpendriveRoadSegment &prev_road=(*this)[incoming_road_id];
- COpendriveRoadSegment &road=(*this)[road_id];
- COpendriveRoadSegment &next_road=(*this)[connecting_road_id];
- prev_road.link_segment(road,from_lane_id,-1);
- road.link_segment(next_road,-1,to_lane_id);
- }
- }
- }
- }
}catch (const xml_schema::exception& e){
std::ostringstream os;
os << e;