Commit a2e7040b authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Updated the example application.

parent 02330a92
......@@ -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)
{
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment