diff --git a/LibCarla/source/carla/opendrive/OpenDrive.cpp b/LibCarla/source/carla/opendrive/OpenDrive.cpp index 2a240dc42..e6f349374 100644 --- a/LibCarla/source/carla/opendrive/OpenDrive.cpp +++ b/LibCarla/source/carla/opendrive/OpenDrive.cpp @@ -109,6 +109,8 @@ namespace opendrive { mapBuilder.SetGeoReference(open_drive_road.geoReference); + mapBuilder.SetTrafficGroupData(open_drive_road.trafficlightgroups); + // Generate road and junction information using junction_data_t = std::map>>; using road_data_t = std::map; @@ -139,15 +141,14 @@ namespace opendrive { RoadGeneralInfo->SetLanesOffset(s, a); } - for(auto &&elevation : it->second->road_profiles.elevation_profile) { + for (auto &&elevation : it->second->road_profiles.elevation_profile) { roadSegment.MakeInfo( - elevation.start_position, - elevation.start_position, - elevation.elevation, - elevation.slope, - elevation.vertical_curvature, - elevation.curvature_change - ); + elevation.start_position, + elevation.start_position, + elevation.elevation, + elevation.slope, + elevation.vertical_curvature, + elevation.curvature_change); } std::map leftLanesGoToSuccessor, leftLanesGoToPredecessor; diff --git a/LibCarla/source/carla/opendrive/parser/OpenDriveParser.h b/LibCarla/source/carla/opendrive/parser/OpenDriveParser.h index 9a97668e9..fb7e86bfc 100644 --- a/LibCarla/source/carla/opendrive/parser/OpenDriveParser.h +++ b/LibCarla/source/carla/opendrive/parser/OpenDriveParser.h @@ -14,6 +14,8 @@ #include "GeometryParser.h" #include "LaneParser.h" +#include "TrafficGroupParser.h" + #include "./pugixml/pugixml.hpp" #include @@ -86,6 +88,12 @@ struct OpenDriveParser { carla::opendrive::parser::JunctionParser::Parse(junction, out_open_drive_data.junctions); } + for (pugi::xml_node tlgroup = xmlDoc.child("OpenDRIVE").child("tlGroup"); + tlgroup; + tlgroup = tlgroup.next_sibling("tlGroup")) { + carla::opendrive::parser::TrafficGroupParser::Parse(tlgroup, out_open_drive_data.trafficlightgroups); + } + out_open_drive_data.geoReference = xmlDoc.child("OpenDRIVE").child("header").child_value("geoReference"); return true; diff --git a/LibCarla/source/carla/opendrive/parser/TrafficGroupParser.cpp b/LibCarla/source/carla/opendrive/parser/TrafficGroupParser.cpp new file mode 100644 index 000000000..b5b8860d4 --- /dev/null +++ b/LibCarla/source/carla/opendrive/parser/TrafficGroupParser.cpp @@ -0,0 +1,65 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "TrafficGroupParser.h" +#include + +void carla::opendrive::parser::TrafficGroupParser::Parse( + const pugi::xml_node &xmlNode, + std::vector &out_trafficLights) { + carla::opendrive::parser::TrafficGroupParser parser; + carla::opendrive::types::TrafficLightGroup traffic_light_group; + + traffic_light_group.red_time = std::atoi(xmlNode.attribute("redTime").value()); + traffic_light_group.yellow_time = std::atoi(xmlNode.attribute("yellowTime").value()); + traffic_light_group.green_time = std::atoi(xmlNode.attribute("greenTime").value()); + + parser.ParseTrafficLight(xmlNode, traffic_light_group.traffic_lights); + out_trafficLights.emplace_back(traffic_light_group); +} + +void carla::opendrive::parser::TrafficGroupParser::ParseTrafficLight( + const pugi::xml_node &xmlNode, + std::vector &out_trafficLight) { + + for (pugi::xml_node trafficlight = xmlNode.child("trafficlight"); + trafficlight; + trafficlight = trafficlight.next_sibling("trafficlight")) { + carla::opendrive::types::TrafficLight jTrafficlight; + + jTrafficlight.x_pos = std::stod(trafficlight.attribute("xPos").value()); + jTrafficlight.y_pos = std::stod(trafficlight.attribute("yPos").value()); + jTrafficlight.z_pos = std::stod(trafficlight.attribute("zPos").value()); + jTrafficlight.x_rot = std::stod(trafficlight.attribute("xRot").value()); + jTrafficlight.y_rot = std::stod(trafficlight.attribute("yRot").value()); + jTrafficlight.z_rot = std::stod(trafficlight.attribute("zRot").value()); + + ParseBoxAreas(trafficlight, jTrafficlight.box_areas); + + out_trafficLight.emplace_back(jTrafficlight); + } + +} + +void carla::opendrive::parser::TrafficGroupParser::ParseBoxAreas( + const pugi::xml_node &xmlNode, + std::vector &out_boxcomponent) { + for (pugi::xml_node boxcomponent = xmlNode.child("tfBox"); + boxcomponent; + boxcomponent = boxcomponent.next_sibling("tfBox")) { + carla::opendrive::types::BoxComponent jBoxComponent; + + jBoxComponent.x_pos = std::stod(boxcomponent.attribute("xPos").value()); + jBoxComponent.y_pos = std::stod(boxcomponent.attribute("yPos").value()); + jBoxComponent.z_pos = std::stod(boxcomponent.attribute("zPos").value()); + jBoxComponent.x_rot = std::stod(boxcomponent.attribute("xRot").value()); + jBoxComponent.y_rot = std::stod(boxcomponent.attribute("yRot").value()); + jBoxComponent.z_rot = std::stod(boxcomponent.attribute("zRot").value()); + + out_boxcomponent.emplace_back(jBoxComponent); + } + +} diff --git a/LibCarla/source/carla/opendrive/parser/TrafficGroupParser.h b/LibCarla/source/carla/opendrive/parser/TrafficGroupParser.h new file mode 100644 index 000000000..81bb9c31e --- /dev/null +++ b/LibCarla/source/carla/opendrive/parser/TrafficGroupParser.h @@ -0,0 +1,38 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "../types.h" + +#include "./pugixml/pugixml.hpp" + +namespace carla { +namespace opendrive { +namespace parser { + + class TrafficGroupParser { + private: + + void ParseTrafficLight( + const pugi::xml_node &xmlNode, + std::vector &out_trafficLights); + + void ParseBoxAreas( + const pugi::xml_node &xmlNode, + std::vector &out_boxareas); + + public: + + static void Parse( + const pugi::xml_node &xmlNode, + std::vector &out_trafficlightgroup); + + }; + +} +} +} diff --git a/LibCarla/source/carla/opendrive/types.h b/LibCarla/source/carla/opendrive/types.h index 4e99fcaf6..fa7a7c75c 100644 --- a/LibCarla/source/carla/opendrive/types.h +++ b/LibCarla/source/carla/opendrive/types.h @@ -236,12 +236,54 @@ namespace types { std::vector connections; }; + struct BoxComponent { + union { + struct { double x_pos, y_pos, z_pos; + }; + double pos[3]; + }; + union { + struct { double x_rot, y_rot, z_rot; + }; + double rot[3]; + }; + double scale; + BoxComponent() : pos{0.0, 0.0, 0.0}, + rot{0.0, 0.0, 0.0}, + scale(1.0) {} + }; + + struct TrafficLight { + union { + struct { double x_pos, y_pos, z_pos; + }; + double pos[3]; + }; + union { + struct { double x_rot, y_rot, z_rot; + }; + double rot[3]; + }; + double scale; + std::vector box_areas; + + TrafficLight() : pos{0.0, 0.0, 0.0}, + rot{0.0, 0.0, 0.0}, + scale(1.0) {} + }; + + struct TrafficLightGroup { + std::vector traffic_lights; + double red_time, yellow_time, green_time; + }; + ///////////////////////////////////////////////////////////////// struct OpenDriveData { std::string geoReference; std::vector roads; std::vector junctions; + std::vector trafficlightgroups; }; struct Waypoint { diff --git a/LibCarla/source/carla/road/MapBuilder.h b/LibCarla/source/carla/road/MapBuilder.h index b57d63df1..a14b42bcd 100644 --- a/LibCarla/source/carla/road/MapBuilder.h +++ b/LibCarla/source/carla/road/MapBuilder.h @@ -27,6 +27,10 @@ namespace road { _map_data.SetGeoReference(geoReference); } + void SetTrafficGroupData(const std::vector &trafficLightData) { + _map_data.SetTrafficLightData(trafficLightData); + } + SharedPtr Build(); private: diff --git a/LibCarla/source/carla/road/MapData.h b/LibCarla/source/carla/road/MapData.h index d2fb53baf..caad7e272 100644 --- a/LibCarla/source/carla/road/MapData.h +++ b/LibCarla/source/carla/road/MapData.h @@ -10,6 +10,7 @@ #include "carla/ListView.h" #include "carla/NonCopyable.h" #include "carla/road/element/RoadSegment.h" +#include "carla/opendrive/types.h" #include @@ -56,6 +57,10 @@ namespace road { return _geo_reference; } + const std::vector &GetTrafficGroups() const { + return _traffic_groups; + } + auto GetRoadSegments() const { using const_ref = const element::RoadSegment &; auto get = [](auto &pair) -> const_ref { return *pair.second; }; @@ -78,6 +83,10 @@ namespace road { _geo_reference = geoReference; } + void SetTrafficLightData(const std::vector &trafficLightData) { + _traffic_groups = trafficLightData; + } + std::string _geo_reference; std::vector _junction_information; @@ -85,6 +94,8 @@ namespace road { std::unordered_map< element::id_type, std::unique_ptr> _elements; + + std::vector _traffic_groups; }; } // namespace road diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp index 4b91c6a8d..ad9dc6941 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp @@ -38,6 +38,14 @@ AOpenDriveActor::AOpenDriveActor(const FObjectInitializer &ObjectInitializer) { PrimaryActorTick.bCanEverTick = false; + static ConstructorHelpers::FObjectFinder TrafficLightBP(TEXT( + "Blueprint'/Game/Carla/Static/TrafficSigns/Streetlights_01/BP_TrafficLightPole.BP_TrafficLightPole'")); + TrafficLightBlueprintClass = (UClass *) TrafficLightBP.Object->GeneratedClass; + + static ConstructorHelpers::FObjectFinder TrafficGroupBP(TEXT( + "Blueprint'/Game/Carla/Static/TrafficSigns/Streetlights_01/BP_TrafficLightPoleGroup.BP_TrafficLightPoleGroup'")); + TrafficGroupBlueprintClass = (UClass *) TrafficGroupBP.Object->GeneratedClass; + // Structure to hold one-time initialization static struct FConstructorStatics { @@ -144,11 +152,17 @@ void AOpenDriveActor::BuildRoutes() BuildRoutes(GetWorld()->GetMapName()); } -void AOpenDriveActor::BuildRoutes(FString MapName) { +void AOpenDriveActor::BuildRoutes(FString MapName) +{ + GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Red, FString::Printf(TEXT("hi"))); + using CarlaMath = carla::geom::Math; using IdType = carla::road::element::id_type; using Waypoint = carla::road::element::Waypoint; using WaypointGen = carla::road::WaypointGenerator; + using TrafficGroup = carla::opendrive::types::TrafficLightGroup; + using TrafficLight = carla::opendrive::types::TrafficLight; + using TrafficBoxComponent = carla::opendrive::types::BoxComponent; std::string ParseError; @@ -195,7 +209,8 @@ void AOpenDriveActor::BuildRoutes(FString MapName) { // Create an identifier of the current lane const auto Identifier = std::make_pair(RoadId, LaneId); - // If Identifier does not exist in AlreadyVisited we haven't visited the lane + // If Identifier does not exist in AlreadyVisited we haven't visited the + // lane if (!std::any_of(AlreadyVisited.begin(), AlreadyVisited.end(), [&Identifier](auto i) { return (i.first == Identifier.first && i.second == Identifier.second); })) @@ -244,6 +259,55 @@ void AOpenDriveActor::BuildRoutes(FString MapName) { } } } + + const std::vector TrafficLightGroup = map.GetTrafficGroups(); + for (TrafficGroup CurrentTrafficLightGroup : TrafficLightGroup) + { + double RedTime = CurrentTrafficLightGroup.red_time; + double YellowTime = CurrentTrafficLightGroup.yellow_time; + double GreenTime = CurrentTrafficLightGroup.green_time; + FActorSpawnParameters SpawnParams; + FOutputDeviceNull ar; + AActor *SpawnedTrafficGroup = GetWorld()->SpawnActor(TrafficGroupBlueprintClass, + FVector(0, 0, 0), + FRotator(0, 0, 0), + SpawnParams); + SpawnedTrafficGroup->CallFunctionByNameWithArguments(TEXT("CallFunctionTest"), ar, NULL, true); + + for (TrafficLight CurrentTrafficLight : CurrentTrafficLightGroup.traffic_lights) + { + FVector TLPos = + FVector(CurrentTrafficLight.x_pos, CurrentTrafficLight.y_pos, CurrentTrafficLight.z_pos); + FRotator TLRot = FRotator(CurrentTrafficLight.x_rot, + CurrentTrafficLight.z_rot, + CurrentTrafficLight.y_rot); + AActor *SpawnedTrafficLight = GetWorld()->SpawnActor(TrafficLightBlueprintClass, + TLPos, + TLRot, + SpawnParams); + + SpawnedTrafficLight->CallFunctionByNameWithArguments(TEXT("InitData"), ar, NULL, true); + for (TrafficBoxComponent TfBoxComponent : CurrentTrafficLight.box_areas) + { + FVector TLBoxPos = FVector(TfBoxComponent.x_pos, + TfBoxComponent.y_pos, + TfBoxComponent.z_pos); + FRotator TLBoxRot = FRotator(TfBoxComponent.x_rot, + TfBoxComponent.z_rot, + TfBoxComponent.y_rot); + + FString BoxCommand = FString::Printf(TEXT("SetBoxLocationAndRotation %f %f %f %f %f %f"), + TLBoxPos.X, + TLBoxPos.Y, + TLBoxPos.Z, + TLBoxRot.Pitch, + TLBoxRot.Roll, + TLBoxRot.Yaw); + SpawnedTrafficLight->CallFunctionByNameWithArguments(*BoxCommand, ar, NULL, true); + } + } + } + } void AOpenDriveActor::RemoveRoutes() diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.h index a5a228df4..0df981378 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.h @@ -16,6 +16,8 @@ #include "Vehicle/VehicleSpawnPoint.h" +#include "Runtime/Core/Public/Misc/OutputDeviceNull.h" + #include #include #include @@ -43,6 +45,12 @@ private: UPROPERTY() TArray VehicleSpawners; + UPROPERTY() + TSubclassOf TrafficLightBlueprintClass; + + UPROPERTY() + TSubclassOf TrafficGroupBlueprintClass; + #if WITH_EDITORONLY_DATA /// Generate the road network using an OpenDrive file (named as the current /// .umap)