Skip to content
Snippets Groups Projects
Commit e8125c69 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Solved a bug when creating connection roads: roads with multiple geometry...

Solved a bug when creating connection roads: roads with multiple geometry block were not properly handled.
Added a check that the connection road actually reaches the desired position to discard impossible roads.
parent 551917b5
No related branches found
No related tags found
No related merge requests found
...@@ -27,6 +27,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns ...@@ -27,6 +27,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
std::stringstream name,id; std::stringstream name,id;
static unsigned int road_id=1000; static unsigned int road_id=1000;
double line1,line2; double line1,line2;
double check_x,check_y,check_heading;
// check if the junction is fully defined and the road exists // check if the junction is fully defined and the road exists
junction=open_drive->GetJunction(junction_index); junction=open_drive->GetJunction(junction_index);
...@@ -68,7 +69,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns ...@@ -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()) 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; start_heading=original_start_heading+3.14158;
if(start_heading>2*3.14158) if(start_heading>2*3.14158)
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 ...@@ -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()) if(predecessor->GetSuccessor()->GetElementType()=="junction" && predecessor->GetSuccessor()->GetElementId()==junction->GetId())
{ {
predecessor->GetLastGeometryBlock()->GetLastCoords(s,start_x,start_y,original_start_heading); 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; start_heading=original_start_heading;
} }
else else
...@@ -113,7 +116,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns ...@@ -113,7 +116,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
} }
// get lane offset // get lane offset
lane_start_offset=0.0; 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++) 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 ...@@ -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_x-=lane_start_offset*sin(start_heading);
start_y+=lane_start_offset*cos(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 // get successor road start position
if(successor->GetPredecessor()!=NULL) if(successor->GetPredecessor()!=NULL)
{ {
if(successor->GetPredecessor()->GetElementType()=="junction" && successor->GetPredecessor()->GetElementId()==junction->GetId()) 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; end_heading=original_end_heading;
} }
else else
...@@ -186,8 +188,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns ...@@ -186,8 +188,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
// get lane offset // get lane offset
lanes=successor->GetLaneSection(0)->GetLaneVector(); lanes=successor->GetLaneSection(0)->GetLaneVector();
lane_end_offset=0.0; lane_end_offset=0.0;
std::cout << original_end_heading << "," << end_heading << std::endl; if(fabs(original_end_heading-end_heading)<0.01)
if(original_end_heading==end_heading)
{ {
for(unsigned int i=0;i<lanes->size();i++) 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 ...@@ -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_x-=lane_end_offset*sin(end_heading);
end_y+=lane_end_offset*cos(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 // compute geometry
// LINE // LINE
if(fabs(start_heading-1.5707)<0.01 || fabs(start_heading-4.71238898)<0.01) 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 ...@@ -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(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; s=0;
length=sqrt(pow(end_x-start_x,2.0)+pow(end_y-start_y,2.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); 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 ...@@ -321,7 +320,74 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
start_length=0.0; start_length=0.0;
end_length=fabs(sol(1))-fabs(sol(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 // add geometry
new_geometry=GeometryBlock();
s=0; s=0;
if(start_length>0) if(start_length>0)
{ {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment