Solved a bug when the lanes have more than one node.

src/opendrive_lane.cpp

@@ -45,7 +45,7 @@ void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
   if(lane!=NULL)
   {
     if(this->get_num_nodes()!=lane->get_num_nodes())
-      throw CException(_HERE_,"Impossible to liink lanes because they have different number of nodes");
+      throw CException(_HERE_,"Impossible to link lanes because they have different number of nodes");
     if(this->get_num_nodes()>0 && lane->get_num_nodes()>0)
     {
       if(this->id*lane->get_id()<0) // oposite directions
@@ -349,7 +349,16 @@ void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t
   for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
   {
     if(i<start_node_index)
+    {
+      for(exist_it=new_node_ref.begin();exist_it!=new_node_ref.end();exist_it++)
+        if((*it)==exist_it->second)
+        {
+          delete *it;
+          new_node_ref.erase(exist_it);
+          break;
+        }
       it=this->nodes.erase(it);
+    }
     else
     {
       exists=false;
@@ -389,7 +398,16 @@ void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &
   for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
   {
     if(i>end_node_index)
+    {
+      for(exist_it=new_node_ref.begin();exist_it!=new_node_ref.end();exist_it++)
+        if((*it)==exist_it->second)
+        {
+          delete *it;
+          new_node_ref.erase(exist_it);
+          break;
+        }
       it=this->nodes.erase(it);
+    }
     else
     {
       exists=false;

src/opendrive_road.cpp

@@ -431,7 +431,6 @@ void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
         (*lane_it)->segment->remove_lane(*lane_it);
         for(unsigned int i=0;i<(*lane_it)->nodes.size();i++)
           this->remove_node((*lane_it)->nodes[i]);
-        delete *lane_it;
         lane_it=this->lanes.erase(lane_it);
       }
       else
@@ -771,6 +770,7 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
     }
   }
   new_road.update_references(new_segment_ref,new_lane_ref,new_node_ref);
+  new_road.clean_references(new_segment_ref,new_lane_ref,new_node_ref);
   // add additional nodes not explicitly in the path
 /*
   for(unsigned int i=0;i<path_nodes.size();i++)
@@ -796,7 +796,6 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
 */
   // remove unconnected elements
   new_road.prune(new_path_nodes);
-  new_road.clean_references(new_segment_ref,new_lane_ref,new_node_ref);
   new_road.complete_open_lanes();
 
   return new_path_nodes;