Updated traffic light serialization on server side

LibCarla/source/carla/sensor/data/ActorDynamicState.h

@@ -119,6 +119,8 @@ namespace detail {
 
     geom::Vector3D acceleration;
 
+    uint32_t num_components;
+
     union TypeDependentState {
       detail::TrafficLightData traffic_light_data;
       detail::VehicleData vehicle_data;
@@ -129,12 +131,27 @@ namespace detail {
 #pragma pack(pop)
 
 static_assert(
-    sizeof(ActorDynamicState) == 93u,
+    sizeof(ActorDynamicState) == 97u,
     "Invalid ActorDynamicState size! "
     "If you modified this class please update the size here, else you may "
     "comment this assert, but your platform may have compatibility issues "
     "connecting to other platforms.");
 
+#pragma pack(push, 1)
+
+/// Dynamic state of a component of an actor at a certain frame.
+struct ComponentDynamicState {
+
+  // World transform
+  geom::Transform transform;
+
+  union TypeDependentState {
+    detail::TrafficLightData traffic_light_data;
+  } state;
+
+};
+#pragma pack(pop)
+
 } // namespace data
 } // namespace sensor
 } // namespace carla

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp

@@ -8,6 +8,9 @@
 #include "Carla/Sensor/WorldObserver.h"
 
 #include "Carla/Traffic/TrafficLightBase.h"
+#include "Carla/Traffic/TrafficLightComponent.h"
+#include "Carla/Traffic/TrafficLightController.h"
+#include "Carla/Traffic/TrafficLightGroup.h"
 #include "Carla/Walker/WalkerController.h"
 
 #include "CoreGlobals.h"
@@ -64,6 +67,11 @@ static auto FWorldObserver_GetActorState(const FActorView &View, const FActorReg
     auto TrafficLight = Cast<ATrafficLightBase>(View.GetActor());
     if (TrafficLight != nullptr)
     {
+
+      UActorComponent* TrafficLightComponent = TrafficLight->FindComponentByClass<UTrafficLightComponent>();
+
+      if(TrafficLightComponent == nullptr) {
+        // Old way: traffic lights are actors
         using TLS = carla::rpc::TrafficLightState;
         state.traffic_light_data.state = static_cast<TLS>(TrafficLight->GetTrafficLightState());
         state.traffic_light_data.green_time = TrafficLight->GetGreenTime();
@@ -74,10 +82,54 @@ static auto FWorldObserver_GetActorState(const FActorView &View, const FActorReg
         state.traffic_light_data.pole_index = TrafficLight->GetPoleIndex();
       }
     }
+  }
 
   return state;
 }
 
