#include <LidarManager.h>

Inheritance diagram for ALidarManager:

Collaboration diagram for ALidarManager:

[legend]

Public Member Functions
	ALidarManager (const FObjectInitializer &ObjectInitializer)

void	SetSimulateLidars (bool bSimulateLidars)

void	SetMeasurePerformance (bool bMeasurePerformance)

void	SetSaveCombinedCloudToDisk (bool bSaveCombinedCloud)

void	SetSendCombinedCloud (bool bSaveCombinedCloud)

void	AddRaycastLidar (ALidar *lidarPtr)

void	RemoveRaycastLidar (ALidar *lidarPtr)

int	GetLidarCount () const

TArray< ALidar * >	GetAllLidars () const

std::vector< ALidar * >	GetAllLidarsVector () const

	UPROPERTY (BlueprintReadOnly)

Public Attributes
FLidarDelegate_LidarSpawned	OnLidarSpawned

FLidarDelegate_LidarDestroyed	OnLidarDestroyed

bool	SendCombinedCloudToROS = true

bool	SaveCombinedCloudToDisk = false

bool	MeasurePerformance = false

float	TimeInMilliseconds

Private Member Functions
virtual void	BeginPlay () override

virtual void	EndPlay (const EEndPlayReason::Type EndPlayReason) override

void	TickParallel (float DeltaTime)

void	SimulateLidars (const float DeltaTime)

void	SendCombinedPointcloud (const std::vector< FPointData > &points)

void	SaveCombinedPointcloud (const std::vector< FPointData > &points)

void	ROSBridgeStateChanged (EROSState RosState)

void	InitTopic ()

void	DestroyTopic ()

Private Attributes
FTickEntry	TickEntry

bool	TickAdded = false

std::vector< ALidar * >	RaycastLidars

UROSIntegrationGameInstance *	RosInstance = nullptr

TWeakObjectPtr< UTopic >	CombinedLidarTopic

TSharedPtr< ROSMessages::sensor_msgs::PointCloud2 >	PointCloudMessage

SimpleTimer *	TotalAverageTimer = nullptr

bool	CanSimulateLidars = true

FString	FileSavePath

int32	LidarSaves = 0

Detailed Description

This class handles simulating all Lidar sensors in parallel.

Definition at line 40 of file LidarManager.h.

Constructor & Destructor Documentation

◆ ALidarManager()

ALidarManager::ALidarManager ( const FObjectInitializer & ObjectInitializer )

Definition at line 31 of file LidarManager.cpp.

                                                                        : Super(ObjectInitializer)
{
    // TickManager handles this Actor ticking
    PrimaryActorTick.bCanEverTick = false;
 
    if (FParse::Param(FCommandLine::Get(), TEXT("-save-combined-pointcloud")))
    {
        SaveCombinedCloudToDisk = true;
    }
}

References SaveCombinedCloudToDisk.

Member Function Documentation

◆ AddRaycastLidar()

void ALidarManager::AddRaycastLidar ( ALidar * lidarPtr )

Add Lidar sensor to this manager. This should only be called from Lidar.cpp

Parameters

lidarPtr Lidar class pointer

Definition at line 229 of file LidarManager.cpp.

{
    if (!lidarPtr)
    {
        return;
    }
 
    RaycastLidars.push_back(lidarPtr);
 
#if WITH_EDITOR
    UE_LOG(LogTemp, Warning, TEXT("LidarManager.cpp: Added Lidar sensor."));
#endif
 
    if (!TickAdded)
    {
        InitTopic();
 
        TickEntry = ATickManager::AddTick(this, BindTick(this, &ALidarManager::TickParallel), ETickType::FullFrameTaskParallel);
        TickAdded = true;
    }
 
    // Broadcast new lidar has been spawned, broadcast Lidar actor
    OnLidarSpawned.Broadcast(lidarPtr);
}

References ATickManager::AddTick(), BindTick(), InitTopic(), OnLidarSpawned, RaycastLidars, TickAdded, TickEntry, and TickParallel().

Referenced by ALidar::BeginPlay().

◆ BeginPlay()

void ALidarManager::BeginPlay ( )

overrideprivatevirtual

Definition at line 42 of file LidarManager.cpp.

