diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index 98f92646bf3b7f0eb9d076bfa8204d5c46cc1c7a..a4e0dce7db7f8e6a6c1dfbbc3a5f43715d067727 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -1119,7 +1119,7 @@ double COpendriveRoad::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorl
 {
   double dist,min_dist=std::numeric_limits<double>::max();
   TOpendriveWorldPose pose_found;
-  double length,closest_length;
+  double length,closest_length=std::numeric_limits<double>::max();
 
   for(unsigned int i=0;i<this->nodes.size();i++)
   {
@@ -1312,7 +1312,7 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(TOpendriveWorldPose &star
   new_road.add_segment(new_segment);
   new_segment_ref[(COpendriveRoadSegment *)segment]=new_segment;
   new_road.add_neighbors(segment,new_segment_ref,new_node_ref,new_lane_ref,new_link_ref,tmp_start_pose,tmp_end_pose);
-  while(segment!=end_segment || node_side!=end_segment_side)
+  while((segment!=end_segment || node_side!=end_segment_side) && rem_length>this->resolution)
   {
     segment=segment->get_next_path_segment(path_nodes,node_side,node_side);
     if((rem_length-segment->get_length())<0.0)