+static void FWorldObserver_GetActorComponentsState(
+  const FActorView &View,
+  const FActorRegistry &Registry,
+  TArray<carla::sensor::data::ComponentDynamicState>& Out)
+{
+  using AType = FActorView::ActorType;
+
+  if (AType::TrafficLight == View.GetActorType())
+  {
+    auto TrafficLightActor = Cast<ATrafficLightBase>(View.GetActor());
+    if (TrafficLightActor == nullptr)
+    {
+      return;
+    }
+
+    TArray<UTrafficLightComponent*> TrafficLights;
+    TrafficLightActor->GetComponents<UTrafficLightComponent>(TrafficLights);
+
+    for(auto& TrafficLight : TrafficLights)
+    {
+      using TLS = carla::rpc::TrafficLightState;
+
+      UTrafficLightController* Controller =  TrafficLight->GetController();
+      ATrafficLightGroup* Group = TrafficLight->GetGroup();
+
+      carla::sensor::data::ComponentDynamicState CompState;
+      CompState.transform = TrafficLight->GetComponentTransform();
+      CompState.state.traffic_light_data.state = static_cast<TLS>(TrafficLight->GetLightState());
+      CompState.state.traffic_light_data.green_time = Controller->GetGreenTime();
+      CompState.state.traffic_light_data.yellow_time = Controller->GetYellowTime();
+      CompState.state.traffic_light_data.red_time = Controller->GetRedTime();
+      CompState.state.traffic_light_data.elapsed_time = Group->GetElapsedTime();
+      CompState.state.traffic_light_data.time_is_frozen = Group->IsFrozen();
+      // Nobody is using right now, perhaps we should remove it?
+      CompState.state.traffic_light_data.pole_index = 0;
+
+      Out.Push(CompState);
+    }
+
+  }
+
+}
+
 static carla::geom::Vector3D FWorldObserver_GetAngularVelocity(const AActor &Actor)
 {
   const auto RootComponent = Cast<UPrimitiveComponent>(Actor.GetRootComponent());
@@ -106,6 +158,7 @@ static carla::Buffer FWorldObserver_Serialize(
 {
   using Serializer = carla::sensor::s11n::EpisodeStateSerializer;
   using ActorDynamicState = carla::sensor::data::ActorDynamicState;
+  using ComponentDynamicState = carla::sensor::data::ComponentDynamicState;
 
   const auto &Registry = Episode.GetActorRegistry();
 
@@ -138,9 +191,20 @@ static carla::Buffer FWorldObserver_Serialize(
       carla::geom::Vector3D{Velocity.X, Velocity.Y, Velocity.Z},
       FWorldObserver_GetAngularVelocity(*View.GetActor()),
       FWorldObserver_GetAcceleration(View, Velocity, DeltaSeconds),
-      FWorldObserver_GetActorState(View, Registry)
+      0,
+      FWorldObserver_GetActorState(View, Registry),
     };
+
+    TArray<ComponentDynamicState> ComponentsState;
+    FWorldObserver_GetActorComponentsState(View, Registry, ComponentsState);
+    info.num_components = ComponentsState.Num();
+
     write_data(info);
+
+    for(auto& CompState : ComponentsState)
+    {
+      write_data(CompState);
+    }
   }
 
   check(begin == buffer.end());

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

@@ -51,6 +51,8 @@ void ATrafficLightBase::OnConstruction(const FTransform &Transform)
 
 void ATrafficLightBase::Tick(float DeltaSeconds)
 {
+  Super::Tick(DeltaSeconds);
+
   if (TimeIsFrozen)
   {
     return;

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

@@ -7,9 +7,10 @@
 
 #include "TrafficLightComponent.h"
 #include "Kismet/GameplayStatics.h"
+#include "TrafficLightController.h"
 #include "TrafficLightGroup.h"
-#include "TrafficLightManager.h"
 #include "TrafficLightInterface.h"
+#include "TrafficLightManager.h"
 
 UTrafficLightComponent::UTrafficLightComponent()
   : Super()

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

@@ -13,6 +13,7 @@
 
 class ATrafficLightManager;
 class ATrafficLightGroup;
+class UTrafficLightController;
 
 // Delegate to define dispatcher
 DECLARE_DYNAMIC_MULTICAST_DELEGATE(FLightChangeDispatcher);
@@ -27,18 +28,21 @@ public:
   // Sets default values for this component's properties
   UTrafficLightComponent();
 
-  UFUNCTION(BlueprintCallable)
+  UFUNCTION(Category = "Traffic Light", BlueprintCallable)
   void SetLightState(ETrafficLightState NewState);
 
-  UFUNCTION(BlueprintCallable)
+  UFUNCTION(Category = "Traffic Light", BlueprintCallable)
   ETrafficLightState GetLightState() const;
 
-  UFUNCTION(BlueprintCallable)
+  UFUNCTION(Category = "Traffic Light", BlueprintCallable)
   void SetFrozenGroup(bool InFreeze);
 
-  UFUNCTION(BlueprintPure)
+  UFUNCTION(Category = "Traffic Light", BlueprintPure)
   ATrafficLightGroup* GetGroup();
 
+  UFUNCTION(Category = "Traffic Light", BlueprintPure)
+  UTrafficLightController* GetController();
+
 protected:
   // Called when the game starts
   virtual void BeginPlay() override;
@@ -52,9 +56,13 @@ private:
   UPROPERTY(Category = "Traffic Light", EditAnywhere)
   ETrafficLightState LightState;
 
-  UPROPERTY(BlueprintAssignable, Category = "Traffic Light")
+  UPROPERTY(Category = "Traffic Light", BlueprintAssignable)
   FLightChangeDispatcher LightChangeDispatcher;
 
   UPROPERTY()
   ATrafficLightGroup *TrafficLightGroup = nullptr;
+
+  UPROPERTY()
+  UTrafficLightController *TrafficLightController = nullptr;
+
 };

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

@@ -77,3 +77,30 @@ void UTrafficLightController::ResetState()
   CurrentState = (LightStates.Num() - 1);
   SetTrafficLightsState(GetCurrentState().State);
 }
+
+float UTrafficLightController::GetGreenTime() const
+{
+  return GetStateTime(ETrafficLightState::Green);
+}
+
+float UTrafficLightController::GetYellowTime() const
+{
+  return GetStateTime(ETrafficLightState::Yellow);
+}
+
+float UTrafficLightController::GetRedTime() const
+{
+  return GetStateTime(ETrafficLightState::Red);
+}
+
+float UTrafficLightController::GetStateTime(const ETrafficLightState State) const
+{
+  for(auto& LightState : LightStates)
+  {
+    if(LightState.State == State)
+    {
+      return LightState.Time;
+    }
+  }
+  return 0.0f;
+}

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

@@ -38,59 +38,74 @@ public:
 
   UTrafficLightController();
 
-  UFUNCTION(BlueprintCallable)
+  UFUNCTION(Category = "Traffic Controller", BlueprintCallable)
   void SetStates(TArray<FTrafficLightStage> States);
 
-  UFUNCTION(BlueprintPure)
+  UFUNCTION(Category = "Traffic Controller", BlueprintPure)
   const FTrafficLightStage &GetCurrentState() const;
 
   // Updates traffic light components to the next state
-  UFUNCTION(BlueprintCallable)
+  UFUNCTION(Category = "Traffic Controller", BlueprintCallable)
   float NextState();
 
-  UFUNCTION(BlueprintPure)
+  UFUNCTION(Category = "Traffic Controller", BlueprintPure)
   const TArray<UTrafficLightComponent *> &GetTrafficLights();
 
-  UFUNCTION(BlueprintPure)
+  UFUNCTION(Category = "Traffic Controller", BlueprintPure)
   const FString &GetControllerId() const;
 
-  UFUNCTION(BlueprintCallable)
+  UFUNCTION(Category = "Traffic Controller", BlueprintCallable)
   void SetControllerId(const FString &Id);
 
-  UFUNCTION(BlueprintCallable)
+  UFUNCTION(Category = "Traffic Controller", BlueprintCallable)
   void AddTrafficLight(UTrafficLightComponent * TrafficLight);
 
-  UFUNCTION(BlueprintCallable)
+  UFUNCTION(Category = "Traffic Controller", BlueprintCallable)
   bool IsCycleFinished() const;
 
-  UFUNCTION(BlueprintCallable)
+  UFUNCTION(Category = "Traffic Controller", BlueprintCallable)
   void SetTrafficLightsState(ETrafficLightState NewState);
 
-  UFUNCTION(BlueprintCallable)
+  UFUNCTION(Category = "Traffic Controller", BlueprintCallable)
   int GetSequence() const;
 
-  UFUNCTION(BlueprintCallable)
+  UFUNCTION(Category = "Traffic Controller", BlueprintCallable)
   void SetSequence(int InSequence);
 
-  UFUNCTION(BlueprintCallable)
+  UFUNCTION(Category = "Traffic Controller", BlueprintCallable)
   void ResetState();
+
+  UFUNCTION(Category = "Traffic Controller", BlueprintCallable)
+  float GetGreenTime() const;
+
+  UFUNCTION(Category = "Traffic Controller", BlueprintCallable)
+  float GetYellowTime() const;
+
+  UFUNCTION(Category = "Traffic Controller", BlueprintCallable)
+  float GetRedTime() const;
+
 private:
 
-  UPROPERTY(EditAnywhere)
+  float GetStateTime(const ETrafficLightState State) const;
+
+  UPROPERTY(Category = "Traffic Controller", EditAnywhere)
   FString ControllerId;
 
-  UPROPERTY(EditAnywhere)
+  UPROPERTY(Category = "Traffic Controller", EditAnywhere)
   int CurrentState = 0;
 
   // Pairs with the state of the semaphors (time - state) e.g. 10s in green
-  UPROPERTY(EditAnywhere)
+  UPROPERTY(Category = "Traffic Controller", EditAnywhere)
   TArray<FTrafficLightStage> LightStates = {
-      {10, ETrafficLightState::Green}, {3, ETrafficLightState::Yellow}, {2, ETrafficLightState::Red}};
+    {10, ETrafficLightState::Green},
+    { 3, ETrafficLightState::Yellow},
+    { 2, ETrafficLightState::Red}
+  };
 
-  UPROPERTY(EditAnywhere)
+  UPROPERTY(Category = "Traffic Controller", EditAnywhere)
   TArray<UTrafficLightComponent *> TrafficLights;
 
   // Sequence within junction (unused for now)
-  UPROPERTY(EditAnywhere)
+  UPROPERTY(Category = "Traffic Controller", EditAnywhere)
   int Sequence = 0;
 };

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