{
    Super::BeginPlay();
 
    // There should only be one instance of this actor which is spawned in AgrarsenseGameModebase.cpp
    UWorld* World = GetWorld();
    TArray<AActor*> FoundActors;
    UGameplayStatics::GetAllActorsOfClass(World, ALidarManager::StaticClass(), FoundActors);
    if (FoundActors.Num() != 1)
    {
#ifdef WITH_EDITOR
        UE_LOG(LogTemp, Warning, TEXT("LidarManager.cpp: LidarManager already exists in the level. Destroying this one."));
#endif
        this->Destroy();
        return;
    }
 
    RosInstance = Cast<UROSIntegrationGameInstance>(GetGameInstance());
 
    TotalAverageTimer = new SimpleTimer();
 
    FString DataLocation = UAgrarsensePaths::GetDataFolder();
    FString UnixTimeStamp = FString::FromInt(FDateTime::Now().ToUnixTimestamp());
    FileSavePath = DataLocation + "LidarCombined_" + UnixTimeStamp + "/";
 
    // Initialize the PointCloudMessage
    PointCloudMessage = MakeShared<ROSMessages::sensor_msgs::PointCloud2>();
    PointCloudMessage->CreateDefaultPointCloud2Message();
}

References Destroy, FileSavePath, UAgrarsensePaths::GetDataFolder(), PointCloudMessage, RosInstance, and TotalAverageTimer.

◆ DestroyTopic()

void ALidarManager::DestroyTopic ( )

private

Destroy ROS topic

Definition at line 318 of file LidarManager.cpp.

{
    if (CombinedLidarTopic.IsValid())
    {
        CombinedLidarTopic->Unadvertise();
        CombinedLidarTopic->Unsubscribe();
        CombinedLidarTopic->MarkAsDisconnected();
        CombinedLidarTopic->ConditionalBeginDestroy();
        CombinedLidarTopic.Reset();
    }
}

References CombinedLidarTopic.

Referenced by EndPlay(), RemoveRaycastLidar(), and ROSBridgeStateChanged().

◆ EndPlay()

void ALidarManager::EndPlay ( const EEndPlayReason::Type EndPlayReason )

overrideprivatevirtual

Definition at line 72 of file LidarManager.cpp.

{
    Super::EndPlay(EndPlayReason);
 
    DestroyTopic();
 
    PointCloudMessage.Reset();
 
    RaycastLidars.clear();
 
    RosInstance = nullptr;
 
    delete(TotalAverageTimer);
 
    if (TickAdded)
    {
        ATickManager::RemoveTick(TickEntry);
    }
}

References DestroyTopic(), PointCloudMessage, RaycastLidars, ATickManager::RemoveTick(), RosInstance, TickAdded, TickEntry, and TotalAverageTimer.

◆ GetAllLidars()

TArray< ALidar * > ALidarManager::GetAllLidars ( ) const

inline

Get all Lidar sensors added to this Lidar Mananger

Returns: Tarray of ALidar pointers.

Definition at line 103 of file LidarManager.h.

    {
        // Create TArray<ALidar*> since LidarManager uses std::vector 
        // to keep track of Lidar sensors and Blueprints only support TArrays
        TArray<ALidar*> Lidars;
 
        for (int32 i = 0; i < RaycastLidars.size(); i++)
        {
            ALidar* LidarPtr = RaycastLidars[i];
            if (LidarPtr)
            {
                Lidars.Add(LidarPtr);
            }
        }
 
        return Lidars;
    }

◆ GetAllLidarsVector()

std::vector< ALidar * > ALidarManager::GetAllLidarsVector ( ) const

inline

Definition at line 121 of file LidarManager.h.

    {
        return RaycastLidars;
    }

◆ GetLidarCount()

int ALidarManager::GetLidarCount ( ) const

inline

Get current Lidar count. Callable from blueprints.

Returns: int lidar count

Definition at line 93 of file LidarManager.h.

    {
        return RaycastLidars.size();
    }

◆ InitTopic()

void ALidarManager::InitTopic ( )

private

Initialize ROS topic

Definition at line 302 of file LidarManager.cpp.

