From 0c18d7e841bcf2fabd1720aac154a95a9c33ad1b Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 22 Aug 2023 16:41:27 +0200
Subject: [PATCH] Updated the examples.

---
 src/examples/build_from_scratch_test.cpp |  7 +++++++
 src/examples/opendrive_import_test.cpp   | 19 ++++++++++++++++---
 2 files changed, 23 insertions(+), 3 deletions(-)

diff --git a/src/examples/build_from_scratch_test.cpp b/src/examples/build_from_scratch_test.cpp
index 9671899..f19e9ac 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 294da6c..e5e26da 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;
-- 
GitLab