Updated the example.

src/examples/opendrive_test.cpp

@@ -6,13 +6,13 @@
 
 int main(int argc, char *argv[])
 {
-//  TOpendriveWorldPoint closest_point;
-//  std::vector<double> x,y;
+//  TOpendriveWorldPose closest_pose;
+  std::vector<double> x,y;
   COpendriveRoad road,new_road;
   std::vector<unsigned int> path;
-  TOpendriveWorldPoint end_point,start_point;
-//  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";
+  TOpendriveWorldPose end_pose,start_pose;
+  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
   {
@@ -21,7 +21,6 @@ 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);
@@ -33,6 +32,47 @@ int main(int argc, char *argv[])
           std::cout << x[k] << "," << y[k] << std::endl;
       }
     }
+/*
+    path.push_back(4);
+    start_pose.x=10.0;
+    start_pose.y=0.0;
+    start_pose.heading=0.0;
+    end_pose.x=17.0;
+    end_pose.y=0.0;
+    end_pose.heading=0.0;
+    road.get_sub_road(path,start_pose,end_pose,new_road);
+    std::cout << "road1" << std::endl;
+    std::cout << new_road << std::endl;
+*/
+/*
+    path.clear();
+    path.push_back(4);
+    path.push_back(6);
+    path.push_back(8);
+    path.push_back(150);
+    path.push_back(67);
+    path.push_back(69);
+    path.push_back(71);
+    start_pose.x=10.0;
+    start_pose.y=0.0;
+    start_pose.heading=0.0;
+    end_pose.x=18.5;
+    end_pose.y=22.0;
+    end_pose.heading=0.0;
+    road.get_sub_road(path,start_pose,end_pose,new_road);
+//    std::cout << "road2" << std::endl;
+//    std::cout << new_road << std::endl;
+    for(unsigned int i=0;i<new_road.get_num_nodes();i++)
+    {
+      const COpendriveRoadNode &node=new_road.get_node(i);
+      for(unsigned int j=0;j<node.get_num_links();j++)
+      {
+        const COpendriveLink &link=node.get_link(j);
+        link.get_trajectory(x,y);
+        for(unsigned int k=0;k<x.size();k++)
+          std::cout << x[k] << "," << y[k] << std::endl;
+      }
+    }
 */
 /*
     if(argc!=4)
@@ -41,33 +81,36 @@ int main(int argc, char *argv[])
       std::cout << argv[0] << " x y heading" << std::endl;
       return -1;
     }
-    end_point.x=std::stod(argv[1]);
-    end_point.y=std::stod(argv[2]);
-    end_point.heading=std::stod(argv[3]);
-    length=road.get_closest_point(end_point,closest_point);
-    std::cout << "closest point to: " << x << "," << y << "," << heading << " -> " << closest_point.x << "," << closest_point.y << "," << closest_point.heading << " at " << length << " m" << std::endl;
+    end_pose.x=std::stod(argv[1]);
+    end_pose.y=std::stod(argv[2]);
+    end_pose.heading=std::stod(argv[3]);
+    length=road.get_closest_pose(end_pose,closest_pose);
+    std::cout << "closest pose to: " << x << "," << y << "," << heading << " -> " << closest_pose.x << "," << closest_pose.y << "," << closest_pose.heading << " at " << length << " m" << std::endl;
 */  
+/*
     path.push_back(0);
-    start_point.x=1.0;
-    start_point.y=0.0;
-    start_point.heading=0.0;
-    end_point.x=2.0;
-    end_point.y=0.0;
-    end_point.heading=0.0;
-    road.get_sub_road(path,start_point,end_point,new_road);
+    start_pose.x=1.0;
+    start_pose.y=0.0;
+    start_pose.heading=0.0;
+    end_pose.x=2.0;
+    end_pose.y=0.0;
+    end_pose.heading=0.0;
+    road.get_sub_road(path,start_pose,end_pose,new_road);
     std::cout << "road1" << std::endl;
     std::cout << new_road << std::endl;
+*/
+/*
     path.clear();
     path.push_back(0);
     path.push_back(22);
     path.push_back(2);
-    start_point.x=1.0;
-    start_point.y=0.0;
-    start_point.heading=0.0;
-    end_point.x=5.0;
-    end_point.y=0.0;
-    end_point.heading=0.0;
-    road.get_sub_road(path,start_point,end_point,new_road);
+    start_pose.x=1.0;
+    start_pose.y=0.0;
+    start_pose.heading=0.0;
+    end_pose.x=5.0;
+    end_pose.y=0.0;
+    end_pose.heading=0.0;
+    road.get_sub_road(path,start_pose,end_pose,new_road);
     std::cout << "road2" << std::endl;
     std::cout << new_road << std::endl;
     path.clear();
@@ -80,15 +123,31 @@ int main(int argc, char *argv[])
     path.push_back(10);
     path.push_back(25);
     path.push_back(21);
-    start_point.x=1.0;
-    start_point.y=0.0;
-    start_point.heading=0.0;
-    end_point.x=3.0;
-    end_point.y=1.5;
-    end_point.heading=0.0;
-    road.get_sub_road(path,start_point,end_point,new_road);
+    path.push_back(24);
+    path.push_back(1);
+    start_pose.x=1.0;
+    start_pose.y=0.0;
+    start_pose.heading=0.0;
+    end_pose.x=2.0;
+    end_pose.y=0.0;
+    end_pose.heading=0.0;
+    road.get_sub_road(path,start_pose,end_pose,new_road);
     std::cout << "road3" << std::endl;
     std::cout << new_road << std::endl;
+*/
+/*
+    for(unsigned int i=0;i<new_road.get_num_nodes();i++)
+    {
+      const COpendriveRoadNode &node=new_road.get_node(i);
+      for(unsigned int j=0;j<node.get_num_links();j++)
+      {
+        const COpendriveLink &link=node.get_link(j);
+        link.get_trajectory(x,y);
+        for(unsigned int k=0;k<x.size();k++)
+          std::cout << x[k] << "," << y[k] << std::endl;
+      }
+    }
+*/
   }
   catch (CException &e)
   {