From 885d9dfac5ef5a27f955298f8f61f17056fddbf2 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 1 Jan 2021 20:01:49 +0100
Subject: [PATCH] Modified the example to input point candidates.
---
src/examples/opendrive_test.cpp | 21 +++++++++++++++++++--
1 file changed, 19 insertions(+), 2 deletions(-)
diff --git a/src/examples/opendrive_test.cpp b/src/examples/opendrive_test.cpp
index 1f901b8..0f7fb59 100644
--- a/src/examples/opendrive_test.cpp
+++ b/src/examples/opendrive_test.cpp
@@ -6,9 +6,12 @@
int main(int argc, char *argv[])
{
- std::vector<double> x,y;
+ TOpendriveWorldPoint closest_point;
+ double length,x,y,heading;
+// std::vector<double> x,y;
COpendriveRoad road;
- 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/add_road_full.xodr";
+ std::string xml_file="/home/sergi/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/adc_road.xodr";
try
{
@@ -17,6 +20,7 @@ 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);
@@ -28,6 +32,19 @@ int main(int argc, char *argv[])
std::cout << x[k] << "," << y[k] << std::endl;
}
}
+*/
+ 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);
+ std::cout << "closest point to: " << x << "," << y << "," << heading << " -> " << closest_point.x << "," << closest_point.y << "," << closest_point.heading << " at " << length << " m" << std::endl;
+
}
catch (CException &e)
{
--
GitLab