Function to find the closest point in the road operational.

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();

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();
 };

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;

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;