@@ -17,18 +17,16 @@ ATrafficLightGroup::ATrafficLightGroup()
   RootComponent = SceneComponent;
 }
 
-// Called when the game starts or when spawned
-void ATrafficLightGroup::BeginPlay()
-{
-  Super::BeginPlay();
-
-}
-
 void ATrafficLightGroup::SetFrozenGroup(bool InFreeze)
 {
   bIsFrozen = InFreeze;
 }
 
+bool ATrafficLightGroup::IsFrozen()
+{
+  return bIsFrozen;
+}
+
 void ATrafficLightGroup::ResetGroup()
 {
   for(auto * Controller : Controllers)
@@ -37,6 +35,19 @@ void ATrafficLightGroup::ResetGroup()
   }
   CurrentController = 0;
   Timer = Controllers[CurrentController]->NextState();
+  CurrentStateTimer = Timer;
+}
+
+float ATrafficLightGroup::GetElapsedTime() const
+{
+  return (CurrentStateTimer - Timer);
+}
+
+// Called when the game starts or when spawned
+void ATrafficLightGroup::BeginPlay()
+{
+  Super::BeginPlay();
+
 }
 
 // Called every frame
@@ -75,6 +86,7 @@ void ATrafficLightGroup::NextController()
   CurrentController = (CurrentController + 1) % Controllers.Num();
   UTrafficLightController* controller = Controllers[CurrentController];
   Timer = controller->NextState();
