diff --git a/include/opendrive_road.h b/include/opendrive_road.h
index a4d7ae398526fc934dc92cbffc65568138365215..d3b1556e906b86bd3d185a518900d304ae7ebe74 100644
--- a/include/opendrive_road.h
+++ b/include/opendrive_road.h
@@ -42,7 +42,7 @@ class COpendriveRoad
     unsigned int get_num_nodes(void) const;
     const COpendriveRoadNode& get_node(unsigned int index) const;
     const COpendriveRoadSegment& operator[](std::size_t index);
-    double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point);
+    double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1);
     void operator=(const COpendriveRoad& object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
     ~COpendriveRoad();
diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index 65d5fdb6174c4b8ecd840b57c7fab103bfc46cde..0e083dedf01bb0a5257b68f3a0752f0f8c07775c 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -48,7 +48,7 @@ class COpendriveRoadNode
     const COpendriveLane &get_lane(unsigned int index) const;
     unsigned int get_num_geometries(void) const;
     const COpendriveGeometry &get_geometry(unsigned int index) const;
-    double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point);
+    double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1);
     friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
     ~COpendriveRoadNode();
 };
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index 2f3710df7fe6dcb6bb7eb18539f0a8b3c414d325..44baef130e0aa113e4424ed0982258f21b6e5c3e 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -316,23 +316,23 @@ const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
     throw CException(_HERE_,"Invalid segment index");
 }
 
-double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point)
+double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol)
 {
   double dist,min_dist=std::numeric_limits<double>::max();
-  double angle,min_angle=std::numeric_limits<double>::max();
+  double angle;
   TOpendriveWorldPoint point;
   double length,closest_length;
 
   for(unsigned int i=0;i<this->nodes.size();i++)
   {
-    length=this->nodes[i]->get_closest_point(x,y,heading,point);
+    length=this->nodes[i]->get_closest_point(x,y,heading,point,angle_tol);
+    dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
+    std::cout << point.x << "," << point.y << "," << point.heading << "," << length << "," << dist << std::endl;
     angle=diff_angle(normalize_angle(heading),point.heading);
-    if(angle<min_angle)
+    if(fabs(angle)<angle_tol)
     {
-      dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
       if(dist<min_dist)
       {
-        min_angle=angle;
         min_dist=dist;
         closest_point=point;
         closest_length=length;
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index 7e21d0e438be4f1ae7397e2fdcfdbc1ba4501d32..3c0dfeaf6befd43b642b2f4dabc39ab7620469bf 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -275,10 +275,10 @@ const COpendriveGeometry &COpendriveRoadNode::get_geometry(unsigned int index) c
     throw CException(_HERE_,"Invalid geometry index");
 }
 
-double COpendriveRoadNode::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point)
+double COpendriveRoadNode::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol)
 {
   double dist,min_dist=std::numeric_limits<double>::max();
-  double angle,min_angle=std::numeric_limits<double>::max();
+  double angle;
   double length,closest_length;
   TOpendriveWorldPoint target;
   TPoint point;
@@ -291,13 +291,13 @@ double COpendriveRoadNode::get_closest_point(double x, double y,double heading,T
     if(&this->links[i]->get_prev()==this)// links starting at this node
     {
       length=this->links[i]->find_closest_world_point(target,point);
+      std::cout << "  " << point.x << "," << point.y << "," << point.heading << "," << length << std::endl;
       angle=diff_angle(normalize_angle(heading),point.heading);
-      if(angle<min_angle)
+      if(fabs(angle)<angle_tol)
       {
         dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
         if(dist<min_dist)
         {
-          min_angle=angle;
           min_dist=dist;
           closest_point.x=point.x;
           closest_point.y=point.y;