Scene Capture Sensors are now enqueued to the end of the frame

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

@@ -12,6 +12,16 @@
 DECLARE_LOG_CATEGORY_EXTERN(LogCarla, Log, All);
 DECLARE_LOG_CATEGORY_EXTERN(LogCarlaServer, Log, All);
 
+// DisplayName, GroupName, Third param is always Advanced.
+// DECLARE_STATS_GROUP(TEXT("Carla"), STATGROUP_Carla, STATCAT_Advanced);
+DECLARE_STATS_GROUP(TEXT("CarlaSensor"), STATGROUP_CarlaSensor, STATCAT_Advanced);
+
+//DECLARE_MEMORY_STAT(TEXT("CARLAMEMORY"), STATGROUP_CARLAMEMORY, STATCAT_Advanced)
+
+DECLARE_CYCLE_STAT(TEXT("Read RT"),     STAT_CarlaSensorReadRT,     STATGROUP_CarlaSensor);
+DECLARE_CYCLE_STAT(TEXT("Buffer Copy"), STAT_CarlaSensorBufferCopy, STATGROUP_CarlaSensor);
+DECLARE_CYCLE_STAT(TEXT("Stream Send"), STAT_CarlaSensorStreamSend, STATGROUP_CarlaSensor);
+
 // Options to compile with extra debug log.
 #if WITH_EDITOR
 // #define CARLA_AI_VEHICLES_EXTRA_LOG

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

@@ -26,10 +26,13 @@ ADepthCamera::ADepthCamera(const FObjectInitializer &ObjectInitializer)
       TEXT("Material'/Carla/PostProcessingMaterials/DepthEffectMaterial.DepthEffectMaterial'")
 #endif
   );
+
+  Offset = carla::sensor::SensorRegistry::get<ADepthCamera*>::type::header_offset;
+
 }
 
 void ADepthCamera::Tick(float DeltaTime)
 {
   Super::Tick(DeltaTime);
-  FPixelReader::SendPixelsInRenderThread(*this);
+  SendPixelsInStream(*this);
 }

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

@@ -61,11 +61,12 @@ static void WritePixelsToBuffer_Vulkan(
     return;
   }
 
+  FIntPoint Rect = RenderResource->GetSizeXY();
   // NS: Extra copy here, don't know how to avoid it.
   TArray<FColor> Pixels;
   InRHICmdList.ReadSurfaceData(
       Texture,
-      FIntRect(0, 0, RenderResource->GetSizeXY().X, RenderResource->GetSizeXY().Y),
+      FIntRect(0, 0, Rect.X, Rect.Y),
       Pixels,
       FReadSurfaceDataFlags(RCM_UNorm, CubeFace_MAX));
 
@@ -184,3 +185,29 @@ void FPixelReader::WritePixelsToBuffer(
     Buffer.copy_from(Offset, Source, ExpectedStride * Height);
   }
 }
+
+// TODO: test on windows
+void FPixelReader::WritePixelsToArray(
+    UTextureRenderTarget2D &RenderTarget,
+    TArray<FColor>& Pixels,
+    FRHICommandListImmediate& InRHICmdList)
+{
+
+  check(IsInRenderingThread());
+  auto RenderResource =
+      static_cast<const FTextureRenderTarget2DResource *>(RenderTarget.Resource);
+  FTexture2DRHIRef Texture = RenderResource->GetRenderTargetTexture();
+  if (!Texture)
+  {
+    UE_LOG(LogCarla, Error, TEXT("FPixelReader: UTextureRenderTarget2D missing render target texture"));
+    return;
+  }
+
+  FIntPoint Rect = RenderResource->GetSizeXY();
+
+  InRHICmdList.ReadSurfaceData(
+      Texture,
+      FIntRect(0, 0, Rect.X, Rect.Y),
+      Pixels,
+      FReadSurfaceDataFlags(RCM_UNorm, CubeFace_MAX));
+}

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/PixelReader.h

@@ -64,6 +64,11 @@ public:
   template <typename TSensor>
   static void SendPixelsInRenderThread(TSensor &Sensor);
 
+  static void WritePixelsToArray(
+      UTextureRenderTarget2D &RenderTarget,
+      TArray<FColor>& Pixels,
+      FRHICommandListImmediate &InRHICmdList);
+
 private:
 
   /// Copy the pixels in @a RenderTarget into @a Buffer.
@@ -102,7 +107,11 @@ void FPixelReader::SendPixelsInRenderThread(TSensor &Sensor)
             Buffer,
             carla::sensor::SensorRegistry::get<TSensor *>::type::header_offset,
             InRHICmdList);
-        Stream.Send(Sensor, std::move(Buffer));
+
+        {
+          SCOPE_CYCLE_COUNTER(STAT_CarlaSensorStreamSend);
+          Stream.Send(Sensor, std::move(Buffer));
+        }
       }
     }
   );

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