{
    if (CombinedLidarTopic.IsValid() || !SendCombinedCloudToROS || !RosInstance->IsROSConnected())
    {
        return;
    }
 
    CombinedLidarTopic = NewObject<UTopic>(UTopic::StaticClass());
    CombinedLidarTopic->Init(RosInstance->ROSIntegrationCore, FString::Printf(TEXT("/agrarsense/out/sensors/lidars_combined")), TEXT("sensor_msgs/PointCloud2"));
    CombinedLidarTopic->Advertise();
 
#if WITH_EDITOR
    UE_LOG(LogTemp, Warning, TEXT("LidarManager.cpp: Created combined Lidar ROS topic. Topic name: /agrarsense/sensors/lidars_combined"));
#endif
}

References CombinedLidarTopic, RosInstance, and SendCombinedCloudToROS.

Referenced by AddRaycastLidar(), ROSBridgeStateChanged(), and SetSendCombinedCloud().

◆ RemoveRaycastLidar()

void ALidarManager::RemoveRaycastLidar ( ALidar * lidarPtr )

Remove Lidar sensor from this manager. This should only be called from Lidar.cpp

Parameters

lidarPtr Lidar class pointer

Definition at line 254 of file LidarManager.cpp.

{
    if (!lidarPtr)
    {
        return;
    }
 
    auto it = std::find(RaycastLidars.begin(), RaycastLidars.end(), lidarPtr);
    if (it != RaycastLidars.end())
    {
        RaycastLidars.erase(it);
 
#if WITH_EDITOR
        UE_LOG(LogTemp, Warning, TEXT("LidarManager.cpp: Removed Lidar sensor"));
#endif
    }
 
    // Disable tick if there's no Lidar sensors.
    if (RaycastLidars.size() == 0)
    {
        TotalAverageTimer->ResetAverageTime();
        if (TickAdded)
        {
            ATickManager::RemoveTick(TickEntry);
            TickAdded = false;
            DestroyTopic();
        }
    }
 
    // Broadcast lidar sensor has been destroyed event and broadcast current lidar count
    int currentLidarCount = RaycastLidars.size();
    OnLidarDestroyed.Broadcast(currentLidarCount);
}

References DestroyTopic(), OnLidarDestroyed, RaycastLidars, ATickManager::RemoveTick(), SimpleTimer::ResetAverageTime(), TickAdded, TickEntry, and TotalAverageTimer.

Referenced by ALidar::EndPlay().

◆ ROSBridgeStateChanged()

void ALidarManager::ROSBridgeStateChanged ( EROSState RosState )

private

Callback when ROS Bridge state changed. must be marked as UFUNCTION, otherwise binding to event will not work.

Definition at line 288 of file LidarManager.cpp.

{
    switch (RosState)
    {
    case EROSState::Connected:
        InitTopic();
        break;
 
    case EROSState::Disconnected:
        DestroyTopic();
        break;
    }
}

References Connected, DestroyTopic(), Disconnected, and InitTopic().

◆ SaveCombinedPointcloud()

void ALidarManager::SaveCombinedPointcloud ( const std::vector< FPointData > & points )

private

Save combined cloud as ply to disk

Definition at line 330 of file LidarManager.cpp.

{
    if (!SaveCombinedCloudToDisk)
    {
        return;
    }
 
    FString Filename = FString::Printf(TEXT("%sCombinedPointcloud_%d.ply"), *FileSavePath, LidarSaves);
    ++LidarSaves;
 
    UPointcloudUtilities::SaveVectorArrayAsPlyAsync(Filename, points);
}

References FileSavePath, LidarSaves, SaveCombinedCloudToDisk, and UPointcloudUtilities::SaveVectorArrayAsPlyAsync().

Referenced by SimulateLidars().

◆ SendCombinedPointcloud()

void ALidarManager::SendCombinedPointcloud ( const std::vector< FPointData > & points )

private

Send combined pointcloud to ROS topic

Definition at line 343 of file LidarManager.cpp.

{
    TRACE_CPUPROFILER_EVENT_SCOPE(ALidarManager::SendCombinedPointcloud);
 
    UTopic* Topic = CombinedLidarTopic.Get();
 
    if (!SendCombinedCloudToROS
        || !RosInstance
        || !RosInstance->IsROSConnected()
        || !Topic
        || !PointCloudMessage.IsValid())
    {
        return;
    }
 
    const size_t PointsSize = points.size();
    const size_t DataSize = PointsSize * sizeof(FPointData);
 
    PointCloudMessage->width = static_cast<uint32>(PointsSize);
    PointCloudMessage->row_step = static_cast<uint32>(DataSize);
    PointCloudMessage->data_ptr = reinterpret_cast<uint8*>(const_cast<FPointData*>(points.data()));
    Topic->Publish(PointCloudMessage);
}

References CombinedLidarTopic, PointCloudMessage, RosInstance, SendCombinedCloudToROS, and SendCombinedPointcloud().

Referenced by SendCombinedPointcloud(), and SimulateLidars().

◆ SetMeasurePerformance()

void ALidarManager::SetMeasurePerformance ( bool bMeasurePerformance )

Set boolean whether to measure Lidar performance (logs milliseconds to console)

Parameters

bMeasurePerformance boolean to toggle performance measurement

Definition at line 206 of file LidarManager.cpp.

{
    MeasurePerformance = bMeasurePerformance;
}

References MeasurePerformance.

◆ SetSaveCombinedCloudToDisk()

void ALidarManager::SetSaveCombinedCloudToDisk ( bool bSaveCombinedCloud )

Set boolean whether to save combined cloud to disk

Parameters

bSaveCombinedCloud boolean to set saving to disk

Definition at line 211 of file LidarManager.cpp.

{
    SaveCombinedCloudToDisk = bSaveCombinedCloud;
}

References SaveCombinedCloudToDisk.

Referenced by UROSCommands::HandleSetSaveCombinedPointcloudToDisk().

◆ SetSendCombinedCloud()

void ALidarManager::SetSendCombinedCloud ( bool bSaveCombinedCloud )

Set boolean whether to send combined cloud to ROS

Parameters

bSaveCombinedCloud boolean to set sending data to ROS

Definition at line 216 of file LidarManager.cpp.

{
    SendCombinedCloudToROS = bSaveCombinedCloud;
    if (SendCombinedCloudToROS)
    {
        InitTopic();
    }
 
#if WITH_EDITOR
    UE_LOG(LogTemp, Warning, TEXT("LidarManager.cpp: Saving combined Lidar pointcloud to disk: %s"), (SendCombinedCloudToROS ? TEXT("true") : TEXT("false")));
#endif
}

References InitTopic(), and SendCombinedCloudToROS.

◆ SetSimulateLidars()

void ALidarManager::SetSimulateLidars ( bool bSimulateLidars )

Set boolean whether Lidar sensors be simulated

Parameters

bSimulateLidars boolean to set simulating Lidars on/off

Definition at line 201 of file LidarManager.cpp.

{
    CanSimulateLidars = bSimulateLidars;
}

References CanSimulateLidars.

◆ SimulateLidars()

void ALidarManager::SimulateLidars ( const float DeltaTime )

private

Simulate all Lidar sensors

Parameters

DeltaTime current DeltaTime

Definition at line 112 of file LidarManager.cpp.

{
    TRACE_CPUPROFILER_EVENT_SCOPE(ALidarManager::SimulateLidars);
 
    const int32 LidarCount = RaycastLidars.size();
 
    if (LidarCount == 0 || !CanSimulateLidars)
    {
        return;
    }
 
    std::vector<FPointData> FlattenedPoints;
 
    if (LidarCount == 1)
    {
        // If we only have one lidar sensor, 
        // we can skip ParallelFor and 2D to 1D std::vector conversion overhead completely
        ALidar* lidar = RaycastLidars[0];
        if (lidar)
        {
#ifdef ParallelLineTraceSingleByChannel_EXISTS
            // ParallelLineTraceSingleByChannel_EXISTS Defined and implemented in AGRARSENSE Unreal Engine fork
            // Lock physics scene while linetracing.
            auto PhysicsLock = FPhysicsObjectExternalInterface::LockRead(GetWorld()->GetPhysicsScene());
            {
#endif
                FlattenedPoints = lidar->SimulateRaycastLidar(DeltaTime);
 
#ifdef ParallelLineTraceSingleByChannel_EXISTS
            }
            PhysicsLock.Release();
#endif
        }
    }
    else
    {
        // Else we have multiple Lidar sensors,
        // Simulate all of them in parallel
        std::vector<std::vector<FPointData>> points(LidarCount);
 
#ifdef ParallelLineTraceSingleByChannel_EXISTS
        // ParallelLineTraceSingleByChannel_EXISTS Defined and implemented in AGRARSENSE Unreal Engine fork
        // Lock physics scene while linetracing.
        auto PhysicsLock = FPhysicsObjectExternalInterface::LockRead(GetWorld()->GetPhysicsScene());
        {
#endif
            ParallelFor(LidarCount, [&](int32 lidarIndex)
            {
                ALidar* lidar = RaycastLidars[lidarIndex];
                if (lidar)
                {
                    points[lidarIndex] = lidar->SimulateRaycastLidar(DeltaTime);
                }
            }, EParallelForFlags::Unbalanced);
 
#ifdef ParallelLineTraceSingleByChannel_EXISTS
        }
        PhysicsLock.Release();
#endif
        if (SaveCombinedCloudToDisk || SendCombinedCloudToROS)
        {
            int32 TotalSize = 0;
            for (const std::vector<FPointData>& vec : points)
            {
                TotalSize += vec.size();
            }
 
            if (TotalSize != 0)
            {
                FlattenedPoints.reserve(TotalSize);
                for (std::vector<FPointData>& vec : points)
                {
                    FlattenedPoints.insert(FlattenedPoints.end(), std::make_move_iterator(vec.begin()), std::make_move_iterator(vec.end()));
                }
            }
        }
    }
 
    if (!FlattenedPoints.empty())
    {
        if (SaveCombinedCloudToDisk)
        {
            SaveCombinedPointcloud(FlattenedPoints);
        }
 
        SendCombinedPointcloud(FlattenedPoints);
    }
}

References CanSimulateLidars, RaycastLidars, SaveCombinedCloudToDisk, SaveCombinedPointcloud(), SendCombinedCloudToROS, SendCombinedPointcloud(), SimulateLidars(), and ALidar::SimulateRaycastLidar().

Referenced by SimulateLidars(), and TickParallel().

◆ TickParallel()

void ALidarManager::TickParallel ( float DeltaTime )

private

Definition at line 92 of file LidarManager.cpp.

{
#ifdef WITH_EDITOR
    if (MeasurePerformance && TotalAverageTimer)
    {
        TotalAverageTimer->TakeTimeStamp();
    }
#endif
 
    SimulateLidars(DeltaTime);
 
#ifdef WITH_EDITOR
    if (MeasurePerformance && TotalAverageTimer)
    {
        double ms = TotalAverageTimer->PrintAverageTime();
        TimeInMilliseconds = FMath::RoundToFloat(ms * 100.0f) / 100.0f;
    }
#endif
}

References MeasurePerformance, SimpleTimer::PrintAverageTime(), SimulateLidars(), SimpleTimer::TakeTimeStamp(), TimeInMilliseconds, and TotalAverageTimer.

Referenced by AddRaycastLidar().

◆ UPROPERTY()

ALidarManager::UPROPERTY ( BlueprintReadOnly )

Member Data Documentation

◆ CanSimulateLidars

bool ALidarManager::CanSimulateLidars = true

private

Definition at line 213 of file LidarManager.h.

Referenced by SetSimulateLidars(), and SimulateLidars().

◆ CombinedLidarTopic

TWeakObjectPtr<UTopic> ALidarManager::CombinedLidarTopic

private

Definition at line 207 of file LidarManager.h.

Referenced by DestroyTopic(), InitTopic(), and SendCombinedPointcloud().

◆ FileSavePath

FString ALidarManager::FileSavePath

private

Definition at line 215 of file LidarManager.h.

Referenced by BeginPlay(), and SaveCombinedPointcloud().

◆ LidarSaves

int32 ALidarManager::LidarSaves = 0

private

Definition at line 217 of file LidarManager.h.

Referenced by SaveCombinedPointcloud().

◆ MeasurePerformance

bool ALidarManager::MeasurePerformance = false

Definition at line 147 of file LidarManager.h.

Referenced by SetMeasurePerformance(), and TickParallel().

◆ OnLidarDestroyed

FLidarDelegate_LidarDestroyed ALidarManager::OnLidarDestroyed

