Added support to process the junctions.

Added support to link road segments.

include/opendrive_lane.h

@@ -27,8 +27,8 @@ class COpendriveLane
     double offset;
     int id;
   protected:
-    void link_left_lane(COpendriveLane *lane);
-    void link_right_lane(COpendriveLane *lane);
+    void link_neightbour_lane(COpendriveLane *lane);
+    void link_lane(COpendriveLane *lane,opendrive_mark_t mark);
     void add_node(COpendriveRoadNode *node);
     void set_resolution(double res);
     void set_scale_factor(double scale);

include/opendrive_link.h

-#ifndef _OPNEDRIVE_LINK_H
+#ifndef _OPENDRIVE_LINK_H
 #define _OPENDRIVE_LINK_H
 
 #include "g2_spline.h"

include/opendrive_road.h

@@ -15,7 +15,9 @@ class COpendriveRoad
     double scale_factor;
     double min_road_length;
   protected:
-    void add_road_nodes(std::unique_ptr<OpenDRIVE::road_type> &road_info);
+    COpendriveRoadSegment &operator[](std::string &key);
+    void link_segments(OpenDRIVE &open_drive);
+    std::string get_junction_road_id(OpenDRIVE &open_drive,std::string &incoming,std::string &connecting);
   public:
     COpendriveRoad();
     COpendriveRoad(const COpendriveRoad& object);

include/opendrive_road_node.h

@@ -18,16 +18,19 @@ class COpendriveRoadNode
     TOpendriveWorldPoint start_point;
     COpendriveLane *lane;
     COpendriveGeometry *geometry;
+    unsigned int index;
   protected:
     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 index);
   public:
     COpendriveRoadNode();
     COpendriveRoadNode(const COpendriveRoadNode &object);
     void load(const planView::geometry_type &node_info,COpendriveLane *lane);
     double get_resolution(void) const;
     double get_scale_factor(void) const;
+    unsigned int get_index(void) const;
     TOpendriveWorldPoint get_start_pose(void) const;
     unsigned int get_num_links(void) const;
     const COpendriveLink &get_link(unsigned int index) const;

include/opendrive_road_segment.h

@@ -19,8 +19,8 @@ class COpendriveRoadSegment
     double resolution;
     double scale_factor;
     double min_road_length;
-    unsigned int num_right_lanes;
-    unsigned int num_left_lanes;
+    int num_right_lanes;
+    int num_left_lanes;
     std::vector<COpendriveSignal *> signals;
     std::vector<COpendriveObject *> objects;
     std::vector<COpendriveRoadSegment *> next;
@@ -35,7 +35,9 @@ class COpendriveRoadSegment
     void update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs);
     void add_lanes(lanes::laneSection_type &lane_section);
     void add_nodes(OpenDRIVE::road_type &road_info);
-    void link_lanes(lanes::laneSection_type &lane_section);
+    void link_neightbour_lanes(lanes::laneSection_type &lane_section);
+    void link_segment(COpendriveRoadSegment &segment);
+    void link_segment(COpendriveRoadSegment &segment,int from, int to);
   public:
     COpendriveRoadSegment();
     COpendriveRoadSegment(const COpendriveRoadSegment &object);

src/opendrive_lane.cpp

@@ -40,38 +40,52 @@ COpendriveLane::COpendriveLane(const COpendriveLane &object)
   this->id=object.id;
 }
 
-void COpendriveLane::link_left_lane(COpendriveLane *lane)
+void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
 {
   if(lane!=NULL)
   {
     if(this->get_num_nodes()!=lane->get_num_nodes())
-      throw CException(_HERE_,"Impossible to link lanes because they have different number of nodes");
-    for(unsigned int i=1;i<this->get_num_nodes();i++)
+      throw CException(_HERE_,"Impossible to liink lanes because they have different number of nodes");
+    if(this->get_num_nodes()>0 && lane->get_num_nodes()>0)
     {
-      this->nodes[i]->add_link(lane->nodes[i-1],this->left_mark);
-      lane->nodes[i]->add_link(this->nodes[i-1],this->left_mark);
-    } 
-    this->left_lane=lane;
+      if(this->id*lane->get_id()<0) // oposite directions
+      {
+        for(unsigned int i=0;i<this->get_num_nodes();i++)
+        {
+          this->nodes[i]->add_link(lane->nodes[lane->get_num_nodes()-i-1],this->left_mark);
+          lane->nodes[lane->get_num_nodes()-i-1]->add_link(this->nodes[i],this->left_mark);
+          lane->right_mark=this->left_mark;
+        }
+      }
+      else
+      {
+        for(unsigned int i=0;i<this->get_num_nodes()-1;i++)
+        {
+          this->nodes[i]->add_link(lane->nodes[i+1],this->left_mark);
+          lane->nodes[i]->add_link(this->nodes[i+1],this->left_mark);
+          if(this->id>0)// left lanes
+            this->right_mark=lane->left_mark;
+          else// right lanes
+            this->left_mark=lane->right_mark;
+        }
+      } 
+      this->left_lane=lane;
+      lane->right_lane=this;
+    }
+    else
+      throw CException(_HERE_,"One of the lanes to link has no nodes");
   }
