Solved a bug when computing the closest point: a valid pose was returned even if it was not found.

src/opendrive_road_node.cpp

@@ -596,6 +596,7 @@ double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendrive
   double dist,min_dist=std::numeric_limits<double>::max();
   double angle;
   double length,closest_length=std::numeric_limits<double>::max();
+  TOpendriveWorldPose tmp_pose;
 
   closest_pose.x=std::numeric_limits<double>::max();
   closest_pose.y=std::numeric_limits<double>::max();
@@ -604,15 +605,16 @@ double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendrive
   {
     if(&this->links[i]->get_prev()==this)// links starting at this node
     {
-      length=this->links[i]->find_closest_world_pose(pose,closest_pose);
-      angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
+      length=this->links[i]->find_closest_world_pose(pose,tmp_pose);
+      angle=diff_angle(normalize_angle(pose.heading),tmp_pose.heading);
       if(fabs(angle)<angle_tol)
       {
-        dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+        dist=sqrt(pow(tmp_pose.x-pose.x,2.0)+pow(tmp_pose.y-pose.y,2.0));
         if(dist<min_dist)
         {
           min_dist=dist;
           closest_length=length;
+          closest_pose=tmp_pose;
         }
       }
     }