Improved general coding.

Improved Exception report information.
clean_references() functions no longer update references.

include/opendrive_geometry.h

@@ -25,6 +25,9 @@ class COpendriveGeometry
     virtual void load_params(const planView::geometry_type &geometry_info) = 0;
     virtual std::string get_name(void)=0;
     void set_scale_factor(double scale);
+    void set_start_point(TOpendriveWorldPoint &point);
+    void set_max_s(double s);
+    void set_min_s(double s);
   public:
     virtual COpendriveGeometry *clone(void) = 0;
     bool get_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const;
@@ -32,6 +35,9 @@ class COpendriveGeometry
     bool in_range(double s) const;
     double get_length(void) const;
     virtual void get_curvature(double &start,double &end)=0;
+    double get_max_s(void) const;
+    double get_min_s(void) const;
+    TOpendriveWorldPoint get_start_point(void) const;
     void operator=(const COpendriveGeometry &object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveGeometry &geom);
     virtual ~COpendriveGeometry();

include/opendrive_lane.h

@@ -34,15 +34,18 @@ class COpendriveLane
   protected:
     COpendriveLane();
     COpendriveLane(const COpendriveLane &object);
-    void load(const right::lane_type &lane_info,COpendriveRoadSegment *segment);
+    void load(const ::lane &lane_info,COpendriveRoadSegment *segment);
     void link_neightbour_lane(COpendriveLane *lane);
     void link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start,bool to_start);
     void add_next_lane(COpendriveLane *lane);
     void add_prev_lane(COpendriveLane *lane);
+    void remove_lane(COpendriveLane *lane);
     void add_node(COpendriveRoadNode *node);
     bool has_node(COpendriveRoadNode *node);
-    bool has_node(unsigned int index);
+    bool has_node_with_index(unsigned int index);
     void set_parent_segment(COpendriveRoadSegment *segment);
+    void set_left_lane(COpendriveLane *lane,opendrive_mark_t mark);
+    void set_right_lane(COpendriveLane *lane,opendrive_mark_t mark);
     void set_resolution(double res);
     void set_scale_factor(double scale);
     void set_width(double width);
@@ -75,7 +78,7 @@ class COpendriveLane
     double get_length(void) const;
     TOpendriveLocalPoint transform_to_local_point(TOpendriveTrackPoint &track);
     TOpendriveWorldPoint transform_to_world_point(TOpendriveTrackPoint &track);
-    unsigned int get_closest_node(TOpendriveWorldPoint &point,double angle_tol=0.1);
+    unsigned int get_closest_node_index(TOpendriveWorldPoint &point,double angle_tol=0.1);
     friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
     ~COpendriveLane();
 };

include/opendrive_link.h

@@ -46,9 +46,7 @@ class COpendriveLink
     const COpendriveLane &get_parent_lane(void) const;
     double get_resolution(void) const;
     double get_scale_factor(void) const;