-  else
-    this->left_lane=NULL;
 }
 
-void COpendriveLane::link_right_lane(COpendriveLane *lane)
+void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark)
 {
   if(lane!=NULL)
   {
-    if(this->get_num_nodes()!=lane->get_num_nodes())
-      throw CException(_HERE_,"Impossible to link lanes because they have different number of nodes");
-    for(unsigned int i=1;i<this->get_num_nodes();i++)
-    {
-      this->nodes[i]->add_link(lane->nodes[i-1],this->right_mark);
-      lane->nodes[i]->add_link(this->nodes[i-1],this->right_mark);
-    } 
-    this->right_lane=lane;
+    if(this->get_num_nodes()>0 && lane->get_num_nodes()>0)
+      this->nodes[this->nodes.size()-1]->add_link(lane->nodes[0],mark);
+    else 
+      throw CException(_HERE_,"One of the lanes to link has no nodes");
   }
-  else
-    this->right_lane=NULL;
 }
 
 void COpendriveLane::add_node(COpendriveRoadNode *node)
@@ -80,6 +94,7 @@ void COpendriveLane::add_node(COpendriveRoadNode *node)
     if(this->nodes[i]==node)
       return;
   // add the new node
+  node->set_index(this->nodes.size());
   this->nodes.push_back(node);
   // link the new node with the previous one in the current lane, if any
   if(this->nodes.size()>1)
@@ -143,32 +158,37 @@ void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegmen
   }
   this->speed=lane_info.speed().begin()->max().get();
   //lane mark
-  if(lane_info.roadMark().size()!=1)
+  if(lane_info.roadMark().size()==0)
+    mark=OD_MARK_NONE;
+  else if(lane_info.roadMark().size()>1)
   {
     error << "Only one road mark supported for lane " << this->id;
     throw CException(_HERE_,error.str());
   }
-  if(lane_info.roadMark().begin()->type().present())
+  else if(lane_info.roadMark().size()==1)
   {
-    if(lane_info.roadMark().begin()->type().get()=="none")
-      mark=OD_MARK_NONE;
-    else if(lane_info.roadMark().begin()->type().get()=="solid")
-      mark=OD_MARK_SOLID;
-    else if(lane_info.roadMark().begin()->type().get()=="broken")
-      mark=OD_MARK_BROKEN;
-    else if(lane_info.roadMark().begin()->type().get()=="solid_solid")
-      mark=OD_MARK_SOLID_SOLID;
-    else if(lane_info.roadMark().begin()->type().get()=="solid_broken")
-      mark=OD_MARK_SOLID_BROKEN;
-    else if(lane_info.roadMark().begin()->type().get()=="broken_solid")
-      mark=OD_MARK_BROKEN_SOLID;
-    else if(lane_info.roadMark().begin()->type().get()=="broken_broken")
-      mark=OD_MARK_BROKEN_BROKEN;
+    if(lane_info.roadMark().begin()->type1().present())
+    {
+      if(lane_info.roadMark().begin()->type1().get()=="none")
+        mark=OD_MARK_NONE;
+      else if(lane_info.roadMark().begin()->type1().get()=="solid")
+        mark=OD_MARK_SOLID;
+      else if(lane_info.roadMark().begin()->type1().get()=="broken")
+        mark=OD_MARK_BROKEN;
+      else if(lane_info.roadMark().begin()->type1().get()=="solid_solid")
+        mark=OD_MARK_SOLID_SOLID;
+      else if(lane_info.roadMark().begin()->type1().get()=="solid_broken")
+        mark=OD_MARK_SOLID_BROKEN;
+      else if(lane_info.roadMark().begin()->type1().get()=="broken_solid")
+        mark=OD_MARK_BROKEN_SOLID;
+      else if(lane_info.roadMark().begin()->type1().get()=="broken_broken")
+        mark=OD_MARK_BROKEN_BROKEN;
+      else
+        mark=OD_MARK_NONE;
+    }
     else
       mark=OD_MARK_NONE;
   }
