Added automatic signal match with OpenDRIVE.

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp

@@ -19,6 +19,9 @@
 
 #include "Carla/Traffic/TrafficLightManager.h"
 
+namespace cr = carla::road;
+namespace cre = carla::road::element;
+
 ACarlaGameModeBase::ACarlaGameModeBase(const FObjectInitializer& ObjectInitializer)
   : Super(ObjectInitializer)
 {
@@ -204,6 +207,7 @@ void ACarlaGameModeBase::ParseOpenDrive(const FString &MapName)
 
 void ACarlaGameModeBase::DebugShowSignals(bool enable)
 {
+
   auto World = GetWorld();
   check(World != nullptr);
 
@@ -238,77 +242,79 @@ void ACarlaGameModeBase::DebugShowSignals(bool enable)
       true
     );
 
-    FString Text = FString(ODSignal->GetSignalId().c_str());
-    Text += FString(" - ");
+    FString Text;
+    Text += "Id: ";
+    Text += FString(ODSignal->GetSignalId().c_str());
+    Text += FString(" - Name: ");
     Text += FString(ODSignal->GetName().c_str());
 
-    UKismetSystemLibrary::DrawDebugString (
-      World,
-      Location + Up * 250.0f,
-      Text,
-      nullptr,
-      FLinearColor(0, 255, 0, 255)
-    );
-
-    FString Text2 = FString(ODSignal->GetType().c_str());
-    Text2 += FString(" - ");
-    Text2 += FString(ODSignal->GetSubtype().c_str());
-
-    UKismetSystemLibrary::DrawDebugString (
-      World,
-      Location + Up * 175.0f,
-      Text,
-      nullptr,
-      FLinearColor(0, 255, 0, 255),
-      10000.0f
-    );
+    // UKismetSystemLibrary::DrawDebugString (
+    //   World,
+    //   Location + Up * 250.0f,
+    //   Text,
+    //   nullptr,
+    //   FLinearColor(0, 255, 0, 255),
+    //   100000000.0
+    // );
   }
 
+  TArray<const cre::RoadInfoSignal*> References;
   auto waypoints = Map->GenerateWaypointsOnRoadEntries();
+  std::unordered_set<cr::RoadId> ExploredRoads;
   for (auto & waypoint : waypoints)
   {
-    auto SignalReferences = Map->GetLane(waypoint).GetRoad()->GetInfos<carla::road::element::RoadInfoSignal>();
+    // Check if we already explored this road
+    if (ExploredRoads.count(waypoint.road_id) > 0)
+    {
+      continue;
+    }
+    ExploredRoads.insert(waypoint.road_id);
+
+    // Multiple times for same road (performance impact, not in behavior)
+    auto SignalReferences = Map->GetLane(waypoint).
+        GetRoad()->GetInfos<cre::RoadInfoSignal>();
     for (auto *SignalReference : SignalReferences)
     {
-      double current_s = waypoint.s;
-      double signal_s = SignalReference->GetS();
+      References.Add(SignalReference);
+    }
+  }
+  for (auto& Reference : References)
+  {
+    auto RoadId = Reference->GetRoadId();
+    const auto* SignalReference = Reference;
+    const FTransform SignalTransform = SignalReference->GetSignal()->GetTransform();
+    for(auto &validity : SignalReference->GetValidities())
+    {
+      for(auto lane : carla::geom::Math::GenerateRange(validity._from_lane, validity._to_lane))
+      {
+        if(lane == 0)
+          continue;
 
-      double delta_s = signal_s - current_s;
-      FTransform ReferenceTransform;
-      if (delta_s == 0)
-      {
-        ReferenceTransform = Map->ComputeTransform(waypoint);
-      }
-      else if (waypoint.lane_id < 0)
-      {
-        auto signal_waypoint = Map->GetNext(waypoint, FMath::Abs(delta_s)).front();
-        ReferenceTransform = Map->ComputeTransform(signal_waypoint);
-      }
-      else if(waypoint.lane_id > 0)
-      {
-        auto signal_waypoint = Map->GetNext(waypoint, FMath::Abs(delta_s)).front();
-        ReferenceTransform = Map->ComputeTransform(signal_waypoint);
-      }
-      else
-      {
-        continue;
-      }
-      DrawDebugSphere(
-          World,
-          ReferenceTransform.GetLocation(),
-          50.0f,
-          10,
-          FColor(0, 255, 0),
-          true
-      );
+        auto signal_waypoint = Map->GetWaypoint(
+            RoadId, lane, SignalReference->GetS()).get();
 
-      DrawDebugLine(
-          World,
-          ReferenceTransform.GetLocation(),
-          FTransform(SignalReference->GetSignal()->GetTransform()).GetLocation(),
-          FColor(0, 255, 0),
-          true
-      );
+        if(Map->GetLane(signal_waypoint).GetType() != cr::Lane::LaneType::Driving)
+          continue;
+
+        FTransform ReferenceTransform = Map->ComputeTransform(signal_waypoint);
+
+        DrawDebugSphere(
+            World,
+            ReferenceTransform.GetLocation(),
+            50.0f,
+            10,
+            FColor(0, 0, 255),
+            true
+        );
+
+        DrawDebugLine(
+            World,
+            ReferenceTransform.GetLocation(),
+            SignalTransform.GetLocation(),
+            FColor(0, 0, 255),
+            true
+        );
+      }
     }
   }
 

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.h