-    double find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point);
-    double get_track_point_world(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world);
-    double get_track_point_local(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
+    double find_closest_world_point(TOpendriveWorldPoint &world,TOpendriveWorldPoint &point);
     void get_trajectory(std::vector<double> &x, std::vector<double> &y,double start_length=0.0, double end_length=-1.0) const;
     void get_trajectory(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double start_length=0.0, double end_length=-1.0) const;
     double get_length(void) const;

include/opendrive_road.h

@@ -23,13 +23,13 @@ class COpendriveRoad
     double scale_factor;
     double min_road_length;
   protected:
-    COpendriveRoadSegment &operator[](std::string &key);
+    COpendriveRoadSegment *get_segment_by_id(std::string &id);
     void free(void);
     void link_segments(OpenDRIVE &open_drive);
     unsigned int add_node(COpendriveRoadNode *node);
-    void remove_node(COpendriveRoadNode *node);
+    bool remove_node(COpendriveRoadNode *node);
     unsigned int add_lane(COpendriveLane *lane);
-    void remove_lane(COpendriveLane *lane);
+    bool remove_lane(COpendriveLane *lane);
     void complete_open_lanes(void);
     void add_segment(COpendriveRoadSegment *segment,std::vector<unsigned int> &old_path,std::vector<unsigned int> &new_path);
     bool has_segment(COpendriveRoadSegment *segment);

include/opendrive_road_node.h

@@ -7,6 +7,13 @@
 #include "opendrive_lane.h"
 #include "opendrive_road_segment.h"
 
+typedef struct
+{
+  COpendriveLane * lane;
+  COpendriveGeometry *geometry;
+  COpendriveRoadSegment *segment;
+}TOpendriveRoadNodeParent;
+
 class COpendriveRoadNode
 {
   friend class COpendriveLane;
@@ -17,44 +24,46 @@ class COpendriveRoadNode
     std::vector<COpendriveLink *> links;
     double resolution;
     double scale_factor;
-    TOpendriveWorldPoint start_point;
-    std::vector<COpendriveLane *> lanes;
-    std::vector<COpendriveGeometry *> geometries;
-    std::vector<COpendriveRoadSegment *>parents;
+    TOpendriveWorldPoint point;
+    std::vector<TOpendriveRoadNodeParent> parents;
     unsigned int index;
   protected:
     COpendriveRoadNode();
     COpendriveRoadNode(const COpendriveRoadNode &object);
-    void load(const planView::geometry_type &geom_info,COpendriveLane *lane);
-    void add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane);
+    void free(void);
+    void load(const planView::geometry_type &geom_info,COpendriveLane *lane,COpendriveRoadSegment *segment);
+    bool add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane);
     void add_link(COpendriveLink *link);
     void remove_link(COpendriveLink *link);
+    bool has_link(COpendriveLink *link);
+    bool has_link_with(COpendriveRoadNode *node);
+    COpendriveLink *get_link_with(COpendriveRoadNode *node);
     void set_resolution(double res);
     void set_scale_factor(double scale);
     void set_index(unsigned int index);
-    void set_start_point(TOpendriveWorldPoint &start);
-    void add_parent_segment(COpendriveRoadSegment *segment);
-    void add_lane(const planView::geometry_type &geom_info,COpendriveLane *lane);
+    void set_point(TOpendriveWorldPoint &start);
+    void add_parent(const planView::geometry_type &geom_info,COpendriveLane *lane,COpendriveRoadSegment *segment);
+    void add_parent(COpendriveGeometry *geometry,COpendriveLane *lane,COpendriveRoadSegment *segment);
+    TOpendriveRoadNodeParent *get_parent_with_lane(const COpendriveLane *lane);
+    TOpendriveRoadNodeParent *get_parent_with_segment(const COpendriveRoadSegment *segment);
     bool updated(node_up_ref_t &refs);
+    COpendriveRoadNode *get_original_node(node_up_ref_t &node_refs);
     void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
     void clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
     void update_start_pose(COpendriveLane *lane,TOpendriveWorldPoint *start=NULL);
     void update_end_pose(COpendriveLane *lane,TOpendriveWorldPoint *end=NULL);
-    COpendriveLink *get_link_with(COpendriveRoadNode *end);
   public:
     double get_resolution(void) const;
     double get_scale_factor(void) const;
     unsigned int get_index(void) const;
-    unsigned int get_num_parent_segments(void) const;
+    unsigned int get_num_parents(void) const;
     const COpendriveRoadSegment& get_parent_segment(unsigned int index) const;
-    TOpendriveWorldPoint get_start_pose(void) const;
+    const COpendriveLane &get_parent_lane(unsigned int index) const;
+    const COpendriveGeometry &get_geometry(unsigned int index) const;
+    TOpendriveWorldPoint get_point(void) const;
     unsigned int get_num_links(void) const;
     const COpendriveLink &get_link(unsigned int index) const;
     const COpendriveLink &get_link_ending_at(unsigned int node_index) const;