-  else
-    mark=OD_MARK_NONE;
   if(this->id<0)//right lanes
     this->right_mark=mark;
   else
@@ -236,17 +256,17 @@ void COpendriveLane::operator=(const COpendriveLane &object)
 
 std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
 {
-  out << "    id: " << lane.id << " (offset: " << lane.offset << ")" << std::endl;
+  out << "  Lane: " << lane.id << " (offset: " << lane.offset << ")" << std::endl;
   out << "    width: " << lane.get_width() << std::endl;
   out << "    speed: " << lane.get_speed() << std::endl;
   if(lane.segment!=NULL)
     out << "    Parent road segment: " << lane.segment->get_name() << std::endl;
   else 
     out << "    No parent segment" << std::endl;
-  if(lane.left_lane!=NULL)
-    out << "    No left lane (" << std::endl;
+  if(lane.left_lane==NULL)
+    out << "    No left lane (";
   else
-    out << "    Left lane " << lane.get_id() << " (" << std::endl;
+    out << "    Left lane " << lane.left_lane->get_id() << " (";
   switch(lane.left_mark)
   {
     case OD_MARK_NONE:
@@ -271,10 +291,10 @@ std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
       out << "broken broken)" << std::endl;
       break;
   }
-  if(lane.right_lane!=NULL)
-    out << "    No right lane (" << std::endl;
+  if(lane.right_lane==NULL)
+    out << "    No right lane (";
   else
-    out << "    Right lane " << lane.get_id() << " (" << std::endl;
+    out << "    Right lane " << lane.right_lane->get_id() << " (";
   switch(lane.right_mark)
   {
     case OD_MARK_NONE:
@@ -301,7 +321,7 @@ std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
   }
   out << "    Number of nodes: " << lane.nodes.size() << std::endl;
   for(unsigned int i=0;i<lane.nodes.size();i++)
-    out << *lane.nodes[i] << std::endl;
+    out << *lane.nodes[i];
 
   return out;
 }

src/opendrive_link.cpp

 #include "opendrive_link.h"
+#include "opendrive_road_node.h"
 
 COpendriveLink::COpendriveLink()
 {
-
+  this->prev=NULL;
+  this->next=NULL;
+  this->spline=NULL;
+  this->mark=OD_MARK_NONE;
+  this->resolution=DEFAULT_RESOLUTION;
+  this->scale_factor=DEFAULT_SCALE_FACTOR;
 }
 
 COpendriveLink::COpendriveLink(const COpendriveLink &object)
 {
-
+  this->prev=object.prev;
+  this->next=object.next;
+  this->spline=new CG2Spline(*object.spline);
+  this->mark=object.mark;
+  this->resolution=object.resolution;
+  this->scale_factor=object.scale_factor;
 }
 
 void COpendriveLink::set_prev(COpendriveRoadNode *node)
 {
-
+  this->prev=node;
 }
 
 void COpendriveLink::set_next(COpendriveRoadNode *node)
 {
-
+  this->next=node;
 }
 
 void COpendriveLink::set_road_mark(opendrive_mark_t mark)
 {
-
+  this->mark=mark;
 }
 
 void COpendriveLink::set_resolution(double res)
 {
-
+  this->resolution=res;
 }
 
 void COpendriveLink::set_scale_factor(double scale)
 {
-
+  this->scale_factor=scale;
 }
 
 void COpendriveLink::generate(void)
@@ -42,27 +53,27 @@ void COpendriveLink::generate(void)
 
 const COpendriveRoadNode &COpendriveLink::get_prev(void) const
 {
-
+  return *this->prev;
 }
 
 const COpendriveRoadNode &COpendriveLink::get_next(void) const
 {
-
+  return *this->next;
 }
 
 opendrive_mark_t COpendriveLink::get_road_mark(void) const
 {
-
+  return this->mark;
 }
 
 double COpendriveLink::get_resolution(void) const
 {
-
+  return this->resolution;
 }
 
 double COpendriveLink::get_scale_factor(void) const
 {
-
+  return this->scale_factor;
 }
 
 double COpendriveLink::find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point)
@@ -92,16 +103,58 @@ double COpendriveLink::get_length(void) const
 
 void COpendriveLink::operator=(const COpendriveLink &object)
 {
-
+  this->prev=object.prev;
+  this->next=object.next;
+  this->spline=new CG2Spline(*object.spline);
+  this->mark=object.mark;
+  this->resolution=object.resolution;
+  this->scale_factor=object.scale_factor;
 }
 
 std::ostream& operator<<(std::ostream& out, COpendriveLink &link)
 {
-
+  out << "        Previous node: " << link.get_prev().get_index() << " of lane " << link.get_prev().get_lane().get_id() << " of road " << link.get_prev().get_lane().get_segment().get_name() << std::endl;
+  out << "        Next node: " << link.get_next().get_index() << " of lane " << link.get_next().get_lane().get_id() << " of road " << link.get_next().get_lane().get_segment().get_name() << std::endl;
+  out << "        Road mark: ";
+  switch(link.mark)
+  {
+    case OD_MARK_NONE:
+      out << "no mark" << std::endl;
+      break;
+    case OD_MARK_SOLID:
+      out << "solid" << std::endl;
+      break;
+    case OD_MARK_BROKEN:
+      out << "broken" << std::endl;
+      break;
+    case OD_MARK_SOLID_SOLID:
+      out << "solid solid" << std::endl;
+      break;
+    case OD_MARK_SOLID_BROKEN:
+      out << "solid broken" << std::endl;
+      break;
+    case OD_MARK_BROKEN_SOLID:
+      out << "broken solid" << std::endl;
+      break;
+    case OD_MARK_BROKEN_BROKEN:
+      out << "broken broken" << std::endl;
+      break;
+  }
+
+  return out;
 }
 
 COpendriveLink::~COpendriveLink()
 {
-
+  this->prev=NULL;
+  this->next=NULL;
+  if(this->spline!=NULL)
+  {
+    delete this->spline;
+    this->spline=NULL;
+  }
+  this->mark=OD_MARK_NONE;
+  this->resolution=DEFAULT_RESOLUTION;
+  this->scale_factor=DEFAULT_SCALE_FACTOR;
 }
 

src/opendrive_road.cpp

@@ -30,10 +30,88 @@ COpendriveRoad::COpendriveRoad(const COpendriveRoad& object)
     this->segments[i]->update_references(new_references);
 }
 
+COpendriveRoadSegment &COpendriveRoad::operator[](std::string &key)
+{
+  for(unsigned int i=0;i<this->segments.size();i++)
+  {
+    if(this->segments[i]->get_id()==(unsigned int)std::stoi(key))
+      return *this->segments[i];
+  }
+
+  throw CException(_HERE_,"No road segment with the given ID");
+}
+
+void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
+{
+  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 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")
+          {
+            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().successor().get().elementType().get()=="road")
+          {
+            COpendriveRoadSegment &link_segment=(*this)[road_it->lane_link().get().successor().get().elementId().get()];
+            segment.link_segment(link_segment);
+          }
+          // else ignore juntions
+        }
+      }
+    }
+  }
+}
+
+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;
-  COpendriveRoadSegment *segment;
 
   if(stat(filename.c_str(),&buffer)==0)
   {
@@ -47,7 +125,7 @@ void COpendriveRoad::load(const std::string &filename)
       for (OpenDRIVE::road_iterator road_it(open_drive->road().begin()); road_it != open_drive->road().end(); ++road_it)
       {
         try{
-          segment=new COpendriveRoadSegment();
+          COpendriveRoadSegment *segment=new COpendriveRoadSegment();
           segment->set_resolution(this->resolution);
           segment->set_scale_factor(this->scale_factor);
           segment->load(*road_it);
@@ -56,6 +134,55 @@ void COpendriveRoad::load(const std::string &filename)
           std::cout << e.what() << std::endl;
         }
       }
+      // 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;
@@ -154,7 +281,7 @@ std::ostream& operator<<(std::ostream& out, COpendriveRoad &road)
   out << "Scale factor: " << road.scale_factor << std::endl;
   out << "Minimum road lenght: " << road.min_road_length << std::endl;
   for(unsigned int i=0;i<road.segments.size();i++)
-    std::cout << *road.segments[i] << std::endl;
+    std::cout << *road.segments[i];
 
   return out;
 }

