Skip to content

feat: support actual depth image with 32FC1 datatype (#349) #352

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Feb 24, 2025
Merged

feat: support actual depth image with 32FC1 datatype (#349) #352

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
342 changes: 184 additions & 158 deletions Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2CameraComponent.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,158 +1,184 @@
// Copyright 2020-2023 Rapyuta Robotics Co., Ltd.

#include "Sensors/RRROS2CameraComponent.h"

#include "BufferVisualizationData.h"

URRROS2CameraComponent::URRROS2CameraComponent()
{
// component initialization
SceneCaptureComponent = CreateDefaultSubobject<USceneCaptureComponent2D>(TEXT("SceneCaptureComponent"));
SceneCaptureComponent->SetupAttachment(this);

CameraComponent = CreateDefaultSubobject<UCameraComponent>(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<UTextureRenderTarget2D>(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<FColor>* 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<FColor> 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<UROS2ImgMsg>(InMessage)->SetMsg(GetROS2Data());
}
// Copyright 2020-2023 Rapyuta Robotics Co., Ltd.

#include "Sensors/RRROS2CameraComponent.h"

#include "BufferVisualizationData.h"

URRROS2CameraComponent::URRROS2CameraComponent()
{
// component initialization
SceneCaptureComponent = CreateDefaultSubobject<USceneCaptureComponent2D>(TEXT("SceneCaptureComponent"));
SceneCaptureComponent->SetupAttachment(this);

CameraComponent = CreateDefaultSubobject<UCameraComponent>(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<UTextureRenderTarget2D>(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<UTextureRenderTarget2D>(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<FColor>* OutData;
FIntRect Rect;
FReadSurfaceDataFlags Flags;
};

struct FReadSurfaceContextDepth
{
FRenderTarget* SrcRenderTarget;
TArray<FFloat16Color>* 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<UROS2ImgMsg>(InMessage)->SetMsg(GetROS2Data());
}
6 changes: 2 additions & 4 deletions Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2CameraComponent.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@ struct FRenderRequest
{
GENERATED_BODY()
TArray<FColor> Image;
TArray<FFloat16Color> Depth;
FRenderCommandFence RenderFence;
};

Expand Down Expand Up @@ -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
Expand All @@ -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");
};
Loading