On Lidar sensor destroyed event. BlueprintAssignable.

Returns: int Lidar sensor count

Definition at line 138 of file LidarManager.h.

Referenced by RemoveRaycastLidar().

◆ OnLidarSpawned

FLidarDelegate_LidarSpawned ALidarManager::OnLidarSpawned

On Lidar sensor spawned event. BlueprintAssignable.

Returns: ALidar Lidar class pointer

Definition at line 131 of file LidarManager.h.

Referenced by AddRaycastLidar().

◆ PointCloudMessage

TSharedPtr<ROSMessages::sensor_msgs::PointCloud2> ALidarManager::PointCloudMessage

private

Definition at line 209 of file LidarManager.h.

Referenced by BeginPlay(), EndPlay(), and SendCombinedPointcloud().

◆ RaycastLidars

std::vector<ALidar*> ALidarManager::RaycastLidars

private

Container that holds references to Lidar sensors

Definition at line 201 of file LidarManager.h.

Referenced by AddRaycastLidar(), EndPlay(), RemoveRaycastLidar(), and SimulateLidars().

◆ RosInstance

UROSIntegrationGameInstance* ALidarManager::RosInstance = nullptr

private

Definition at line 204 of file LidarManager.h.

Referenced by BeginPlay(), EndPlay(), InitTopic(), and SendCombinedPointcloud().

◆ SaveCombinedCloudToDisk

bool ALidarManager::SaveCombinedCloudToDisk = false

Definition at line 144 of file LidarManager.h.

Referenced by ALidarManager(), SaveCombinedPointcloud(), SetSaveCombinedCloudToDisk(), and SimulateLidars().

◆ SendCombinedCloudToROS

bool ALidarManager::SendCombinedCloudToROS = true

Definition at line 141 of file LidarManager.h.

Referenced by InitTopic(), SendCombinedPointcloud(), SetSendCombinedCloud(), and SimulateLidars().

◆ TickAdded

bool ALidarManager::TickAdded = false

private

Definition at line 162 of file LidarManager.h.

Referenced by AddRaycastLidar(), EndPlay(), and RemoveRaycastLidar().

◆ TickEntry

FTickEntry ALidarManager::TickEntry

private

Definition at line 160 of file LidarManager.h.

Referenced by AddRaycastLidar(), EndPlay(), and RemoveRaycastLidar().

◆ TimeInMilliseconds

float ALidarManager::TimeInMilliseconds

Definition at line 150 of file LidarManager.h.

Referenced by TickParallel().

◆ TotalAverageTimer

SimpleTimer* ALidarManager::TotalAverageTimer = nullptr

private

Definition at line 211 of file LidarManager.h.

Referenced by BeginPlay(), EndPlay(), RemoveRaycastLidar(), and TickParallel().

The documentation for this class was generated from the following files:

Unreal/Agrarsense/Source/Agrarsense/Sensor/Lidar/LidarManager.h
Unreal/Agrarsense/Source/Agrarsense/Sensor/Lidar/LidarManager.cpp

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ ALidarManager()

Member Function Documentation

◆ AddRaycastLidar()

◆ BeginPlay()

◆ DestroyTopic()

◆ EndPlay()

◆ GetAllLidars()

◆ GetAllLidarsVector()

◆ GetLidarCount()

◆ InitTopic()

◆ RemoveRaycastLidar()

◆ ROSBridgeStateChanged()

◆ SaveCombinedPointcloud()

◆ SendCombinedPointcloud()

◆ SetMeasurePerformance()

◆ SetSaveCombinedCloudToDisk()

◆ SetSendCombinedCloud()

◆ SetSimulateLidars()

◆ SimulateLidars()

◆ TickParallel()

◆ UPROPERTY()

Member Data Documentation

◆ CanSimulateLidars

◆ CombinedLidarTopic

◆ FileSavePath

◆ LidarSaves

◆ MeasurePerformance

◆ OnLidarDestroyed

◆ OnLidarSpawned

◆ PointCloudMessage

◆ RaycastLidars

◆ RosInstance

◆ SaveCombinedCloudToDisk

◆ SendCombinedCloudToROS

◆ TickAdded

◆ TickEntry

◆ TimeInMilliseconds

◆ TotalAverageTimer