diff --git a/OpenRoadEd/create_junction_road.cpp b/OpenRoadEd/create_junction_road.cpp index 6fe182bb0dcbd04f638faaf7ce6463b50019c699..f0cfa2f0c8b5bb8fcc16bce7f79519157dc870a2 100644 --- a/OpenRoadEd/create_junction_road.cpp +++ b/OpenRoadEd/create_junction_road.cpp @@ -22,12 +22,13 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns double s,start_x,start_y,start_heading,original_start_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; + unsigned int right_lane_width_index,right_lane_speed_index; LaneWidth *lane_width; std::stringstream name,id; static unsigned int road_id=1000; double line1,line2; double check_x,check_y,check_heading; + double max_speed; // check if the junction is fully defined and the road exists junction=open_drive->GetJunction(junction_index); @@ -101,7 +102,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 + // get lane size and speed lanes=predecessor->GetLastLaneSection()->GetLaneVector(); for(unsigned int i=0;i<lanes->size();i++) { @@ -109,7 +110,10 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns if(std::signbit(lane_link->GetFrom())==std::signbit(lane->GetId())) { if(abs(lane->GetId())==abs(lane_link->GetFrom())) + { lane_start_width=lane->GetWidthValue(s); + max_speed=lane->GetLaneSpeed(0)->GetMax(); + } } } // get lane offset @@ -123,7 +127,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns { if(lane->GetId()>0)//right lanes lane_start_offset+=lane->GetWidthValue(s); - else if(abs(lane->GetId())<abs(lane_link->GetFrom())) + else if(abs(lane->GetId())<abs(lane_link->GetFrom())) lane_start_offset-=lane->GetWidthValue(s); } } @@ -137,7 +141,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns { if(lane->GetId()<0)//right lanes lane_start_offset+=lane->GetWidthValue(s); - else if(abs(lane->GetId())<abs(lane_link->GetFrom())) + else if(abs(lane->GetId())<abs(lane_link->GetFrom())) lane_start_offset-=lane->GetWidthValue(s); } } @@ -161,7 +165,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; - end_heading=normalize_angle(end_heading); + end_heading=normalize_angle(end_heading); } else { @@ -193,7 +197,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns { if(lane->GetId()>0)//right lanes lane_end_offset+=lane->GetWidthValue(s); - else if(abs(lane->GetId())<abs(lane_link->GetTo())) + else if(abs(lane->GetId())<abs(lane_link->GetTo())) lane_end_offset-=lane->GetWidthValue(s); } } @@ -207,7 +211,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns { if(lane->GetId()<0)//right lanes lane_end_offset+=lane->GetWidthValue(s); - else if(abs(lane->GetId())<abs(lane_link->GetTo())) + else if(abs(lane->GetId())<abs(lane_link->GetTo())) lane_end_offset-=lane->GetWidthValue(s); } } @@ -249,7 +253,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");// que sigui el tipus del predecessor + road->AddRoadType(0,predecessor->GetRoadType(0)->GetType()); road_tree->AddRoadType(road_index,0,false); road_id++; } @@ -363,7 +367,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");// que sigui el tipus del predecessor + road->AddRoadType(0,predecessor->GetRoadType(0)->GetType()); road_tree->AddRoadType(road_index,0,false); road_id++; } @@ -420,10 +424,10 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns // add lane section // RIGHT center_lane_index=new_lane_section.AddLane(0,0,"driving", false); - //left_lane_index=new_lane_section.AddLane(1,1,"driving", false); right_lane_index=new_lane_section.AddLane(-1,-1,"driving", false); lane=new_lane_section.GetLastAddedLane(); right_lane_width_index=lane->AddWidthRecord(0.0,lane_start_width,0.0,0.0,0.0); + right_lane_speed_index=lane->AddSpeedRecord(0.0,max_speed); if(road->GetLaneSectionCount()==0) { road->AddLaneSection(0.0); @@ -431,9 +435,9 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns (*lane_section)=new_lane_section; road_tree->AddLaneSection(road_index,0, false); road_tree->AddLaneCenter(road_index,0,center_lane_index, false); -// road_tree->AddLaneLeft(road_index,0,left_lane_index, false); road_tree->AddLaneRight(road_index,0,right_lane_index, false); road_tree->AddLaneWidth(road_index,0,right_lane_index,right_lane_width_index, false); + road_tree->AddLaneSpeed(road_index,0,right_lane_index,right_lane_speed_index, false); } else {