From 90c05d56071dcca0f3659fcfebd87d6929fbfeff Mon Sep 17 00:00:00 2001 From: Trim Bresilla Date: Sat, 22 Feb 2025 15:38:32 +0900 Subject: [PATCH] feat: support actual depth image with 32FC1 datatype (#349) - Add separate processing for RGB and Depth camera types with specific capture source and image properties. - Introduce new data structures to handle RGB and Depth surface contexts. - Adjust pixel format and data handling based on camera type, including custom encoding and image data initialization. - Improve conditions for processing captured data by ensuring proper checks are in place for RGB and Depth data retrieval. - Add a `Depth` array to `FRenderRequest` struct to store depth information. - Remove obsolete comments and redundant code sections to improve readability and maintainability. --- .../Private/Sensors/RRROS2CameraComponent.cpp | 342 ++++++++++-------- .../Public/Sensors/RRROS2CameraComponent.h | 6 +- 2 files changed, 186 insertions(+), 162 deletions(-) diff --git a/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2CameraComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2CameraComponent.cpp index e0f4d62f..7358cb74 100644 --- a/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2CameraComponent.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2CameraComponent.cpp @@ -1,158 +1,184 @@ -// Copyright 2020-2023 Rapyuta Robotics Co., Ltd. - -#include "Sensors/RRROS2CameraComponent.h" - -#include "BufferVisualizationData.h" - -URRROS2CameraComponent::URRROS2CameraComponent() -{ - // component initialization - SceneCaptureComponent = CreateDefaultSubobject(TEXT("SceneCaptureComponent")); - SceneCaptureComponent->SetupAttachment(this); - - CameraComponent = CreateDefaultSubobject(TEXT("CameraComponent")); - CameraComponent->SetupAttachment(this); - - TopicName = TEXT("raw_image"); - MsgClass = UROS2ImgMsg::StaticClass(); -} - -void URRROS2CameraComponent::PreInitializePublisher(UROS2NodeComponent* InROS2Node, const FString& InTopicName) -{ - SceneCaptureComponent->FOVAngle = CameraComponent->FieldOfView; - SceneCaptureComponent->OrthoWidth = CameraComponent->OrthoWidth; - - if (CameraType == EROS2CameraType::DEPTH) - { - FWeightedBlendable blendable(1.0f, GetBufferVisualizationData().GetMaterial(TEXT("SceneDepth"))); - CameraComponent->PostProcessSettings.WeightedBlendables.Array.Add(blendable); - SceneCaptureComponent->PostProcessSettings = CameraComponent->PostProcessSettings; - SceneCaptureComponent->CaptureSource = ESceneCaptureSource::SCS_FinalColorHDR; - } - - RenderTarget = NewObject(this, UTextureRenderTarget2D::StaticClass()); - RenderTarget->InitCustomFormat(Width, Height, EPixelFormat::PF_B8G8R8A8, true); - SceneCaptureComponent->TextureTarget = RenderTarget; - - // Initialize image data - Data.Header.FrameId = FrameId; - Data.Width = Width; - Data.Height = Height; - Data.Encoding = Encoding; - Data.Step = Width * 3; // todo should be variable based on encoding - Data.Data.AddUninitialized(Width * Height * 3); - - QueueSize = QueueSize < 1 ? 1 : QueueSize; // QueueSize should be more than 1 - - Super::PreInitializePublisher(InROS2Node, InTopicName); -} - -void URRROS2CameraComponent::SensorUpdate() -{ - if (Render) { - SceneCaptureComponent->CaptureScene(); - CaptureNonBlocking(); - } -} - -// reference https://github.com/TimmHess/UnrealImageCapture -void URRROS2CameraComponent::CaptureNonBlocking() -{ - SceneCaptureComponent->TextureTarget->TargetGamma = GEngine->GetDisplayGamma(); - // Get RenderContext - FTextureRenderTargetResource* renderTargetResource = SceneCaptureComponent->TextureTarget->GameThread_GetRenderTargetResource(); - - struct FReadSurfaceContext - { - FRenderTarget* SrcRenderTarget; - TArray* OutData; - FIntRect Rect; - FReadSurfaceDataFlags Flags; - }; - - // Init new RenderRequest - FRenderRequest* renderRequest = new FRenderRequest(); - - // Setup GPU command - FReadSurfaceContext readSurfaceContext = { - renderTargetResource, - &(renderRequest->Image), - FIntRect(0, 0, renderTargetResource->GetSizeXY().X, renderTargetResource->GetSizeXY().Y), - FReadSurfaceDataFlags(RCM_UNorm, CubeFace_MAX)}; - - // Above 4.22 use this - ENQUEUE_RENDER_COMMAND(SceneDrawCompletion) - ( - [readSurfaceContext, this](FRHICommandListImmediate& RHICmdList) - { - RHICmdList.ReadSurfaceData(readSurfaceContext.SrcRenderTarget->GetRenderTargetTexture(), - readSurfaceContext.Rect, - *readSurfaceContext.OutData, - readSurfaceContext.Flags); - }); - - // Notify new task in RenderQueue - RenderRequestQueue.Enqueue(renderRequest); - if (QueueCount > QueueSize) - { - RenderRequestQueue.Pop(); - } - else - { - QueueCount++; - } - - // Set RenderCommandFence - renderRequest->RenderFence.BeginFence(); -} - -FROSImg URRROS2CameraComponent::GetROS2Data() -{ - if (!RenderRequestQueue.IsEmpty()) - { - // Timestamp - Data.Header.Stamp = URRConversionUtils::FloatToROSStamp(UGameplayStatics::GetTimeSeconds(GetWorld())); - - // Peek the next RenderRequest from queue - FRenderRequest* nextRenderRequest = nullptr; - RenderRequestQueue.Peek(nextRenderRequest); - if (nextRenderRequest) - { // nullptr check - if (nextRenderRequest->RenderFence.IsFenceComplete()) - { // Check if rendering is done, indicated by RenderFence - for (int I = 0; I < nextRenderRequest->Image.Num(); I++) - { - Data.Data[I * 3 + 0] = nextRenderRequest->Image[I].R; - Data.Data[I * 3 + 1] = nextRenderRequest->Image[I].G; - Data.Data[I * 3 + 2] = nextRenderRequest->Image[I].B; - } - - // Delete the first element from RenderQueue - RenderRequestQueue.Pop(); - QueueCount--; - delete nextRenderRequest; - } - } - } - - // SceneCaptureComponent->CaptureScene(); - // FTextureRenderTarget2DResource* RenderTargetResource; - // RenderTargetResource = (FTextureRenderTarget2DResource*)RenderTarget->GameThread_GetRenderTargetResource(); - // if (RenderTargetResource) { - // TArray buffer; - // RenderTargetResource->ReadPixels(buffer); - // for (int I = 0; I < buffer.Num(); I++) - // { - // Data.data[I * 3 + 0] = buffer[I].R; - // Data.data[I * 3 + 1] = buffer[I].G; - // Data.data[I * 3 + 2] = buffer[I].B; - // } - // } - - return Data; -} - -void URRROS2CameraComponent::SetROS2Msg(UROS2GenericMsg* InMessage) -{ - CastChecked(InMessage)->SetMsg(GetROS2Data()); -} +// Copyright 2020-2023 Rapyuta Robotics Co., Ltd. + +#include "Sensors/RRROS2CameraComponent.h" + +#include "BufferVisualizationData.h" + +URRROS2CameraComponent::URRROS2CameraComponent() +{ + // component initialization + SceneCaptureComponent = CreateDefaultSubobject(TEXT("SceneCaptureComponent")); + SceneCaptureComponent->SetupAttachment(this); + + CameraComponent = CreateDefaultSubobject(TEXT("CameraComponent")); + CameraComponent->SetupAttachment(this); + + TopicName = TEXT("raw_image"); + MsgClass = UROS2ImgMsg::StaticClass(); +} + +void URRROS2CameraComponent::PreInitializePublisher(UROS2NodeComponent* InROS2Node, const FString& InTopicName) +{ + SceneCaptureComponent->FOVAngle = CameraComponent->FieldOfView; + SceneCaptureComponent->OrthoWidth = CameraComponent->OrthoWidth; + + // Set capture source and image properties based on camera type + if (CameraType == EROS2CameraType::RGB) + { + SceneCaptureComponent->CaptureSource = ESceneCaptureSource::SCS_FinalColorHDR; + RenderTarget = NewObject(this, UTextureRenderTarget2D::StaticClass()); + RenderTarget->InitCustomFormat(Width, Height, EPixelFormat::PF_B8G8R8A8, true); + + Data.Encoding = TEXT("bgr8"); + Data.Step = Width * 3; + } + else if (CameraType == EROS2CameraType::DEPTH) + { + CameraComponent->PostProcessSettings.WeightedBlendables.Array.Add( + FWeightedBlendable(1.0f, GetBufferVisualizationData().GetMaterial(TEXT("SceneDepth")))); + SceneCaptureComponent->PostProcessSettings = CameraComponent->PostProcessSettings; + SceneCaptureComponent->CaptureSource = ESceneCaptureSource::SCS_SceneDepth; + + RenderTarget = NewObject(this, UTextureRenderTarget2D::StaticClass()); + RenderTarget->InitCustomFormat(Width, Height, EPixelFormat::PF_FloatRGBA, false); + Data.Encoding = TEXT("32FC1"); + Data.Step = Width * 4; + } + + // Common setup + SceneCaptureComponent->TextureTarget = RenderTarget; + Data.Header.FrameId = FrameId; + Data.Width = Width; + Data.Height = Height; + Data.Data.AddUninitialized(Width * Height * (CameraType == EROS2CameraType::RGB ? 3 : 4)); + QueueSize = QueueSize < 1 ? 1 : QueueSize; // QueueSize should be more than 1 + Super::PreInitializePublisher(InROS2Node, InTopicName); +} + +void URRROS2CameraComponent::SensorUpdate() +{ + if (Render) + { + SceneCaptureComponent->CaptureScene(); + CaptureNonBlocking(); + } +} + +// reference https://github.com/TimmHess/UnrealImageCapture +void URRROS2CameraComponent::CaptureNonBlocking() +{ + SceneCaptureComponent->TextureTarget->TargetGamma = GEngine->GetDisplayGamma(); + // Get RenderContext + FTextureRenderTargetResource* renderTargetResource = SceneCaptureComponent->TextureTarget->GameThread_GetRenderTargetResource(); + + struct FReadSurfaceContextRGB + { + FRenderTarget* SrcRenderTarget; + TArray* OutData; + FIntRect Rect; + FReadSurfaceDataFlags Flags; + }; + + struct FReadSurfaceContextDepth + { + FRenderTarget* SrcRenderTarget; + TArray* Depth; + FIntRect Rect; + FReadSurfaceDataFlags Flags; + }; + + // Init new RenderRequest + FRenderRequest* renderRequest = new FRenderRequest(); + + // Setup GPU command + FIntRect rect(0, 0, renderTargetResource->GetSizeXY().X, renderTargetResource->GetSizeXY().Y); + FReadSurfaceDataFlags flags(RCM_UNorm, CubeFace_MAX); + + if (CameraType == EROS2CameraType::RGB) + { + FReadSurfaceContextRGB readSurfaceContext = {renderTargetResource, &(renderRequest->Image), rect, flags}; + + ENQUEUE_RENDER_COMMAND(SceneDrawCompletion) + ( + [readSurfaceContext, this](FRHICommandListImmediate& RHICmdList) + { + RHICmdList.ReadSurfaceData(readSurfaceContext.SrcRenderTarget->GetRenderTargetTexture(), + readSurfaceContext.Rect, + *readSurfaceContext.OutData, + readSurfaceContext.Flags); + }); + } + else if (CameraType == EROS2CameraType::DEPTH) + { + FReadSurfaceContextDepth readSurfaceContext = {renderTargetResource, &(renderRequest->Depth), rect, flags}; + + ENQUEUE_RENDER_COMMAND(SceneDrawCompletion) + ( + [readSurfaceContext, this](FRHICommandListImmediate& RHICmdList) + { + RHICmdList.ReadSurfaceFloatData(readSurfaceContext.SrcRenderTarget->GetRenderTargetTexture(), + readSurfaceContext.Rect, + *readSurfaceContext.Depth, + readSurfaceContext.Flags); + }); + } + + // Notify new task in RenderQueue + RenderRequestQueue.Enqueue(renderRequest); + if (QueueCount > QueueSize) + { + RenderRequestQueue.Pop(); + } + else + { + QueueCount++; + } + + // Set RenderCommandFence + renderRequest->RenderFence.BeginFence(); +} + +FROSImg URRROS2CameraComponent::GetROS2Data() +{ + if (!RenderRequestQueue.IsEmpty()) + { + // Timestamp + Data.Header.Stamp = URRConversionUtils::FloatToROSStamp(UGameplayStatics::GetTimeSeconds(GetWorld())); + // Peek the next RenderRequest from queue + FRenderRequest* nextRenderRequest = nullptr; + RenderRequestQueue.Peek(nextRenderRequest); + if (nextRenderRequest && nextRenderRequest->RenderFence.IsFenceComplete()) + { + if (CameraType == EROS2CameraType::RGB) + { + // Process RGB data + for (int i = 0; i < nextRenderRequest->Image.Num(); i++) + { + Data.Data[i * 3 + 0] = nextRenderRequest->Image[i].B; + Data.Data[i * 3 + 1] = nextRenderRequest->Image[i].G; + Data.Data[i * 3 + 2] = nextRenderRequest->Image[i].R; + } + } + else if (CameraType == EROS2CameraType::DEPTH) + { + // Process Depth data + for (int i = 0; i < nextRenderRequest->Depth.Num(); i++) + { + float value = nextRenderRequest->Depth[i].R.GetFloat() / 100; + std::memcpy(&Data.Data[i * 4], &value, sizeof(value)); + } + } + + // Delete the first element from RenderQueue + RenderRequestQueue.Pop(); + QueueCount--; + delete nextRenderRequest; + } + } + return Data; +} + +void URRROS2CameraComponent::SetROS2Msg(UROS2GenericMsg* InMessage) +{ + CastChecked(InMessage)->SetMsg(GetROS2Data()); +} diff --git a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2CameraComponent.h b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2CameraComponent.h index 89420d82..1d49eed1 100644 --- a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2CameraComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2CameraComponent.h @@ -30,6 +30,7 @@ struct FRenderRequest { GENERATED_BODY() TArray Image; + TArray Depth; FRenderCommandFence RenderFence; }; @@ -116,7 +117,7 @@ class RAPYUTASIMULATIONPLUGINS_API URRROS2CameraComponent : public URRROS2BaseSe UPROPERTY(EditAnywhere, BlueprintReadWrite) bool Render = true; - + // ROS /** * @brief Update ROS 2 Msg structure from #RenderRequestQueue @@ -132,7 +133,4 @@ class RAPYUTASIMULATIONPLUGINS_API URRROS2CameraComponent : public URRROS2BaseSe * @param InMessage */ virtual void SetROS2Msg(UROS2GenericMsg* InMessage) override; - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - FString Encoding = TEXT("rgb8"); };