Updated the example application.

src/examples/opendrive_test.cpp

@@ -8,19 +8,21 @@ int main(int argc, char *argv[])
 {
 //  TOpendriveWorldPose closest_pose;
   std::vector<double> x,y;
-  COpendriveRoad road,new_road;
+  COpendriveRoad road,new_road,new_road2;
   std::vector<unsigned int> path;
   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";
+  double length=5.0;
+//  std::string xml_file="/home/shernand/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/add_road_full.xodr";
+  std::string xml_file="/home/shernand/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/adc_road.xodr";
   
   try
   {
-    road.set_resolution(0.01);
-    road.set_scale_factor(10.0);
-    road.set_min_road_length(0.01);
+    road.set_resolution(0.1);
+    road.set_scale_factor(1.0);
+    road.set_min_road_length(0.1);
     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);
@@ -32,6 +34,7 @@ int main(int argc, char *argv[])
           std::cout << x[k] << "," << y[k] << std::endl;
       }
     }
+    return 0;
 /*
     path.push_back(4);
     start_pose.x=10.0;
@@ -44,7 +47,7 @@ int main(int argc, char *argv[])
     std::cout << "road1" << std::endl;
     std::cout << new_road << std::endl;
 */
-/*
+    /*
     path.clear();
     path.push_back(4);
     path.push_back(6);
@@ -53,15 +56,56 @@ int main(int argc, char *argv[])
     path.push_back(67);
     path.push_back(69);
     path.push_back(71);
-    start_pose.x=10.0;
+    start_pose.x=15.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;
+    end_pose.heading=3.14159;
+    */
+    /*
+    path.clear();
+    path.push_back(61);
+    path.push_back(63);
+    path.push_back(65);
+    path.push_back(153);
+    path.push_back(10);
+    path.push_back(12);
+    path.push_back(14);
+    end_pose.x=15.0;
+    end_pose.y=0.0;
+    end_pose.heading=3.14159;
+    start_pose.x=18.5;
+    start_pose.y=22.0;
+    start_pose.heading=0.0;
+    */
+   
+    path.clear();
+    path.push_back(0);
+    path.push_back(98);
+    path.push_back(100);
+    path.push_back(16);
+    path.push_back(17);
+    path.push_back(18);
+    path.push_back(133);
+    path.push_back(134);
+    path.push_back(59);
+    path.push_back(157);
+    path.push_back(158);
+    path.push_back(11);
+    path.push_back(13);
+    path.push_back(15);
+    end_pose.x=9.5;
+    end_pose.y=0.6;
+    end_pose.heading=3.14159;
+    start_pose.x=6.7;
+    start_pose.y=2.3;
+    start_pose.heading=1.5707;
+    
     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);
@@ -74,6 +118,22 @@ int main(int argc, char *argv[])
       }
     }
 */
+
+    length=21.0;
+    new_road.get_sub_road(start_pose,length,1.0,new_road2);
+//    std::cout << "road2" << std::endl;
+//    std::cout << new_road2 << std::endl;
+    for(unsigned int i=0;i<new_road2.get_num_nodes();i++)
+    {
+      const COpendriveRoadNode &node=new_road2.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)
     {