diff --git a/OpenRoadEd/create_junction_road.cpp b/OpenRoadEd/create_junction_road.cpp
index 1194c60cc840ab5e8793cb28f9098a81241e7284..84f6454d573288744dc842b392c0d1734cc816d9 100644
--- a/OpenRoadEd/create_junction_road.cpp
+++ b/OpenRoadEd/create_junction_road.cpp
@@ -27,6 +27,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
std::stringstream name,id;
static unsigned int road_id=1000;
double line1,line2;
+ double check_x,check_y,check_heading;
// check if the junction is fully defined and the road exists
junction=open_drive->GetJunction(junction_index);
@@ -68,7 +69,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
{
if(predecessor->GetPredecessor()->GetElementType()=="junction" && predecessor->GetPredecessor()->GetElementId()==junction->GetId())
{
- predecessor->GetLastGeometryBlock()->GetCoords(0,start_x,start_y,original_start_heading);
+ 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;
@@ -80,6 +81,8 @@ 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;
start_heading=original_start_heading;
}
else
@@ -113,7 +116,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
}
// get lane offset
lane_start_offset=0.0;
- if(original_start_heading==start_heading)
+ if(fabs(original_start_heading-start_heading)<0.01)
{
for(unsigned int i=0;i<lanes->size();i++)
{
@@ -141,17 +144,16 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
}
}
}
- 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)
{
if(successor->GetPredecessor()->GetElementType()=="junction" && successor->GetPredecessor()->GetElementId()==junction->GetId())
{
- successor->GetLastGeometryBlock()->GetCoords(0,end_x,end_y,original_end_heading);
+ 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;
end_heading=original_end_heading;
}
else
@@ -186,8 +188,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;
- std::cout << original_end_heading << "," << end_heading << std::endl;
- if(original_end_heading==end_heading)
+ if(fabs(original_end_heading-end_heading)<0.01)
{
for(unsigned int i=0;i<lanes->size();i++)
{
@@ -215,47 +216,9 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
}
}
}
- 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)
- {
- // the road does not exist
- id << road_id;
- // Creates a new record in openDrive structure and returns it's index
- road_index=open_drive->AddRoad("", 0, "", "");
- // Add the road to the road tree
- road_tree->AddRoad(road_index,false);
- road=open_drive->GetLastRoad();
- road->SetRoadName(name.str());
- road->SetRoadId(id.str());
- road->SetRoadJunction(junction->GetId());
- if(predecessor->GetPredecessor()->GetElementType()=="junction")
- road->SetPredecessor("road", connection->GetIncomingRoad(), "start");
- else
- road->SetPredecessor("road", connection->GetIncomingRoad(), "end");
- if(successor->GetPredecessor()->GetElementType()=="junction")
- road->SetSuccessor("road",connection->GetConnectingRoad(), "start");
- else
- road->SetSuccessor("road",connection->GetConnectingRoad(), "end");
- road->AddRoadType(0,"town");// que sigui el tipus del predecessor
- road_tree->AddRoadType(road_index,0,false);
- road_id++;
- }
-
- if(road->GetGeometryBlockCount()!=0)
- {
- while(road->GetGeometryBlockCount()!=0)
- {
- road_tree->SelectGeometry(road_index,road->GetGeometryBlockCount()-1);
- road_tree->DeleteLastGeometry();
- road->DeleteGeometryBlock(road->GetGeometryBlockCount()-1);
- }
- road->SetRoadLength(0);
- }
// compute geometry
// LINE
if(fabs(start_heading-1.5707)<0.01 || fabs(start_heading-4.71238898)<0.01)
@@ -270,6 +233,42 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
}
if(fabs(line2-line1)<0.01 && fabs(start_heading-end_heading)<0.01)
{
+ if(road==NULL)
+ {
+ // the road does not exist
+ id << road_id;
+ // Creates a new record in openDrive structure and returns it's index
+ road_index=open_drive->AddRoad("", 0, "", "");
+ // Add the road to the road tree
+ road_tree->AddRoad(road_index,false);
+ road=open_drive->GetLastRoad();
+ road->SetRoadName(name.str());
+ road->SetRoadId(id.str());
+ road->SetRoadJunction(junction->GetId());
+ if(predecessor->GetPredecessor()->GetElementType()=="junction")
+ road->SetPredecessor("road", connection->GetIncomingRoad(), "start");
+ else
+ road->SetPredecessor("road", connection->GetIncomingRoad(), "end");
+ if(successor->GetPredecessor()->GetElementType()=="junction")
+ road->SetSuccessor("road",connection->GetConnectingRoad(), "start");
+ else
+ road->SetSuccessor("road",connection->GetConnectingRoad(), "end");
+ road->AddRoadType(0,"town");// que sigui el tipus del predecessor
+ road_tree->AddRoadType(road_index,0,false);
+ road_id++;
+ }
+
+ if(road->GetGeometryBlockCount()!=0)
+ {
+ while(road->GetGeometryBlockCount()!=0)
+ {
+ road_tree->SelectGeometry(road_index,road->GetGeometryBlockCount()-1);
+ road_tree->DeleteLastGeometry();
+ road->DeleteGeometryBlock(road->GetGeometryBlockCount()-1);
+ }
+ road->SetRoadLength(0);
+ }
+
s=0;
length=sqrt(pow(end_x-start_x,2.0)+pow(end_y-start_y,2.0));
new_geometry.AddGeometryLine(s,start_x,start_y,start_heading,length);
@@ -321,7 +320,74 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
start_length=0.0;
end_length=fabs(sol(1))-fabs(sol(0));
}
+ // check geometry
+ s=0;
+ check_x=start_x;
+ check_y=start_y;
+ check_heading=start_heading;
+ if(start_length>0)
+ {
+ new_geometry.AddGeometryLine(s,check_x,check_y,check_heading,start_length);
+ new_geometry.GetLastCoords(s,check_x,check_y,check_heading);
+ }
+ new_geometry.AddGeometrySpiral(s,check_x,check_y,check_heading,0.001,0.0,curvature);
+ new_geometry.GetLastCoords(s,check_x,check_y,check_heading);
+ new_geometry.AddGeometryArc(s,check_x,check_y,check_heading,length,curvature);
+ new_geometry.GetLastCoords(s,check_x,check_y,check_heading);
+ new_geometry.AddGeometrySpiral(s,check_x,check_y,check_heading,0.001,curvature,0.0);
+ new_geometry.GetLastCoords(s,check_x,check_y,check_heading);
+ if(end_length>0)
+ {
+ 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)
+ {
+ 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;
+ }
+ if(road==NULL)
+ {
+ // the road does not exist
+ id << road_id;
+ // Creates a new record in openDrive structure and returns it's index
+ road_index=open_drive->AddRoad("", 0, "", "");
+ // Add the road to the road tree
+ road_tree->AddRoad(road_index,false);
+ road=open_drive->GetLastRoad();
+ road->SetRoadName(name.str());
+ road->SetRoadId(id.str());
+ road->SetRoadJunction(junction->GetId());
+ if(predecessor->GetPredecessor()->GetElementType()=="junction")
+ road->SetPredecessor("road", connection->GetIncomingRoad(), "start");
+ else
+ road->SetPredecessor("road", connection->GetIncomingRoad(), "end");
+ if(successor->GetPredecessor()->GetElementType()=="junction")
+ road->SetSuccessor("road",connection->GetConnectingRoad(), "start");
+ else
+ road->SetSuccessor("road",connection->GetConnectingRoad(), "end");
+ road->AddRoadType(0,"town");// que sigui el tipus del predecessor
+ road_tree->AddRoadType(road_index,0,false);
+ road_id++;
+ }
+
+ if(road->GetGeometryBlockCount()!=0)
+ {
+ while(road->GetGeometryBlockCount()!=0)
+ {
+ road_tree->SelectGeometry(road_index,road->GetGeometryBlockCount()-1);
+ road_tree->DeleteLastGeometry();
+ road->DeleteGeometryBlock(road->GetGeometryBlockCount()-1);
+ }
+ road->SetRoadLength(0);
+ }
+
// add geometry
+ new_geometry=GeometryBlock();
s=0;
if(start_length>0)
{