diff --git a/src/examples/opendrive_test.cpp b/src/examples/opendrive_test.cpp
index 1f901b88a6c38cfc6a52f1ba090731b0568a3e0d..0f7fb59ebbd7d8770a16f1ae14369a8c12e819a2 100644
--- a/src/examples/opendrive_test.cpp
+++ b/src/examples/opendrive_test.cpp
@@ -6,9 +6,12 @@
 
 int main(int argc, char *argv[])
 {
-  std::vector<double> x,y;
+  TOpendriveWorldPoint closest_point;
+  double length,x,y,heading;
+//  std::vector<double> x,y;
   COpendriveRoad road;
-  std::string xml_file="/home/sergi/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/add_road_full.xodr";
+//  std::string xml_file="/home/sergi/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/add_road_full.xodr";
+  std::string xml_file="/home/sergi/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/adc_road.xodr";
   
   try
   {
@@ -17,6 +20,7 @@ int main(int argc, char *argv[])
     road.set_min_road_length(0.01);
     road.load(xml_file);
 //    std::cout << road << std::endl;
+/*
     for(unsigned int i=0;i<road.get_num_nodes();i++)
     {
       const COpendriveRoadNode &node=road.get_node(i);
@@ -28,6 +32,19 @@ int main(int argc, char *argv[])
           std::cout << x[k] << "," << y[k] << std::endl;
       }
     }
+*/
+    if(argc!=4)
+    {
+      std::cout << "Invalid number of parameters" << std::endl;
+      std::cout << argv[0] << " x y heading" << std::endl;
+      return -1;
+    }
+    x=std::stod(argv[1]);
+    y=std::stod(argv[2]);
+    heading=std::stod(argv[3]);
+    length=road.get_closest_point(x,y,heading,closest_point);
+    std::cout << "closest point to: " << x << "," << y << "," << heading << " -> " << closest_point.x << "," << closest_point.y << "," << closest_point.heading << " at " << length << " m" << std::endl;
+    
   }
   catch (CException &e)
   {