-    unsigned int get_num_lanes(void) const; 
-    const COpendriveLane &get_lane(unsigned int index) const;
-    unsigned int get_num_geometries(void) const;
-    const COpendriveGeometry &get_geometry(unsigned int index) const;
     unsigned int get_closest_link(TOpendriveWorldPoint &point,double angle_tol=0.1) const;
     double get_closest_distance(TOpendriveWorldPoint &point,double angle_tol=0.1) const;
     double get_closest_point(TOpendriveWorldPoint &point,TOpendriveWorldPoint &closest_point,double angle_tol=0.1) const;

include/opendrive_road_segment.h

@@ -49,15 +49,18 @@ class COpendriveRoadSegment
     void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
     void clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
     void add_lanes(lanes::laneSection_type &lane_section);
+    void add_lane(const ::lane &lane_info);
     void remove_lane(COpendriveLane *lane);
     void add_nodes(OpenDRIVE::road_type &road_info);
-    void add_node(COpendriveRoadNode *node);
+    void add_node(const planView::geometry_type &geom_info,COpendriveLane *lane);
+    void add_empty_node(TOpendriveWorldPoint &point,COpendriveLane *lane);
     void remove_node(COpendriveRoadNode *node);
     bool has_node(COpendriveRoadNode *node);
     int get_node_side(COpendriveRoadNode *node);
     void link_neightbour_lanes(lanes::laneSection_type &lane_section);
-    void link_segment(COpendriveRoadSegment &segment);
-    void link_segment(COpendriveRoadSegment &segment,int from,bool from_start, int to,bool to_start);
+    void link_neightbour_lanes(void);
+    void link_segment(COpendriveRoadSegment *segment);
+    void link_segment(COpendriveRoadSegment *segment,int from,bool from_start, int to,bool to_start);
     bool connects_to(COpendriveRoadSegment *segment);
     bool connects_segments(COpendriveRoadSegment *segment1,COpendriveRoadSegment *segment2);
     COpendriveRoadSegment *get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,int node_side,TOpendriveWorldPoint *start=NULL,TOpendriveWorldPoint *end=NULL);

src/opendrive_geometry.cpp

@@ -23,9 +23,11 @@ COpendriveGeometry::COpendriveGeometry(const COpendriveGeometry &object)
 
 void COpendriveGeometry::print(std::ostream& out)
 {
+  TOpendriveWorldPoint tmp_point=this->get_start_point();
+
   out << "        Geometry " << this->get_name() << std::endl; 
   out << "          lenght: " << this->get_length() << std::endl;
-  out << "          pose: x = " << this->pose.x/this->scale_factor << ", y = " << this->pose.y/this->scale_factor << ", heading = " << this->pose.heading << std::endl;
+  out << "          pose: x = " << tmp_point.x << ", y = " << tmp_point.y << ", heading = " << tmp_point.heading << std::endl;
 }
 
 void COpendriveGeometry::load(const planView::geometry_type &geometry_info)
@@ -43,6 +45,23 @@ void COpendriveGeometry::set_scale_factor(double scale)
   this->scale_factor=scale;
 }
 
+void COpendriveGeometry::set_start_point(TOpendriveWorldPoint &point)
+{
+  this->pose.x=point.x*this->scale_factor;
+  this->pose.y=point.y*this->scale_factor;
+  this->pose.heading=point.heading;
+}
+
+void COpendriveGeometry::set_max_s(double s)
+{
+  this->max_s=s*this->scale_factor;
+}
+
+void COpendriveGeometry::set_min_s(double s)
+{
+  this->min_s=s*this->scale_factor;
+}
+
 bool COpendriveGeometry::get_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const
 {
   return this->transform_local_pose(track,local);
@@ -76,6 +95,27 @@ double COpendriveGeometry::get_length(void) const
   return (this->max_s-this->min_s)/this->scale_factor;
 }
 