@@ -20,10 +20,12 @@ ASceneCaptureCamera::ASceneCaptureCamera(const FObjectInitializer &ObjectInitial
 {
   AddPostProcessingMaterial(
       TEXT("Material'/Carla/PostProcessingMaterials/PhysicLensDistortion.PhysicLensDistortion'"));
+
+  Offset = carla::sensor::SensorRegistry::get<ASceneCaptureCamera*>::type::header_offset;
 }
 
 void ASceneCaptureCamera::Tick(float DeltaTime)
 {
   Super::Tick(DeltaTime);
-  FPixelReader::SendPixelsInRenderThread(*this);
+  SendPixelsInStream(*this);
 }

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

@@ -43,6 +43,9 @@ namespace SceneCaptureSensor_local_ns {
 // -- ASceneCaptureSensor ------------------------------------------------------
 // =============================================================================
 
+TArray<ASceneCaptureSensor*> ASceneCaptureSensor::CaptureSensors = {};
+int32 ASceneCaptureSensor::NumCaptureSensors = 0;
+
 ASceneCaptureSensor::ASceneCaptureSensor(const FObjectInitializer &ObjectInitializer)
   : Super(ObjectInitializer)
 {
@@ -481,6 +484,15 @@ void ASceneCaptureSensor::BeginPlay()
   GetEpisode().GetWeather()->NotifyWeather();
 
   Super::BeginPlay();
+
+  CaptureSensors.Add(this);
+
+  for(int32 i = 0; i < MaxNumTextures; i++)
+  {
+    Pixels[i].Reserve(ImageWidth * ImageHeight);
+  }
+
+  CaptureDelegate = FCoreDelegates::OnEndFrame.AddUObject(this, &ASceneCaptureCamera::Capture);
 }
 
 void ASceneCaptureSensor::Tick(float DeltaTime)
@@ -492,12 +504,56 @@ void ASceneCaptureSensor::Tick(float DeltaTime)
       CaptureComponent2D->GetComponentLocation(),
       ImageWidth,
       ImageWidth / FMath::Tan(CaptureComponent2D->FOVAngle));
+
+  PreviousTexture = CurrentTexture;
+  CurrentTexture = (CurrentTexture + 1) & ~MaxNumTextures;
 }
 
 void ASceneCaptureSensor::EndPlay(const EEndPlayReason::Type EndPlayReason)
 {
   Super::EndPlay(EndPlayReason);
   SCENE_CAPTURE_COUNTER = 0u;
+
+  CaptureSensors.Remove(this);
+
+  FCoreDelegates::OnEndFrame.Remove(CaptureDelegate);
+}
+
+void ASceneCaptureSensor::Capture()
+{
+
+  check(CaptureRenderTarget != nullptr);
+
+  ASceneCaptureSensor* This = this;
+  ENQUEUE_RENDER_COMMAND(ASceneCaptureSensor_SendPixelsInRenderThread)
+  (
+    [This, Sensors=CaptureSensors](FRHICommandListImmediate& RHICmdList) mutable
+    {
+
+      NumCaptureSensors++;
+
+      if( NumCaptureSensors < Sensors.Num() )
+      {
+        return;
+      }
+
+      for(ASceneCaptureSensor* Camera : Sensors)
+      {
+        if (Camera && Camera-> HasActorBegunPlay() && !Camera->IsPendingKill())
+        {
+          SCOPE_CYCLE_COUNTER(STAT_CarlaSensorReadRT);
+
+          ASceneCaptureSensor& CameraRef = *Camera;
+          FPixelReader::WritePixelsToArray(
+            *(CameraRef.CaptureRenderTarget),
+            CameraRef.Pixels[CameraRef.PreviousTexture],
+            RHICmdList);
+
+        }
+      }
+      NumCaptureSensors=0;
+    }
+  );
 }
 
 // =============================================================================

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureSensor.h

@@ -279,7 +279,42 @@ protected:
 
   virtual void SetUpSceneCaptureComponent(USceneCaptureComponent2D &SceneCapture) {}
 
-private:
+  uint32 Offset = 0u;
+
+  void Capture();
+
+  template <typename TSensor>
+  void SendPixelsInStream(TSensor &Sensor) {
+    TArray<FColor>& PixelsData = Pixels[PreviousTexture];
+
+    if(PixelsData.Num() > 0)
+    {
+      auto Stream = GetDataStream(Sensor);
+      auto Buffer = Stream.PopBufferFromPool();
+
+      {
+        SCOPE_CYCLE_COUNTER(STAT_CarlaSensorBufferCopy);
+        Buffer.copy_from(Offset, PixelsData);
+      }
+
+      {
+        SCOPE_CYCLE_COUNTER(STAT_CarlaSensorStreamSend);
+        Stream.Send(Sensor, std::move(Buffer));
+      }
+    }
+  }
+
+protected:
+
+  FDelegateHandle CaptureDelegate;
+
+  static TArray<ASceneCaptureSensor*> CaptureSensors;
+  static int32 NumCaptureSensors;
+  static const int32 MaxNumTextures = 2; // This has to be POT
+  int32 CurrentTexture = 0;
+  int32 PreviousTexture = 0;
+
+  TArray<FColor> Pixels[MaxNumTextures];
 
   /// Image width in pixels.
   UPROPERTY(EditAnywhere)

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

@@ -22,10 +22,12 @@ ASemanticSegmentationCamera::ASemanticSegmentationCamera(
       TEXT("Material'/Carla/PostProcessingMaterials/PhysicLensDistortion.PhysicLensDistortion'"));
   AddPostProcessingMaterial(
       TEXT("Material'/Carla/PostProcessingMaterials/GTMaterial.GTMaterial'"));
+
+  Offset = carla::sensor::SensorRegistry::get<ASemanticSegmentationCamera*>::type::header_offset;
 }
 
 void ASemanticSegmentationCamera::Tick(float DeltaTime)
 {
   Super::Tick(DeltaTime);
-  FPixelReader::SendPixelsInRenderThread(*this);
+  SendPixelsInStream(*this);
 }