Updated the opendrive example file.

src/examples/opendrive_test.cpp

@@ -9,9 +9,10 @@ int main(int argc, char *argv[])
 //  TOpendriveWorldPose closest_pose;
   std::vector<double> x,y;
   COpendriveRoad road,new_road,new_road2;
-  std::vector<unsigned int> path,new_path;
+  std::vector<unsigned int> path,new_path,new_path2;
   TOpendriveWorldPose end_pose,start_pose;
-  double length=5.0;
+  double length=100.0;
+//  std::string xml_file="/home/shernand/Downloads/osm2xodr_old/output.xodr";
   std::string xml_file="/home/shernand/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/add.xodr";
 //  std::string xml_file="/home/shernand/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/adc_road.xodr";
 //  std::string xml_file="/home/shernand/iri-lab/iri_team_ws/src/iri_adc_launch/data/adc/adc.xodr";
@@ -20,10 +21,9 @@ int main(int argc, char *argv[])
   {
     road.set_resolution(0.1);
     road.set_scale_factor(1.0);
-    road.set_min_road_length(0.1);
+    road.set_min_road_length(0.01);
     road.load(xml_file);
-    //std::cout << road << std::endl;
-
+//    std::cout << road << std::endl;
     for(unsigned int i=0;i<road.get_num_nodes();i++)
     {
       const COpendriveRoadNode &node=road.get_node(i);
@@ -35,6 +35,45 @@ int main(int argc, char *argv[])
           std::cout << x[k] << "," << y[k] << std::endl;
       }
     }
+
+    return 0;
+    path.clear();
+    path.push_back(51);
+    path.push_back(34);
+    path.push_back(55);
+    path.push_back(0);
+    path.push_back(2);
+    path.push_back(4);
+    path.push_back(6);
+    path.push_back(47);
+    path.push_back(8);
+    path.push_back(49);
+    path.push_back(10);
+    path.push_back(12);
+    path.push_back(14);
+    path.push_back(16);
+    path.push_back(59);
+    path.push_back(18);
+    end_pose.x=60.2742;
+    end_pose.y=105.16;
+    end_pose.heading=3.14159;
+    start_pose.x=52.2;
+    start_pose.y=15.0;
+    start_pose.heading=0.0;
+    new_path=road.get_sub_road(path,end_pose,new_road,false);
+    new_path2=new_road.get_sub_road(start_pose,end_pose,length,new_road2,new_path,-1);
+
+    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;
+      }
+    }
     return 0;
     /*
     path.clear();