diff --git a/OpenRoadEd/create_junction_road.cpp b/OpenRoadEd/create_junction_road.cpp
index 84f6454d573288744dc842b392c0d1734cc816d9..48e48fcc32aaf4d6531b3e19b3e1c1c985beecf5 100644
--- a/OpenRoadEd/create_junction_road.cpp
+++ b/OpenRoadEd/create_junction_road.cpp
@@ -71,8 +71,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
     {
       predecessor->GetGeometryBlock(0)->GetCoords(0,start_x,start_y,original_start_heading);
       start_heading=original_start_heading+3.14158;
-      if(start_heading>2*3.14158)
-        start_heading-=2*3.14158;
+      start_heading=normalize_angle(start_heading);
     }
     else 
     {
@@ -81,8 +80,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
         if(predecessor->GetSuccessor()->GetElementType()=="junction" && predecessor->GetSuccessor()->GetElementId()==junction->GetId())
         {
           predecessor->GetLastGeometryBlock()->GetLastCoords(s,start_x,start_y,original_start_heading);
-          if(original_start_heading>2*3.14158)
-            original_start_heading-=2*3.14158;
+          original_start_heading=normalize_angle(original_start_heading);
           start_heading=original_start_heading;
         }
         else
@@ -116,7 +114,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
   }
   // get lane offset
   lane_start_offset=0.0;
-  if(fabs(original_start_heading-start_heading)<0.01)
+  if(diff_angle(original_start_heading,start_heading)<0.01)
   {
     for(unsigned int i=0;i<lanes->size();i++)
     {
@@ -152,8 +150,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
     if(successor->GetPredecessor()->GetElementType()=="junction" && successor->GetPredecessor()->GetElementId()==junction->GetId())
     {
       successor->GetGeometryBlock(0)->GetCoords(0,end_x,end_y,original_end_heading);
-      if(original_end_heading>2*3.14158)
-        original_end_heading-=2*3.14158;
+      original_end_heading=normalize_angle(original_end_heading);
       end_heading=original_end_heading;
     }
     else 
@@ -164,8 +161,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
         {
           successor->GetLastGeometryBlock()->GetLastCoords(s,end_x,end_y,original_end_heading);
           end_heading=original_end_heading+3.14158;
-          if(end_heading>2*3.14158)
-            end_heading-=2*3.14158;
+	  end_heading=normalize_angle(end_heading);
         }
         else
         {
@@ -188,7 +184,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
   // get lane offset
   lanes=successor->GetLaneSection(0)->GetLaneVector();
   lane_end_offset=0.0;
-  if(fabs(original_end_heading-end_heading)<0.01)
+  if(diff_angle(original_end_heading,end_heading)<0.01)
   {
     for(unsigned int i=0;i<lanes->size();i++)
     {
@@ -221,7 +217,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
 
   // compute geometry
   // LINE
-  if(fabs(start_heading-1.5707)<0.01 || fabs(start_heading-4.71238898)<0.01)
+  if(diff_angle(start_heading,1.5707)<0.01 || diff_angle(start_heading,4.71238898)<0.01)
   {
     line1=start_x;
     line2=end_x;
@@ -231,7 +227,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
     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)
+  if(fabs(line2-line1)<0.01 && diff_angle(start_heading,end_heading)<0.01)
   {
     if(road==NULL)
     {
@@ -245,11 +241,11 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
       road->SetRoadName(name.str());
       road->SetRoadId(id.str());
       road->SetRoadJunction(junction->GetId());
-      if(predecessor->GetPredecessor()->GetElementType()=="junction")
+      if(predecessor->GetPredecessor()->GetElementType()=="junction" && predecessor->GetPredecessor()->GetElementId()==junction->GetId())
         road->SetPredecessor("road", connection->GetIncomingRoad(), "start");
       else
         road->SetPredecessor("road", connection->GetIncomingRoad(), "end");
-      if(successor->GetPredecessor()->GetElementType()=="junction")
+      if(successor->GetPredecessor()->GetElementType()=="junction" && successor->GetPredecessor()->GetElementId()==junction->GetId())
         road->SetSuccessor("road",connection->GetConnectingRoad(), "start");
       else
         road->SetSuccessor("road",connection->GetConnectingRoad(), "end");
@@ -341,11 +337,8 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
       new_geometry.AddGeometryLine(s,check_x,check_y,check_heading,end_length);
       new_geometry.GetLastCoords(s,check_x,check_y,check_heading);
     }
-    std::cout << end_heading << "," << check_heading << std::endl;
-    if(check_heading>2*3.14159)
-      check_heading-=2*3.14159;
-    std::cout << end_heading << "," << check_heading << std::endl;
-    if(fabs(end_x-check_x)>0.1 || fabs(end_y-check_y)>0.1 || fabs(end_heading-check_heading)>0.1)
+    check_heading=normalize_angle(check_heading);
+    if(fabs(end_x-check_x)>0.1 || fabs(end_y-check_y)>0.1 || diff_angle(end_heading,check_heading)>0.1)
     {
       std::cout << "Warning: Impossible to create connecting road from " << connection->GetIncomingRoad() << " lane " << lane_link->GetFrom() << " to " << connection->GetConnectingRoad() << " lane " << lane_link->GetTo() << std::endl;
       return -1;
@@ -362,11 +355,11 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
       road->SetRoadName(name.str());
       road->SetRoadId(id.str());
       road->SetRoadJunction(junction->GetId());
-      if(predecessor->GetPredecessor()->GetElementType()=="junction")
+      if(predecessor->GetPredecessor()->GetElementType()=="junction" && predecessor->GetPredecessor()->GetElementId()==junction->GetId())
         road->SetPredecessor("road", connection->GetIncomingRoad(), "start");
       else
         road->SetPredecessor("road", connection->GetIncomingRoad(), "end");
-      if(successor->GetPredecessor()->GetElementType()=="junction")
+      if(successor->GetPredecessor()->GetElementType()=="junction" && successor->GetPredecessor()->GetElementId()==junction->GetId())
         road->SetSuccessor("road",connection->GetConnectingRoad(), "start");
       else
         road->SetSuccessor("road",connection->GetConnectingRoad(), "end");
@@ -451,3 +444,32 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
 
   return road_index;
 }
+
+double normalize_angle(double angle)
+{
+  double norm_angle;
+
+  if(angle>=2*3.14159)
+    norm_angle=angle-2*3.14159;
+  else if(angle<0.0)
+    norm_angle=angle+2*3.14159;
+  else
+    norm_angle=angle;
+
+  return norm_angle;
+}
+
+double diff_angle(double angle1,double angle2)
+{
+  double diff1,diff2;
+
+  diff1=fabs(angle1-angle2);
+  if(angle1<angle2)
+    diff2=fabs(angle1+2*3.14159-angle2);
+  else
+    diff2=fabs(angle1-angle2-2*3.14159);
+
+  return std::min(diff1,diff2);
+}
+
+
diff --git a/OpenRoadEd/create_junction_road.h b/OpenRoadEd/create_junction_road.h
index 0e8393c6ecb23e67cb7097bf3c21bee9a443f59c..f163bbf8ddb8cbd860a0086bb7ff0717f3ae1223 100644
--- a/OpenRoadEd/create_junction_road.h
+++ b/OpenRoadEd/create_junction_road.h
@@ -6,5 +6,7 @@
 #include "RoadTree.h"
 
 unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,unsigned int junction_index,unsigned int connection_index,unsigned int lane_link_index);
+double normalize_angle(double angle);
+double diff_angle(double angle1,double angle2);
 
 #endif