+  CurrentStateTimer = Timer;
 }
 
 int ATrafficLightGroup::GetJunctionId() const

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

@@ -25,21 +25,27 @@ public:
   // Sets default values for this actor's properties
   ATrafficLightGroup();
 
-  UFUNCTION(BlueprintPure)
+  UFUNCTION(Category = "Traffic Group", BlueprintPure)
   TArray<UTrafficLightController*>& GetControllers()
   {
     return Controllers;
   }
 
-  UFUNCTION(BlueprintCallable)
+  UFUNCTION(Category = "Traffic Group", BlueprintCallable)
   void SetFrozenGroup(bool InFreeze);
 
-  UFUNCTION(BlueprintPure)
+  UFUNCTION(Category = "Traffic Group", BlueprintCallable)
+  bool IsFrozen();
+
+  UFUNCTION(Category = "Traffic Group", BlueprintPure)
   int GetJunctionId() const;
 
-  UFUNCTION(BlueprintCallable)
+  UFUNCTION(Category = "Traffic Group", BlueprintCallable)
   void ResetGroup();
 
+  UFUNCTION(Category = "Traffic Group", BlueprintCallable)
+  float GetElapsedTime() const;
+
 protected:
   // Called when the game starts or when spawned
   virtual void BeginPlay() override;
@@ -50,6 +56,7 @@ protected:
 private:
 
   friend ATrafficLightManager;
+
   UPROPERTY(Category = "Traffic Group", VisibleDefaultsOnly, BlueprintReadOnly, meta = (AllowPrivateAccess = "true"))
   USceneComponent *SceneComponent;
 
@@ -57,7 +64,10 @@ private:
   TArray<UTrafficLightController*> Controllers;
 
   UPROPERTY()
-  float Timer = 0;
+  float Timer = 0.0f;
+
+  UPROPERTY()
+  float CurrentStateTimer = 0.0f;
 
   UPROPERTY()
   int CurrentController = 0;

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

@@ -71,6 +71,7 @@ void ATrafficLightManager::RegisterLightComponent(UTrafficLightComponent * Traff
   auto *TrafficLightController = TrafficControllers[ControllerId.c_str()];
 
   TrafficLight->TrafficLightGroup = TrafficLightGroup;
+  TrafficLight->TrafficLightController = TrafficLightController;
 
   // Add signal to controller
   TrafficLightController->AddTrafficLight(TrafficLight);