src/opendrive_road_node.cpp

@@ -10,6 +10,7 @@ COpendriveRoadNode::COpendriveRoadNode()
   this->start_point.heading=0.0;
   this->lane=NULL;
   this->geometry=NULL;
+  this->index=-1;
 }
 
 COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
@@ -29,6 +30,7 @@ COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
     new_link=new COpendriveLink(*object.links[i]);
     this->links.push_back(new_link);
   }
+  this->index=object.index;
 }
 
 void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark)
@@ -72,6 +74,11 @@ void COpendriveRoadNode::set_scale_factor(double scale)
     this->links[i]->set_scale_factor(scale);
 }
 
+void COpendriveRoadNode::set_index(unsigned int index)
+{
+  this->index=index;
+}
+
 void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendriveLane *lane)
 {
   TOpendriveTrackPoint track_point;
@@ -123,6 +130,11 @@ double COpendriveRoadNode::get_scale_factor(void) const
   return this->scale_factor;
 }
 
+unsigned int COpendriveRoadNode::get_index(void) const
+{
+  return this->index;
+}
+
 TOpendriveWorldPoint COpendriveRoadNode::get_start_pose(void) const
 {
   return this->start_point;
@@ -173,22 +185,28 @@ void COpendriveRoadNode::operator=(const COpendriveRoadNode &object)
     new_link=new COpendriveLink(*object.links[i]);
     this->links.push_back(new_link);
   }
+  this->index=object.index;
 }
 
 std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node)
 {
+  out << "    Node " << node.index << ":" << std::endl;
+  out << "      parent lane: " << node.lane->get_id() << std::endl;
   out << "      start point: x: " << node.start_point.x << " y: " << node.start_point.y << " heading: " << node.start_point.heading << std::endl;
   if(node.lane!=NULL)
     out << "      lane id: " << node.lane->get_id() << std::endl;
   else
     out << "      Does not belong to a lane." << std::endl;
   if(node.geometry!=NULL)
-    out << *node.geometry << std::endl;
+    out << *node.geometry;
   else
     out << "      Does not have any associated geometry." << std::endl;
   out << "      Number of links: " << node.links.size() << std::endl;
   for(unsigned int i=0;i<node.links.size();i++)
-    out << *node.links[i] << std::endl;
+  {
+    out << "      Link " << i << ":" << std::endl;
+    out << *node.links[i];
+  }
   return out;
 }
 

src/opendrive_road_segment.cpp

