Updated the examples.

src/examples/build_from_scratch_test.cpp

@@ -14,7 +14,7 @@ int main(int argc, char *argv[])
     std::vector<double> x,y,heading;
  
     road0=new CRoad();
-    road0->set_num_lanes(1);
+    road0->set_num_lanes(2);
     road0->set_lane_width(4.0);
     road0->set_start_point(20.0,60.0,3.14159,0.0);
     road0->add_segment(0.0,60.0,3.14159,0.0);
@@ -23,7 +23,7 @@ int main(int argc, char *argv[])
     road0->add_segment(0.0,0.0,0.0,0.0);
     road0->add_segment(20.0,0.0,0.0,0.0);
     road1=new CRoad();
-    road1->set_num_lanes(1);
+    road1->set_num_lanes(2);
     road1->set_lane_width(4.0);
     road1->set_start_point(20.0,0.0,3.14159,0.0);
     road1->add_segment(0.0,0.0,3.14159,0.0);
@@ -32,17 +32,17 @@ int main(int argc, char *argv[])
     road1->add_segment(0.0,60.0,0.0,0.0);
     road1->add_segment(20.0,60.0,0.0,0.0);
     road2=new CRoad();
-    road2->set_num_lanes(1);
+    road2->set_num_lanes(2);
     road2->set_lane_width(4.0);
     road2->set_start_point(30.0,50.0,4.71238898,0.0);
     road2->add_segment(30.0,10.0,4.71238898,0.0);
     road3=new CRoad();
-    road3->set_num_lanes(1);
+    road3->set_num_lanes(2);
     road3->set_lane_width(4.0);
     road3->set_start_point(30.0,10.0,1.5707,0.0);
     road3->add_segment(30.0,50.0,1.5707,0.0);
     road4=new CRoad();
-    road4->set_num_lanes(1);
+    road4->set_num_lanes(2);
     road4->set_lane_width(4.0);
     road4->set_start_point(40.0,60.0,0.0,0.0);
     road4->add_segment(60.0,60.0,0.0,0.0);
@@ -51,7 +51,7 @@ int main(int argc, char *argv[])
     road4->add_segment(60.0,0.0,3.14159,0.0);
     road4->add_segment(40.0,0.0,3.14159,0.0);
     road5=new CRoad();
-    road5->set_num_lanes(1);
+    road5->set_num_lanes(2);
     road5->set_lane_width(4.0);
     road5->set_start_point(40.0,0.0,0.0,0.0);
     road5->add_segment(60.0,0.0,0.0,0.0);
@@ -70,6 +70,10 @@ int main(int argc, char *argv[])
     map.add_road(road4);
     map.add_road(road5);
 
+    map.set_opposite_direction_roads_by_id(road0->get_id(),road1->get_id());
+    map.set_opposite_direction_roads_by_id(road2->get_id(),road3->get_id());
+    map.set_opposite_direction_roads_by_id(road4->get_id(),road5->get_id());
+
     map.add_junction(junction0);
     map.add_junction(junction1);
 
@@ -80,17 +84,17 @@ int main(int argc, char *argv[])
     junction0->add_outgoing_road(road2);
     junction0->add_outgoing_road(road4);
     junction0->link_roads_by_id(road1->get_id(),road2->get_id());
-    junction0->link_lanes_by_id(road1->get_id(),0,road2->get_id(),0);
+    junction0->link_same_lanes_by_id(road1->get_id(),road2->get_id());
     junction0->link_roads_by_id(road1->get_id(),road4->get_id());
-    junction0->link_lanes_by_id(road1->get_id(),0,road4->get_id(),0);
+    junction0->link_same_lanes_by_id(road1->get_id(),road4->get_id());
     junction0->link_roads_by_id(road3->get_id(),road0->get_id());
-    junction0->link_lanes_by_id(road3->get_id(),0,road0->get_id(),0);
+    junction0->link_same_lanes_by_id(road3->get_id(),road0->get_id());
     junction0->link_roads_by_id(road3->get_id(),road4->get_id());
-    junction0->link_lanes_by_id(road3->get_id(),0,road4->get_id(),0);
+    junction0->link_same_lanes_by_id(road3->get_id(),road4->get_id());
     junction0->link_roads_by_id(road5->get_id(),road0->get_id());
-    junction0->link_lanes_by_id(road5->get_id(),0,road0->get_id(),0);
+    junction0->link_same_lanes_by_id(road5->get_id(),road0->get_id());
     junction0->link_roads_by_id(road5->get_id(),road2->get_id());
-    junction0->link_lanes_by_id(road5->get_id(),0,road2->get_id(),0);
+    junction0->link_same_lanes_by_id(road5->get_id(),road2->get_id());
 
     junction1->add_incomming_road(road0);
     junction1->add_incomming_road(road2);
@@ -99,18 +103,20 @@ int main(int argc, char *argv[])
     junction1->add_outgoing_road(road3);
     junction1->add_outgoing_road(road5);
     junction1->link_roads_by_id(road0->get_id(),road3->get_id());
-    junction1->link_lanes_by_id(road0->get_id(),0,road3->get_id(),0);
+    junction1->link_same_lanes_by_id(road0->get_id(),road3->get_id());
     junction1->link_roads_by_id(road0->get_id(),road5->get_id());
-    junction1->link_lanes_by_id(road0->get_id(),0,road5->get_id(),0);
+    junction1->link_same_lanes_by_id(road0->get_id(),road5->get_id());
     junction1->link_roads_by_id(road2->get_id(),road1->get_id());
-    junction1->link_lanes_by_id(road2->get_id(),0,road1->get_id(),0);
+    junction1->link_same_lanes_by_id(road2->get_id(),road1->get_id());
     junction1->link_roads_by_id(road2->get_id(),road5->get_id());
-    junction1->link_lanes_by_id(road2->get_id(),0,road5->get_id(),0);
+    junction1->link_same_lanes_by_id(road2->get_id(),road5->get_id());
     junction1->link_roads_by_id(road4->get_id(),road3->get_id());
-    junction1->link_lanes_by_id(road4->get_id(),0,road3->get_id(),0);
+    junction1->link_same_lanes_by_id(road4->get_id(),road3->get_id());
     junction1->link_roads_by_id(road4->get_id(),road1->get_id());
-    junction1->link_lanes_by_id(road4->get_id(),0,road1->get_id(),0);
+    junction1->link_same_lanes_by_id(road4->get_id(),road1->get_id());
 
+    map.save_opendrive("test_road.xodr");
+  
     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;

src/examples/opendrive_import_test.cpp

@@ -3,7 +3,8 @@
 
 #include <iostream>
 
-const std::string opendrive_file="/home/sergi/iri-lab/add_robot/add_ws/code/autonomous_driving_tools/src/xml/adc_road.xodr";
+//const std::string opendrive_file="/home/sergi/iri-lab/add_robot/add_ws/code/autonomous_driving_tools/src/xml/adc_road.xodr";
+const std::string opendrive_file="/home/sergi/iri-lab/add_robot/add_ws/code/autonomous_driving_tools/build/test_road.xodr";
 
 int main(int argc, char *argv[])
 {
@@ -15,16 +16,17 @@ int main(int argc, char *argv[])
  
     map.load_opendrive(opendrive_file);
 
+
     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(3);
+//    old_path.push_back(4);
     old_path.push_back(22);
-    old_path.push_back(10);
+//    old_path.push_back(10);
     new_path=map.get_path_sub_roadmap(old_path,new_map);
 
     new_map.get_segment_connectivity(connectivity,id_map);