Solved some bugs when linking road segments.

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;