Updated the example program to test the creatiion of partial roads.

src/examples/opendrive_test.cpp

@@ -6,10 +6,11 @@
 
 int main(int argc, char *argv[])
 {
-  TOpendriveWorldPoint closest_point;
-  double length,x,y,heading;
+//  TOpendriveWorldPoint closest_point;
 //  std::vector<double> x,y;
-  COpendriveRoad road;
+  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";
   
@@ -33,18 +34,61 @@ int main(int argc, char *argv[])
       }
     }
 */
+/*
     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);
+    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;
-    
+*/  
+    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);
+    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);
+    std::cout << "road2" << std::endl;
+    std::cout << new_road << std::endl;
+    path.clear();
+    path.push_back(0);
+    path.push_back(22);
+    path.push_back(2);
+    path.push_back(4);
+    path.push_back(6);
+    path.push_back(8);
+    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);
+    std::cout << "road3" << std::endl;
+    std::cout << new_road << std::endl;
   }
   catch (CException &e)
   {