+double COpendriveGeometry::get_max_s(void) const
+{
+  return this->max_s/this->scale_factor;
+}
+
+double COpendriveGeometry::get_min_s(void) const
+{
+  return this->min_s/this->scale_factor;
+}
+
+TOpendriveWorldPoint COpendriveGeometry::get_start_point(void) const
+{
+  TOpendriveWorldPoint tmp_point;
+  
+  tmp_point.x=this->pose.x/this->scale_factor;
+  tmp_point.y=this->pose.y/this->scale_factor;
+  tmp_point.heading=this->pose.heading;
+
+  return tmp_point;
+}
+
 void COpendriveGeometry::operator=(const COpendriveGeometry &object)
 {
   this->min_s = object.min_s;

src/opendrive_link.cpp

@@ -65,12 +65,12 @@ void COpendriveLink::generate(double start_curvature,double end_curvature,double
   TOpendriveWorldPoint node_start,node_end;
   TPoint start,end;
 
-  node_start=this->prev->get_start_pose();
+  node_start=this->prev->get_point();
   start.x=node_start.x;
   start.y=node_start.y;
   start.heading=node_start.heading;
   start.curvature=start_curvature;
-  node_end=this->next->get_start_pose();
+  node_end=this->next->get_point();
   end.x=node_end.x;
   end.y=node_end.y;
   end.heading=node_end.heading;
@@ -107,27 +107,33 @@ bool COpendriveLink::clean_references(node_up_ref_t &refs)
 
 void COpendriveLink::update_start_pose(TOpendriveWorldPoint *start)
 {
-  TPoint spline_start;
+  TPoint start_point;
   double length;
 
   if(start==NULL)
     return;
-  length=this->find_closest_world_point(*start,spline_start);
-  length=this->spline->get_length()-length;
-  this->spline->set_start_point(spline_start);
-  this->spline->generate(this->resolution,length);
+  if(this->spline!=NULL)
+  {
+    length=this->spline->find_closest_point(start->x,start->y,start_point);
+    length=this->spline->get_length()-length;
+    this->spline->set_start_point(start_point);
+    this->spline->generate(this->resolution,length);
+  }
 }
 
 void COpendriveLink::update_end_pose(TOpendriveWorldPoint *end)
 {
-  TPoint spline_end;
+  TPoint end_point;
   double length;
 
   if(end==NULL)
     return;
-  length=this->find_closest_world_point(*end,spline_end);
-  this->spline->set_end_point(spline_end);
-  this->spline->generate(this->resolution,length);
+  if(this->spline!=NULL)
+  {
+    length=this->spline->find_closest_point(end->x,end->y,end_point);
+    this->spline->set_end_point(end_point);
+    this->spline->generate(this->resolution,length);
+  }
 }
 
 const COpendriveRoadNode &COpendriveLink::get_prev(void) const
@@ -165,14 +171,17 @@ double COpendriveLink::get_scale_factor(void) const
   return this->scale_factor;
 }
 
-double COpendriveLink::find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point)
+double COpendriveLink::find_closest_world_point(TOpendriveWorldPoint &world,TOpendriveWorldPoint &point)
 {
+  TPoint spline_point;
   double length;
 
   if(this->spline!=NULL)
   {
-    length=this->spline->find_closest_point(world.x,world.y,point);
-    point.heading=normalize_angle(point.heading);
+    length=this->spline->find_closest_point(world.x,world.y,spline_point);
+    point.x=spline_point.x;
+    point.y=spline_point.y;
+    point.heading=normalize_angle(spline_point.heading);
   }
   else
     length=std::numeric_limits<double>::max();
@@ -180,16 +189,6 @@ double COpendriveLink::find_closest_world_point(TOpendriveWorldPoint &world,TPoi
   return length;
 }
 
-double COpendriveLink::get_track_point_world(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world)
-{
-
-}
-
-double COpendriveLink::get_track_point_local(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local)
-{
-
-}
-
 void COpendriveLink::get_trajectory(std::vector<double> &x, std::vector<double> &y,double start_length, double end_length) const
 {
   std::vector<double> curvature,heading;
@@ -234,6 +233,7 @@ std::ostream& operator<<(std::ostream& out, COpendriveLink &link)
 {
   out << "        Previous node: " << link.get_prev().get_index() << std::endl;
   out << "        Next node: " << link.get_next().get_index() << std::endl;
+  out << "        Parent segment: " << link.get_parent_segment().get_name() << " (lane " << link.get_parent_lane().get_id() << ")" << std::endl;
   out << "        Road mark: ";
   switch(link.get_road_mark())
   {

src/opendrive_road.cpp

@@ -46,17 +46,21 @@ COpendriveRoad::COpendriveRoad(const COpendriveRoad& object)
     new_segment_ref[object.segments[i]]=segment;
   }
   // update references
+  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
 }
 
-COpendriveRoadSegment &COpendriveRoad::operator[](std::string &key)
+COpendriveRoadSegment *COpendriveRoad::get_segment_by_id(std::string &id)
 {
+  std::stringstream error;
+
   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];
+    if(this->segments[i]->get_id()==(unsigned int)std::stoi(id))
+      return this->segments[i];
   }
 
-  throw CException(_HERE_,"No road segment with the given ID");
+  error << "No road segment with the " << id << " ID";
+  throw CException(_HERE_,error.str());
 }
 
 void COpendriveRoad::free(void)
