From 3dddbc16b2b09d9bfd650d1fe74ad068cb93d6b1 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 8 Jan 2021 15:36:55 +0100
Subject: [PATCH] Updated the example program to test the creatiion of partial
roads.
---
src/examples/opendrive_test.cpp | 60 ++++++++++++++++++++++++++++-----
1 file changed, 52 insertions(+), 8 deletions(-)
diff --git a/src/examples/opendrive_test.cpp b/src/examples/opendrive_test.cpp
index 0f7fb59..7798ea4 100644
--- a/src/examples/opendrive_test.cpp
+++ b/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)
{
--
GitLab