@@ -44,7 +44,7 @@ public:
     return Map;
   }
 
-  UFUNCTION(Exec, CallInEditor, meta=(DevelopmentOnly))
+  UFUNCTION(Exec, Category = "CARLA Game Mode")
   void DebugShowSignals(bool enable);
 
 protected:

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightManager.cpp

@@ -169,6 +169,75 @@ void ATrafficLightManager::RemoveGeneratedSignalsAndTrafficLights()
   TrafficLightsGenerated = false;
 }
 
+void ATrafficLightManager::MatchTrafficLightActorsWithOpenDriveSignals()
+{
+  TArray<AActor*> Actors;
+  UGameplayStatics::GetAllActorsOfClass(GetWorld(), ATrafficLightBase::StaticClass(), Actors);
+
+  FString MapName = GetWorld()->GetName();
+  std::string opendrive_xml = carla::rpc::FromFString(UOpenDrive::LoadXODR(MapName));
+  auto Map = carla::opendrive::OpenDriveParser::Load(opendrive_xml);
+
+  if (!Map)
+  {
+    carla::log_warning("Map not found");
+    return;
+  }
+
+  TArray<ATrafficLightBase*> TrafficLights;
+  for (AActor* Actor : Actors)
+  {
+    ATrafficLightBase* TrafficLight = Cast<ATrafficLightBase>(Actor);
+    if (TrafficLight)
+    {
+      TrafficLight->GetTrafficLightComponent()->SetSignId("");
+      TrafficLights.Add(TrafficLight);
+    }
+  }
+
+  if (!TrafficLights.Num())
+  {
+    carla::log_warning("No actors in the map");
+    return;
+  }
+
+  const auto& Signals = Map->GetSignals();
+  const auto& Controllers = Map->GetControllers();
+
+  for(const auto& Signal : Signals) {
+    const auto& ODSignal = Signal.second;
+    const FTransform Transform = ODSignal->GetTransform();
+    const FVector Location = Transform.GetLocation();
+    if (ODSignal->GetName() == "")
+    {
+      continue;
+    }
+    ATrafficLightBase* ClosestActor = TrafficLights.Top();
+    float MinDistance = FVector::DistSquaredXY(TrafficLights.Top()->GetActorLocation(), Location);
+    for (ATrafficLightBase* Actor : TrafficLights)
+    {
+      float Distance = FVector::DistSquaredXY(Actor->GetActorLocation(), Location);
+      if (Distance < MinDistance)
+      {
+        MinDistance = Distance;
+        ClosestActor = Actor;
+      }
+    }
+    ATrafficLightBase* TrafficLight = ClosestActor;
+    auto* Component = TrafficLight->GetTrafficLightComponent();
+    if (Component->GetSignId() == "")
+    {
+      Component->SetSignId(carla::rpc::ToFString(ODSignal->GetSignalId()));
+    }
+    else
+    {
+      carla::log_warning("Could not find a suitable traffic light for signal", ODSignal->GetSignalId(),
+          "Closest traffic light has id", carla::rpc::FromFString(Component->GetSignId()));
+    }
+
+  }
+}
+
 // Called when the game starts
 void ATrafficLightManager::BeginPlay()
 {
@@ -180,11 +249,7 @@ void ATrafficLightManager::BeginPlay()
     return;
   }
 
-  if (TrafficLightsGenerated)
-  {
-    ResetTrafficLightObjects();
-  }
-  else
+  if (!TrafficLightsGenerated)
   {
     GenerateSignalsAndTrafficLights();
   }

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightManager.h

@@ -44,6 +44,9 @@ public:
   UFUNCTION(CallInEditor)
   void RemoveGeneratedSignalsAndTrafficLights();
 
+  UFUNCTION(CallInEditor)
+  void MatchTrafficLightActorsWithOpenDriveSignals();
+
 protected:
   // Called when the game starts or when spawned
   virtual void BeginPlay() override;
@@ -92,9 +95,7 @@ private:
   UPROPERTY(Category = "Traffic Light Manager", VisibleDefaultsOnly, BlueprintReadOnly, meta = (AllowPrivateAccess = "true"))
   USceneComponent *SceneComponent;
 
-  boost::optional<carla::road::Map> Map;
-
-  UPROPERTY()
+  UPROPERTY(EditAnywhere, Category= "Traffic Light Manager")
   bool TrafficLightsGenerated = false;
 
   UPROPERTY()