Solved a bug when generating junction roads: vertical roads were not properly created.

Added support for roads with multiple lanes in each direction.

OpenRoadEd/create_junction_road.cpp

@@ -18,9 +18,9 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
   Eigen::Vector2d b,sol;
   double radius,curvature,length,start_length,end_length,angle;
   LaneSection *lane_section,new_lane_section(0.0);
-  double lane_start_width;
+  double lane_start_width,lane_start_offset;
   double s,start_x,start_y,start_heading,original_start_heading;
-  double end_x,end_y,end_heading,original_end_heading;
+  double end_x,end_y,end_heading,original_end_heading,lane_end_offset;
   unsigned int left_lane_index,right_lane_index,center_lane_index;
   unsigned int right_lane_width_index;
   LaneWidth *lane_width;
@@ -100,7 +100,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
     std::cout << "Warning: the predecessor (" << connection->GetIncomingRoad() << ") has invalid predecessor element" << std::endl;
     return -1;
   }
-  // get lane size and offset
+  // get lane size
   lanes=predecessor->GetLastLaneSection()->GetLaneVector();
   for(unsigned int i=0;i<lanes->size();i++)
   {
@@ -111,22 +111,41 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
         lane_start_width=lane->GetWidthValue(s);
     }
   }
+  // get lane offset
+  lane_start_offset=0.0;
   if(original_start_heading==start_heading)
   {
-    if(lane_link->GetFrom()>0)
+    for(unsigned int i=0;i<lanes->size();i++)
     {
-      start_x-=lane_start_width*sin(start_heading);
-      start_y+=lane_start_width*cos(start_heading);
+      lane=&lanes->at(i);
+      if(std::signbit(lane_link->GetFrom())==std::signbit(lane->GetId()))
+      {
+        if(lane->GetId()>0)//right lanes
+          lane_start_offset+=lane->GetWidthValue(s);
+	else if(abs(lane->GetId())<abs(lane_link->GetFrom()))
+          lane_start_offset-=lane->GetWidthValue(s);
+      }
     }
   }
   else
   {
-    if(lane_link->GetFrom()<0)
+    for(unsigned int i=0;i<lanes->size();i++)
     {
-      start_x-=lane_start_width*sin(start_heading);
-      start_y+=lane_start_width*cos(start_heading);
+      lane=&lanes->at(i);
+      if(std::signbit(lane_link->GetFrom())==std::signbit(lane->GetId()))
+      {
+        if(lane->GetId()<0)//right lanes
+          lane_start_offset+=lane->GetWidthValue(s);
+	else if(abs(lane->GetId())<abs(lane_link->GetFrom()))
+          lane_start_offset-=lane->GetWidthValue(s);
+      }
     }
   }
+  std::cout << "start offset: " << lane_start_offset << std::endl;
+  std::cout << "start position: " << start_x << "," << start_y << std::endl;
+  start_x-=lane_start_offset*sin(start_heading);
+  start_y+=lane_start_offset*cos(start_heading);
+  std::cout << "start position: " << start_x << "," << start_y << std::endl;
   // get successor road start position
   if(successor->GetPredecessor()!=NULL)
   {
@@ -164,23 +183,43 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
     std::cout << "Warning: the successor (" << connection->GetConnectingRoad() << ") has invalid predecessor element" << std::endl;
     return -1;
   }
-
-  if(end_heading==original_end_heading)
+  // get lane offset
+  lanes=successor->GetLaneSection(0)->GetLaneVector();
+  lane_end_offset=0.0;
+  std::cout << original_end_heading << "," << end_heading << std::endl;
+  if(original_end_heading==end_heading)
   {
-    if(lane_link->GetTo()>0)
+    for(unsigned int i=0;i<lanes->size();i++)
     {
-      end_x-=lane_start_width*sin(end_heading);
-      end_y+=lane_start_width*cos(end_heading);
+      lane=&lanes->at(i);
+      if(std::signbit(lane_link->GetTo())==std::signbit(lane->GetId()))
+      {
+        if(lane->GetId()>0)//right lanes
+          lane_end_offset+=lane->GetWidthValue(s);
+	else if(abs(lane->GetId())<abs(lane_link->GetTo()))
+          lane_end_offset-=lane->GetWidthValue(s);
+      }
     }
   }
   else
   {
-    if(lane_link->GetTo()<0)
+    for(unsigned int i=0;i<lanes->size();i++)
     {
-      end_x-=lane_start_width*sin(end_heading);
-      end_y+=lane_start_width*cos(end_heading);
+      lane=&lanes->at(i);
+      if(std::signbit(lane_link->GetTo())==std::signbit(lane->GetId()))
+      {
+        if(lane->GetId()<0)//right lanes
+          lane_end_offset+=lane->GetWidthValue(s);
+	else if(abs(lane->GetId())<abs(lane_link->GetTo()))
+          lane_end_offset-=lane->GetWidthValue(s);
+      }
     }
   }
+  std::cout << "end offset: " << lane_end_offset << std::endl;
+  std::cout << "end position: " << end_x << "," << end_y << std::endl;
+  end_x-=lane_end_offset*sin(end_heading);
+  end_y+=lane_end_offset*cos(end_heading);
+  std::cout << "end position: " << end_x << "," << end_y << std::endl;
 
   if(road==NULL)
   {
@@ -202,7 +241,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
       road->SetSuccessor("road",connection->GetConnectingRoad(), "start");
     else
       road->SetSuccessor("road",connection->GetConnectingRoad(), "end");
-    road->AddRoadType(0,"town");
+    road->AddRoadType(0,"town");// que sigui el tipus del predecessor
     road_tree->AddRoadType(road_index,0,false);
     road_id++;
   }
@@ -219,9 +258,17 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
   }
   // compute geometry
   // LINE
-  line1=(end_x-start_x)*tan(start_heading);
-  line2=(end_y-start_y);
-  if(fabs(line1-line2)<0.01 && fabs(start_heading-end_heading)<0.01)
+  if(fabs(start_heading-1.5707)<0.01 || fabs(start_heading-4.71238898)<0.01)
+  {
+    line1=start_x;
+    line2=end_x;
+  }
+  else
+  {
+    line1=start_y-tan(start_heading)*start_x;
+    line2=end_y-tan(start_heading)*end_x;
+  }
+  if(fabs(line2-line1)<0.01 && fabs(start_heading-end_heading)<0.01)
   {
     s=0;
     length=sqrt(pow(end_x-start_x,2.0)+pow(end_y-start_y,2.0));