@@ -85,11 +89,13 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
 {
   std::string predecessor_id,successor_id;
   std::string predecessor_contact,successor_contact;
+  COpendriveRoadSegment *prev_road,*road,*next_road;
+  std::stringstream error;
 
   for(OpenDRIVE::road_iterator road_it(open_drive.road().begin());road_it!=open_drive.road().end();++road_it)
   {
     // get current segment
-    COpendriveRoadSegment &road=(*this)[road_it->id().get()];
+    road=this->get_segment_by_id(road_it->id().get());
     // get predecessor and successor
     if(road_it->lane_link().present())
     {
@@ -114,14 +120,14 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
     {
       if(!predecessor_id.empty())
       {
-        COpendriveRoadSegment &prev_road=(*this)[predecessor_id];
-        prev_road.link_segment(road);
+        prev_road=this->get_segment_by_id(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);
+        next_road=this->get_segment_by_id(successor_id);
+        road->link_segment(next_road);
         successor_id.clear();
       }
     }
@@ -136,15 +142,21 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
           if(connection_it->incomingRoad().present())
             incoming_road_id=connection_it->incomingRoad().get();
           else
-            throw CException(_HERE_,"Connectivity information missing");
+          {
+            error << "Connectivity information missing for segment " << road->get_name() << ": No incomming segment";
+            throw CException(_HERE_,error.str());
+          }
           if(connection_it->connectingRoad().present())
             connecting_road_id=connection_it->connectingRoad().get();
           else
-            throw CException(_HERE_,"Connectivity information missing");
+          {
+            error << "Connectivity information missing for segment " << road->get_name() << ": No connecting segment";
+            throw CException(_HERE_,error.str());
+          }
           if(predecessor_id.compare(incoming_road_id)==0 && successor_id.compare(connecting_road_id)==0)// this is the connection
           {
-            COpendriveRoadSegment &prev_road=(*this)[predecessor_id];
-            COpendriveRoadSegment &next_road=(*this)[successor_id];
+            prev_road=this->get_segment_by_id(predecessor_id);
+            next_road=this->get_segment_by_id(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;
@@ -152,27 +164,40 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
               if(lane_link_it->from().present())
                 from_lane_id=lane_link_it->from().get();
               else
-                throw CException(_HERE_,"Connectivity information missing");
+              {
+                error << "Connectivity information missing for segment " << road->get_name() << ": lane link missing from identifier";
+                throw CException(_HERE_,error.str());
+              }
               if(lane_link_it->to().present())
                 to_lane_id=lane_link_it->to().get();
               else
-                throw CException(_HERE_,"Connectivity information missing");
+              {
+                error << "Connectivity information missing for segment " << road->get_name() << ": lane link missing to identifier";
+                throw CException(_HERE_,error.str());
+              }
               if(predecessor_contact=="end")
-                prev_road.link_segment(road,from_lane_id,false,-1,true);
+                prev_road->link_segment(road,from_lane_id,false,-1,true);
               else 
-                prev_road.link_segment(road,from_lane_id,true,-1,true);
-              TOpendriveWorldPoint end=road.get_lane(-1).get_end_point();
+                prev_road->link_segment(road,from_lane_id,true,-1,true);
+              TOpendriveWorldPoint end=road->get_lane(-1).get_end_point();
+              TOpendriveWorldPoint start;
               if(successor_contact=="end")
               {
-                TOpendriveWorldPoint start=next_road.get_lane(to_lane_id).get_end_point();
+                if(to_lane_id<0)
+                  start=next_road->get_lane(to_lane_id).get_end_point();
+                else
+                  start=next_road->get_lane(to_lane_id).get_start_point();
                 if(fabs(end.x-start.x)<this->resolution && fabs(end.y-start.y)<this->resolution)
-                  road.link_segment(next_road,-1,false,to_lane_id,false);
+                  road->link_segment(next_road,-1,false,to_lane_id,false);
               }
               else
               {
-                TOpendriveWorldPoint start=next_road.get_lane(to_lane_id).get_start_point();
+                if(to_lane_id<0)
+                  start=next_road->get_lane(to_lane_id).get_start_point();
+                else
+                  start=next_road->get_lane(to_lane_id).get_end_point();
                 if(fabs(end.x-start.x)<this->resolution && fabs(end.y-start.y)<this->resolution)
-                  road.link_segment(next_road,-1,false,to_lane_id,true);
+                  road->link_segment(next_road,-1,false,to_lane_id,true);
               }
             }
           }
@@ -184,17 +209,22 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
 
 unsigned int COpendriveRoad::add_node(COpendriveRoadNode *node)
 {
+  std::stringstream error;
+
   for(unsigned int i=0;i<this->nodes.size();i++)
   {
     if(this->nodes[i]==node)
-      throw CException(_HERE_,"Node already present");
+    {
+      error << "Node " << node->get_index() << " already present";
+      throw CException(_HERE_,error.str());
+    }
   }
   this->nodes.push_back(node);
 
   return this->nodes.size()-1;
 }
 
-void COpendriveRoad::remove_node(COpendriveRoadNode *node)
+bool COpendriveRoad::remove_node(COpendriveRoadNode *node)
 {
   std::vector<COpendriveRoadNode *>::iterator it;
 
@@ -204,26 +234,33 @@ void COpendriveRoad::remove_node(COpendriveRoadNode *node)
     {
       delete *it;
       it=this->nodes.erase(it);
-      break;
+      return true;
     }
     else
       it++;
   }
+
+  return false;
 }
 
 unsigned int COpendriveRoad::add_lane(COpendriveLane *lane)
 {
+  std::stringstream error;
+
   for(unsigned int i=0;i<this->lanes.size();i++)
   {
     if(this->lanes[i]==lane)
-      throw CException(_HERE_,"Lane already present");
+    {
+      error << "Lane " << lane->get_index() << " already present";
+      throw CException(_HERE_,error.str());
+    }
   }
   this->lanes.push_back(lane);
 
   return this->lanes.size()-1;
 }
 
-void COpendriveRoad::remove_lane(COpendriveLane *lane)
+bool COpendriveRoad::remove_lane(COpendriveLane *lane)
 {
   std::vector<COpendriveLane *>::iterator it;
 
@@ -233,44 +270,37 @@ void COpendriveRoad::remove_lane(COpendriveLane *lane)
     {
       delete *it;
       it=this->lanes.erase(it);
-      break;
+      return true;
     }
     else
       it++;
   }
+
+  return false;
 }
 
 void COpendriveRoad::complete_open_lanes(void)
 {
   std::vector<COpendriveLane *>::iterator lane_it;
-  COpendriveRoadNode *new_node;
   TOpendriveWorldPoint end_point;
-  unsigned int new_node_index;
 
   // remove all nodes and lanes not present in the path
   for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
   {
     if((*lane_it)->next.empty())// lane is not connected
     {
-      if((*lane_it)->get_id()<0)
+      try{
         end_point=(*lane_it)->get_end_point();
-      else
-        end_point=(*lane_it)->get_start_point();
-      if(!this->node_exists_at(end_point))// there is no node at the end point
-      {
-        new_node=new COpendriveRoadNode();
-        new_node->set_resolution(this->resolution);
-        new_node->set_scale_factor(this->scale_factor);
-        new_node->set_start_point(end_point);
-        new_node_index=this->add_node(new_node);
-        new_node->set_index(new_node_index);
-        (*lane_it)->add_node(new_node);
-        (*lane_it)->segment->add_node(new_node);
-        (*lane_it)->link_neightbour_lane((*lane_it)->left_lane);
-        (*lane_it)->link_neightbour_lane((*lane_it)->right_lane);
-      }
-      else
+        if(!this->node_exists_at(end_point))// there is no node at the end point
+        {
+          (*lane_it)->segment->add_empty_node(end_point,(*lane_it));
+          (*lane_it)->segment->link_neightbour_lanes();
+        }
+        else
+          lane_it++;
+      }catch(CException &e){
         lane_it++;
+      }
     }
     else
       lane_it++;
@@ -313,6 +343,8 @@ void COpendriveRoad::add_segment(COpendriveRoadSegment *segment,std::vector<unsi
     segment->nodes[i]->set_index(this->nodes.size());
     this->nodes.push_back(segment->nodes[i]);
   }
+  // update the road reference
+  segment->set_parent_road(this);
 }
 
 void COpendriveRoad::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
@@ -347,13 +379,7 @@ void COpendriveRoad::clean_references(segment_up_ref_t &segment_refs,lane_up_ref
 
   for(seg_it=this->segments.begin();seg_it!=this->segments.end();)
   {
-    if(segment_refs.find(*seg_it)!=segment_refs.end())
-    {
-      (*seg_it)=segment_refs[*seg_it];
-      (*seg_it)->clean_references(segment_refs,lane_refs,node_refs);
-      seg_it++;
-    }
-    else if((*seg_it)->updated(segment_refs))
+    if((*seg_it)->updated(segment_refs))
     {
       (*seg_it)->clean_references(segment_refs,lane_refs,node_refs);
       seg_it++;
@@ -363,24 +389,14 @@ void COpendriveRoad::clean_references(segment_up_ref_t &segment_refs,lane_up_ref
   }
   for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
   {
-    if(lane_refs.find(*lane_it)!=lane_refs.end())
-    {
-      (*lane_it)=lane_refs[*lane_it];
-      lane_it++;
-    }
-    else if(!(*lane_it)->updated(lane_refs))
+    if(!(*lane_it)->updated(lane_refs))
       lane_it=this->lanes.erase(lane_it);
     else
       lane_it++;
   }
   for(node_it=this->nodes.begin();node_it!=this->nodes.end();)
   {
-    if(node_refs.find(*node_it)!=node_refs.end())
-    {
-      (*node_it)=node_refs[*node_it];
-      node_it++;
-    }
-    else if(!(*node_it)->updated(node_refs))
+    if(!(*node_it)->updated(node_refs))
       node_it=this->nodes.erase(node_it);
     else
       node_it++;
@@ -399,7 +415,7 @@ void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
     present=false;
     for(unsigned int i=0;i<path_nodes.size();i++)
     {
-      if((*lane_it)->has_node(path_nodes[i]))
+      if((*lane_it)->has_node_with_index(path_nodes[i]))
       {
         present=true;
         break;
@@ -412,7 +428,7 @@ void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
       {
         for(unsigned int i=0;i<path_nodes.size();i++)
         {
-          if(neightbour_lane->has_node(path_nodes[i]))
+          if(neightbour_lane->has_node_with_index(path_nodes[i]))
           {
             present=true;
             break;
@@ -427,7 +443,7 @@ void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
       {
         for(unsigned int i=0;i<path_nodes.size();i++)
         {
-          if(neightbour_lane->has_node(path_nodes[i]))
+          if(neightbour_lane->has_node_with_index(path_nodes[i]))
           {
             present=true;
             break;
@@ -478,7 +494,7 @@ bool COpendriveRoad::node_exists_at(TOpendriveWorldPoint &pose)
 
   for(unsigned int i=0;i<nodes.size();i++)
   {
-    node_pose=this->nodes[i]->get_start_pose();
+    node_pose=this->nodes[i]->get_point();
     if(std::abs(pose.x-node_pose.x)<0.01 && std::abs(pose.y-node_pose.y)<0.01)
       return true;
   }
@@ -492,7 +508,7 @@ COpendriveRoadNode* COpendriveRoad::get_node_at(TOpendriveWorldPoint &pose)
 
   for(unsigned int i=0;i<nodes.size();i++)
   {
-    node_pose=this->nodes[i]->get_start_pose();
+    node_pose=this->nodes[i]->get_point();
     if(std::abs(pose.x-node_pose.x)<0.01 && std::abs(pose.y-node_pose.y)<0.01)
       return this->nodes[i];
   }
@@ -528,7 +544,6 @@ void COpendriveRoad::load(const std::string &filename)
       }
       // link segments
       this->link_segments(*open_drive);
-      // process junctions
     }catch (const xml_schema::exception& e){
       std::ostringstream os;
       os << e;
@@ -588,10 +603,15 @@ unsigned int COpendriveRoad::get_num_segments(void) const
 
 const COpendriveRoadSegment& COpendriveRoad::get_segment(unsigned int index) const
 {
+  std::stringstream error;
+
   if(index>=0 && index<this->segments.size())
     return *this->segments[index];
   else
-    throw CException(_HERE_,"Invalid segment index");
+  {
+    error << "Invalid segment index " << index;
+    throw CException(_HERE_,error.str());
+  }
 }
 
 unsigned int COpendriveRoad::get_num_nodes(void) const
@@ -601,21 +621,30 @@ unsigned int COpendriveRoad::get_num_nodes(void) const
 
 const COpendriveRoadNode& COpendriveRoad::get_node(unsigned int index) const
 {
+  std::stringstream error;
+
   if(index>=0 && index<this->nodes.size())
     return *this->nodes[index];
   else
-    throw CException(_HERE_,"Invalid node index");
+  {
+    error << "Invalid node index " << index;
+    throw CException(_HERE_,error.str());
+  }
 }
 
 const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
 {
+  std::stringstream error;
+
   if(index>=0 && index<this->segments.size())
     return *this->segments[index];
   else
-    throw CException(_HERE_,"Invalid segment index");
+  {
+    error << "Invalid node index " << index;
+    throw CException(_HERE_,error.str());
+  }
 }
 
-
 unsigned int COpendriveRoad::get_closest_node(TOpendriveWorldPoint &point,double angle_tol)
 {
   double dist,min_dist=std::numeric_limits<double>::max();
@@ -681,7 +710,8 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
   lane_up_ref_t new_lane_ref;
   node_up_ref_t new_node_ref;
   COpendriveRoadNode *node,*next_node;
-  COpendriveRoadSegment *segment,*new_segment,*original_seg1,original_seg2;
+  COpendriveRoadSegment *segment,*new_segment;
+//  CopendriveRoadSegment *original_seg1,*original_seg2;
   COpendriveLink *link;
   std::vector<unsigned int> new_path_nodes;
   unsigned int link_index;
@@ -793,6 +823,7 @@ void COpendriveRoad::operator=(const COpendriveRoad& object)
     new_segment_ref[object.segments[i]]=segment;
   }
   // update references
+  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
 }
 
 std::ostream& operator<<(std::ostream& out, COpendriveRoad &road)