diff --git a/src/examples/build_from_scratch_test.cpp b/src/examples/build_from_scratch_test.cpp index 967189947e644efcc7fb1d7f44126b5442d351cf..f19e9aceae41a5617555e10faa59fe7c398147a1 100644 --- a/src/examples/build_from_scratch_test.cpp +++ b/src/examples/build_from_scratch_test.cpp @@ -11,6 +11,7 @@ int main(int argc, char *argv[]) CJunction *junction0,*junction1; Eigen::MatrixXi connectivity; std::vector<unsigned int> id_map; + std::vector<double> x,y,heading; road0=new CRoad(); road0->set_num_lanes(1); @@ -110,6 +111,12 @@ int main(int argc, char *argv[]) junction1->link_roads_by_id(road4->get_id(),road1->get_id()); junction1->link_lanes_by_id(road4->get_id(),0,road1->get_id(),0); + map.get_lane_geometry(x,y,heading); + for(unsigned int i=0;i<x.size();i++) + std::cout << x[i] << "," << y[i] << "," << heading[i] << std::endl; + + return 0; + map.get_segment_connectivity(connectivity,id_map); for(unsigned int i=0;i<id_map.size();i++) std::cout << i << " -> " << id_map[i] << " "; diff --git a/src/examples/opendrive_import_test.cpp b/src/examples/opendrive_import_test.cpp index 294da6cb41d4e1959f6e34a1f26d00275759b9e4..e5e26daab66f4b1445ec7d53d181b1e43b57f208 100644 --- a/src/examples/opendrive_import_test.cpp +++ b/src/examples/opendrive_import_test.cpp @@ -8,13 +8,26 @@ const std::string opendrive_file="/home/sergi/iri-lab/add_robot/add_ws/code/auto int main(int argc, char *argv[]) { try{ - CRoadMap map; + CRoadMap map,new_map; Eigen::MatrixXi connectivity; - std::vector<unsigned int> id_map; + std::vector<unsigned int> id_map,new_path,old_path; + std::vector<double> x,y,heading; map.load_opendrive(opendrive_file); - map.get_segment_connectivity(connectivity,id_map); + map.get_lane_geometry(x,y,heading); + for(unsigned int i=0;i<x.size();i++) + std::cout << x[i] << "," << y[i] << "," << heading[i] << std::endl; + + return 0; + + old_path.push_back(3); + old_path.push_back(4); + old_path.push_back(22); + old_path.push_back(10); + new_path=map.get_path_sub_roadmap(old_path,new_map); + + new_map.get_segment_connectivity(connectivity,id_map); for(unsigned int i=0;i<id_map.size();i++) std::cout << i << " -> " << id_map[i] << " "; std::cout << std::endl;