Solved several bugs:

  * New nodes are first linked before adding them to the lane. This avoids using invalid lane end positions.
  * In the get_start_point() and get_end_point() functions: the lane object address is compared instead of the lane ID.
  * In the get_start_point() and get_end_point() functions: for left lanes, the last node is used instead of the first one to get the desired point.

src/opendrive_lane.cpp

@@ -88,11 +88,11 @@ void COpendriveLane::add_node(COpendriveRoadNode *node)
   for(unsigned int i=0;i<this->nodes.size();i++)
     if(this->nodes[i]==node)
       return;
+  // link the new node with the previous one in the current lane, if any
+  if(this->nodes.size()>0)
+    this->nodes[this->nodes.size()-1]->add_link(node,OD_MARK_NONE);
   // add the new node
   this->nodes.push_back(node);
-  // link the new node with the previous one in the current lane, if any
-  if(this->nodes.size()>1)
-    this->nodes[this->nodes.size()-2]->add_link(node,OD_MARK_NONE);
 }
 
 void COpendriveLane::set_resolution(double res)
@@ -244,14 +244,14 @@ TOpendriveWorldPoint COpendriveLane::get_start_point(void)
     track_point.t=this->get_offset()-this->get_width()/2.0;
     track_point.s=0.0;
     for(unsigned int i=0;i<this->nodes[0]->get_num_lanes();i++)
-      if(this->nodes[0]->get_lane(i).get_id()==this->id)
+      if(&this->nodes[0]->get_lane(i)==this)
         this->nodes[0]->get_geometry(i).get_world_pose(track_point,world_point);
   }
   else
   {
     track_point.t=this->get_offset()+this->get_width()/2.0;
     for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
-      if(this->nodes[this->nodes.size()-1]->get_lane(i).get_id()==this->id)
+      if(&this->nodes[this->nodes.size()-1]->get_lane(i)==this)
       {
         track_point.s=this->nodes[this->nodes.size()-1]->get_geometry(i).get_length();
         this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);
@@ -271,17 +271,17 @@ TOpendriveWorldPoint COpendriveLane::get_end_point(void)
   track_point.heading=0.0;
   if(this->id>0)// left lane
   { 
-    track_point.t=this->get_offset()-this->get_width()/2.0;
+    track_point.t=this->get_offset()+this->get_width()/2.0;
     track_point.s=0.0;
-    for(unsigned int i=0;i<this->nodes[0]->get_num_lanes();i++)
-      if(this->nodes[0]->get_lane(i).get_id()==this->id)
-        this->nodes[0]->get_geometry(i).get_world_pose(track_point,world_point);
+    for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
+      if(&this->nodes[this->nodes.size()-1]->get_lane(i)==this)
+        this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);
   }
   else
   { 
-    track_point.t=this->get_offset()+this->get_width()/2.0;
+    track_point.t=this->get_offset()-this->get_width()/2.0;
     for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
-      if(this->nodes[this->nodes.size()-1]->get_lane(i).get_id()==this->id)
+      if(&this->nodes[this->nodes.size()-1]->get_lane(i)==this)
       {
         track_point.s=this->nodes[this->nodes.size()-1]->get_geometry(i).get_length();
         this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);