@@ -57,7 +57,7 @@ COpendriveRoadSegment::COpendriveRoadSegment(const COpendriveRoadSegment &object
 
 void COpendriveRoadSegment::free(void)
 {
-  for(unsigned int i=-this->num_right_lanes;i<=this->num_left_lanes && i!=0;i++)
+  for(int i=-this->num_right_lanes;i<=this->num_left_lanes && i!=0;i++)
   {
     delete this->lanes[i];
     this->lanes[i]=NULL;
@@ -170,7 +170,7 @@ void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
     for(geom_it=road_info.planView().geometry().begin(); geom_it!=road_info.planView().geometry().end(); ++geom_it)
     {
       lane_offset=0.0;
-      for(unsigned int i=-1;i>=-this->num_right_lanes;i--)
+      for(int i=-1;i>=-this->num_right_lanes;i--)
       {
         new_node=new COpendriveRoadNode();
         new_node->set_resolution(this->resolution);
@@ -188,7 +188,7 @@ void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
     {
       geom_it--;
       lane_offset=0.0;
-      for(unsigned int i=1;i<=this->num_left_lanes;i++)
+      for(int i=1;i<=this->num_left_lanes;i++)
       {
         new_node=new COpendriveRoadNode();
         new_node->set_resolution(this->resolution);
@@ -210,7 +210,7 @@ void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
   }
 }
 
-void COpendriveRoadSegment::link_lanes(lanes::laneSection_type &lane_section)
+void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_section)
 {
   opendrive_mark_t center_mark;
   center::lane_type center_lane;
@@ -218,66 +218,107 @@ void COpendriveRoadSegment::link_lanes(lanes::laneSection_type &lane_section)
   if(lane_section.center().lane().present())
   {
     center_lane=lane_section.center().lane().get();
-    if(center_lane.roadMark().begin()->type().present())
+    if(center_lane.roadMark().size()>1)
+      throw CException(_HERE_,"Only one road mark supported for lane");
+    else if(center_lane.roadMark().size()==0)
+      center_mark=OD_MARK_NONE;
+    else if(center_lane.roadMark().size()==1)
     {
-      if(center_lane.roadMark().begin()->type().get()=="none")
-        center_mark=OD_MARK_NONE;
-      else if(center_lane.roadMark().begin()->type().get()=="solid")
-        center_mark=OD_MARK_SOLID;
-      else if(center_lane.roadMark().begin()->type().get()=="broken")
-        center_mark=OD_MARK_BROKEN;
-      else if(center_lane.roadMark().begin()->type().get()=="solid_solid")
-        center_mark=OD_MARK_SOLID_SOLID;
-      else if(center_lane.roadMark().begin()->type().get()=="solid_broken")
-        center_mark=OD_MARK_SOLID_BROKEN;
-      else if(center_lane.roadMark().begin()->type().get()=="broken_solid")
-        center_mark=OD_MARK_BROKEN_SOLID;
-      else if(center_lane.roadMark().begin()->type().get()=="broken_broken")
-        center_mark=OD_MARK_BROKEN_BROKEN;
+      if(center_lane.roadMark().begin()->type1().present())
+      {
+        if(center_lane.roadMark().begin()->type1().get()=="none")
+          center_mark=OD_MARK_NONE;
+        else if(center_lane.roadMark().begin()->type1().get()=="solid")
+          center_mark=OD_MARK_SOLID;
+        else if(center_lane.roadMark().begin()->type1().get()=="broken")
+          center_mark=OD_MARK_BROKEN;
+        else if(center_lane.roadMark().begin()->type1().get()=="solid_solid")
+          center_mark=OD_MARK_SOLID_SOLID;
+        else if(center_lane.roadMark().begin()->type1().get()=="solid_broken")
+          center_mark=OD_MARK_SOLID_BROKEN;
+        else if(center_lane.roadMark().begin()->type1().get()=="broken_solid")
+         center_mark=OD_MARK_BROKEN_SOLID;
+        else if(center_lane.roadMark().begin()->type1().get()=="broken_broken")
+          center_mark=OD_MARK_BROKEN_BROKEN;
+        else
+          center_mark=OD_MARK_NONE;
+      }
       else
         center_mark=OD_MARK_NONE;
     }
+  }
+  for(int i=-this->num_right_lanes;i<0;i++)
+  {
+    if(this->lanes.find(i+1)!=this->lanes.end())// if the lane exists 
+      this->lanes[i]->link_neightbour_lane(this->lanes[i+1]);
     else
-      center_mark=OD_MARK_NONE;
+      this->lanes[i]->left_mark=OD_MARK_NONE;
   }
-  for(unsigned int i=-this->num_right_lanes;i<0;i++)
+  for(int i=this->num_left_lanes;i>0;i--)
+  {
+    if(this->lanes.find(i-1)!=this->lanes.end())// if the lane exists 
+      this->lanes[i]->link_neightbour_lane(this->lanes[i-1]);
+    else
+      this->lanes[i]->right_mark=OD_MARK_NONE;
+  }
+  if(this->lanes.find(1)!=this->lanes.end() && this->lanes.find(-1)!=this->lanes.end())
+  {
+    this->lanes[-1]->left_mark=center_mark;
+    this->lanes[1]->right_mark=center_mark;
+    this->lanes[-1]->link_neightbour_lane(this->lanes[1]);
+  }
+}
+
+void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment)
+{
+  for(unsigned int i=0;i<this->next.size();i++)
+    if(this->next[i]->get_id()==segment.get_id())// the segment is already included
+      return;
+  this->next.push_back(&segment);
+  // link lanes
+  for(int i=-this->num_right_lanes;i<0;i++)
   {
-    if(i==-this->num_right_lanes)// right most lane
+    for(int j=i-1;j<=i+1;j++)
     {
-      if((i+1)==0)//center lane
+      if(segment.lanes.find(j)!=segment.lanes.end())
       {
-        if(this->lanes.find(i+2)!=this->lanes.end())
-          this->lanes[i]->link_left_lane(this->lanes[i+2]);
-        this->lanes[i]->left_mark=center_mark;
-      }
-      else
-      {
-        if(this->lanes.find(i+1)!=this->lanes.end())
-          this->lanes[i]->link_left_lane(this->lanes[i+1]);
-        this->lanes[i]->left_mark=this->lanes[i+1]->right_mark;
+        if(j==i-1)
+          this->lanes[i]->link_lane(segment.lanes[j],this->lanes[i]->right_mark);
+        else if(j==i)
+          this->lanes[i]->link_lane(segment.lanes[j],OD_MARK_NONE);
+        else
+          this->lanes[i]->link_lane(segment.lanes[j],this->lanes[i]->left_mark);
       }
     }
   }
-  for(unsigned int i=this->num_left_lanes;i>0;i--)
+  for(int i=1;i<=segment.num_left_lanes;i++)
   {
-    if(i==this->num_left_lanes)// right most lane
+    for(int j=i-1;j<=i+1;j++)
     {
-      if((i-1)==0)//center lane
-      {
-        if(this->lanes.find(i-2)!=this->lanes.end())
-          this->lanes[i]->link_right_lane(this->lanes[i-2]);
-        this->lanes[i]->right_mark=center_mark;
-      }
-      else
+      if(this->lanes.find(j)!=this->lanes.end())
       {
-        if(this->lanes.find(i-1)!=this->lanes.end())
-          this->lanes[i]->link_left_lane(this->lanes[i-1]);
-        this->lanes[i]->right_mark=this->lanes[i+1]->left_mark;
+        if(j==i-1)
+          segment.lanes[i]->link_lane(this->lanes[j],segment.lanes[i]->right_mark);
+        else if(j==i)
+          segment.lanes[i]->link_lane(this->lanes[j],OD_MARK_NONE);
+        else
+          segment.lanes[i]->link_lane(this->lanes[j],segment.lanes[i]->left_mark);
       }
     }
   }
 }
 
+void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment,int from, int to)
+{
+  for(unsigned int i=0;i<this->next.size();i++)
+    if(this->next[i]->get_id()==segment.get_id())// the segment is already included
+      return;
+  this->next.push_back(&segment);
+  // link lanes
+  if(this->lanes.find(from)!=this->lanes.end() && segment.lanes.find(to)!=segment.lanes.end())
+    this->lanes[from]->link_lane(segment.lanes[to],OD_MARK_NONE);
+}
+
 void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
 {
   std::map<int,COpendriveLane *>::const_iterator lane_it;
@@ -303,7 +344,7 @@ void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
   // add road nodes
   this->add_nodes(road_info);
   // link lanes
-  this->link_lanes(*lane_section);
+  this->link_neightbour_lanes(*lane_section);
   // add road signals
   if(road_info.signals().present())
   {
@@ -448,7 +489,7 @@ void COpendriveRoadSegment::operator=(const COpendriveRoadSegment &object)
 
 std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment)
 {
-  out << "Road " << segment.get_name() << std::endl;
+  out << "Road " << segment.get_name() << " (" << segment.get_id() << ")" << std::endl;
   out << "  Previous road segments: " << segment.prev.size() << std::endl;
   for(unsigned int i=0;i<segment.prev.size();i++)
     out << "    " << segment.prev[i]->get_name() << std::endl;
@@ -456,17 +497,17 @@ std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment)
   for(unsigned int i=0;i<segment.next.size();i++)
     out << "    " << segment.next[i]->get_name() << std::endl;
   out << "  Number of right lanes: " << segment.num_right_lanes << std::endl;
-  for(unsigned int i=-1;i>=-segment.num_right_lanes;i--)
-    out << *segment.lanes[i] << std::endl;
+  for(int i=-1;i>=-segment.num_right_lanes;i--)
+    out << *segment.lanes[i];
   out << "  Number of left lanes: " << segment.num_left_lanes << std::endl;
-  for(unsigned int i=1;i<=segment.num_left_lanes;i++)
-    out << *segment.lanes[i] << std::endl;
+  for(int i=1;i<=segment.num_left_lanes;i++)
+    out << *segment.lanes[i];
   out << "  Number of signals: " << segment.signals.size() << std::endl;
   for(unsigned int i=0;i<segment.signals.size();i++)
-    std::cout << *segment.signals[i] << std::endl;
+    std::cout << *segment.signals[i];
   out << "  Number of objects: " << segment.objects.size() << std::endl;
   for(unsigned int i=0;i<segment.objects.size();i++)
-    std::cout << *segment.objects[i] << std::endl;
+    std::cout << *segment.objects[i];
 
   return out;
 }