Import now work with custom traffic lights and groups

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<int, std::map<int, std::vector<carla::road::lane_junction_t>>>;
     using road_data_t = std::map<int, carla::opendrive::types::RoadInformation *>;
@@ -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<carla::road::element::RoadElevationInfo>(
-          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<int, int> leftLanesGoToSuccessor, leftLanesGoToPredecessor;

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 <iostream>
@@ -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;

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 <https://opensource.org/licenses/MIT>.
+
+#include "TrafficGroupParser.h"
+#include <iostream>
+
+void carla::opendrive::parser::TrafficGroupParser::Parse(
+    const pugi::xml_node &xmlNode,
+    std::vector<carla::opendrive::types::TrafficLightGroup> &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<carla::opendrive::types::TrafficLight> &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<carla::opendrive::types::BoxComponent> &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);
+  }
+
+}

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 <https://opensource.org/licenses/MIT>.
+
+#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<carla::opendrive::types::TrafficLight> &out_trafficLights);
+
+    void ParseBoxAreas(
+        const pugi::xml_node &xmlNode,
+        std::vector<carla::opendrive::types::BoxComponent> &out_boxareas);
+
+  public:
+
+    static void Parse(
+        const pugi::xml_node &xmlNode,
+        std::vector<carla::opendrive::types::TrafficLightGroup> &out_trafficlightgroup);
+
+  };
+
+}
+}
+}

LibCarla/source/carla/opendrive/types.h

@@ -236,12 +236,54 @@ namespace types {
     std::vector<JunctionConnection> 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<BoxComponent> box_areas;
+
+    TrafficLight() : pos{0.0, 0.0, 0.0},
+                     rot{0.0, 0.0, 0.0},
+                     scale(1.0) {}
+  };
+
+  struct TrafficLightGroup {
+    std::vector<TrafficLight> traffic_lights;
+    double red_time, yellow_time, green_time;
+  };
+
   /////////////////////////////////////////////////////////////////
 
   struct OpenDriveData {
     std::string geoReference;
     std::vector<RoadInformation> roads;
     std::vector<Junction> junctions;
+    std::vector<TrafficLightGroup> trafficlightgroups;
   };
 
   struct Waypoint {

LibCarla/source/carla/road/MapBuilder.h

@@ -27,6 +27,10 @@ namespace road {
       _map_data.SetGeoReference(geoReference);
     }
 
+    void SetTrafficGroupData(const std::vector<opendrive::types::TrafficLightGroup> &trafficLightData) {
+      _map_data.SetTrafficLightData(trafficLightData);
+    }
+
     SharedPtr<Map> Build();
 
   private:

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 <boost/iterator/transform_iterator.hpp>
 
@@ -56,6 +57,10 @@ namespace road {
       return _geo_reference;
     }
 
+    const std::vector<opendrive::types::TrafficLightGroup> &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<opendrive::types::TrafficLightGroup> &trafficLightData) {
+      _traffic_groups = trafficLightData;
+    }
+
     std::string _geo_reference;
 
     std::vector<lane_junction_t> _junction_information;
@@ -85,6 +94,8 @@ namespace road {
     std::unordered_map<
         element::id_type,
         std::unique_ptr<element::RoadSegment>> _elements;
+
+    std::vector<opendrive::types::TrafficLightGroup> _traffic_groups;
   };
 
 } // namespace road

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp

@@ -38,6 +38,14 @@ AOpenDriveActor::AOpenDriveActor(const FObjectInitializer &ObjectInitializer)
 {
   PrimaryActorTick.bCanEverTick = false;
 
+  static ConstructorHelpers::FObjectFinder<UBlueprint> TrafficLightBP(TEXT(
+      "Blueprint'/Game/Carla/Static/TrafficSigns/Streetlights_01/BP_TrafficLightPole.BP_TrafficLightPole'"));
+  TrafficLightBlueprintClass = (UClass *) TrafficLightBP.Object->GeneratedClass;
+
+  static ConstructorHelpers::FObjectFinder<UBlueprint> 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<TrafficGroup> 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<AActor>(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<AActor>(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()

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.h

@@ -16,6 +16,8 @@
 
 #include "Vehicle/VehicleSpawnPoint.h"
 
+#include "Runtime/Core/Public/Misc/OutputDeviceNull.h"
+
 #include <compiler/disable-ue4-macros.h>
 #include <carla/opendrive/OpenDrive.h>
 #include <compiler/enable-ue4-macros.h>
@@ -43,6 +45,12 @@ private:
   UPROPERTY()
   TArray<AVehicleSpawnPoint *> VehicleSpawners;
 
+  UPROPERTY()
+  TSubclassOf<class AActor> TrafficLightBlueprintClass;
+
+  UPROPERTY()
+  TSubclassOf<class AActor> TrafficGroupBlueprintClass;
+
 #if WITH_EDITORONLY_DATA
   /// Generate the road network using an OpenDrive file (named as the current
   /// .umap)