LidarManager.cpp

Go to the documentation of this file.

// Copyright (c) 2023 FrostBit Software Lab at the Lapland University of Applied Sciences
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
 
#include "LidarManager.h"
 
#include "Agrarsense/Utils/PointcloudUtilities.h"
#include "Agrarsense/Utils/PlatformUtilities.h"
#include "Agrarsense/Sensor/Lidar/Lidar.h"
#include "Agrarsense/ROS/ROSHandler.h"
#include "Agrarsense/Debugging/SimpleTimer.h"
 
#include "ROSIntegration/Classes/ROSIntegrationGameInstance.h"
#include "ROSIntegration/Classes/RI/Topic.h"
 
#include "PhysicsEngine/PhysicsObjectExternalInterface.h"
#include "Physics/Experimental/PhysScene_Chaos.h"
#include "Physics/PhysicsInterfaceUtils.h"
 
#include "Math/Vector.h"
#include "Engine/CollisionProfile.h"
#include "Runtime/Engine/Classes/Kismet/KismetMathLibrary.h"
#include "Runtime/Core/Public/Async/ParallelFor.h"
#include "Kismet/GameplayStatics.h"
#include "Kismet/KismetMathLibrary.h"
#include "Misc/CommandLine.h"
 
#include <iterator>
 
ALidarManager::ALidarManager(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
{
    // TickManager handles this Actor ticking
    PrimaryActorTick.bCanEverTick = false;
 
    if (FParse::Param(FCommandLine::Get(), TEXT("-save-combined-pointcloud")))
    {
        SaveCombinedCloudToDisk = true;
    }
}
 
void ALidarManager::BeginPlay()
{
    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();
}
 
void ALidarManager::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    Super::EndPlay(EndPlayReason);
 
    DestroyTopic();
 
    PointCloudMessage.Reset();
 
    RaycastLidars.clear();
 
    RosInstance = nullptr;
 
    delete(TotalAverageTimer);
 
    if (TickAdded)
    {
        ATickManager::RemoveTick(TickEntry);
    }
}
 
void ALidarManager::TickParallel(float DeltaTime)
{
#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
}
 
void ALidarManager::SimulateLidars(const float DeltaTime)
{
    TRACE_CPUPROFILER_EVENT_SCOPE(ALidarManager::SimulateLidars);
 
    const int32 LidarCount = RaycastLidars.size();
 
    if (LidarCount == 0 || !CanSimulateLidars)
    {
        return;
    }
 
    std::vector<FPointData> FlattenedPoints;
 
    FPhysicsCommand::ExecuteRead(GetWorld()->GetPhysicsScene(), [&]
        {
            if (LidarCount == 1)
            {
                // If we only have one lidar sensor, 
                // we can skip ParallelFor and 2D to 1D std::vector conversion
                ALidar* lidar = RaycastLidars[0];
                if (lidar)
                {
                    FlattenedPoints = lidar->SimulateRaycastLidar(DeltaTime);
                }
            }
            else
            {
                // Else we have multiple Lidar sensors,
                // Simulate all of them in parallel
                std::vector<std::vector<FPointData>> points(LidarCount);
 
                ParallelFor(LidarCount, [&](int32 lidarIndex)
                    {
                        ALidar* lidar = RaycastLidars[lidarIndex];
                        if (lidar)
                        {
                            points[lidarIndex] = lidar->SimulateRaycastLidar(DeltaTime);
                        }
                    }, EParallelForFlags::Unbalanced);
 
                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);
    }
}
 
void ALidarManager::SetSimulateLidars(bool bSimulateLidars)
{
    CanSimulateLidars = bSimulateLidars;
}
 
void ALidarManager::SetMeasurePerformance(bool bMeasurePerformance)
{
    MeasurePerformance = bMeasurePerformance;
}
 
void ALidarManager::SetSaveCombinedCloudToDisk(bool bSaveCombinedCloud)
{
    SaveCombinedCloudToDisk = bSaveCombinedCloud;
}
 
void ALidarManager::SetSendCombinedCloud(bool bSaveCombinedCloud)
{
    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
}
 
void ALidarManager::AddRaycastLidar(ALidar* lidarPtr)
{
    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);
}
 
void ALidarManager::RemoveRaycastLidar(ALidar* lidarPtr)
{
    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
    int32 currentLidarCount = RaycastLidars.size();
    OnLidarDestroyed.Broadcast(currentLidarCount);
}
 
void ALidarManager::ROSBridgeStateChanged(EROSState RosState)
{
    switch (RosState)
    {
    case EROSState::Connected:
        InitTopic();
        break;
 
    case EROSState::Disconnected:
        DestroyTopic();
        break;
    }
}
 
void ALidarManager::InitTopic()
{
    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
}
 
void ALidarManager::DestroyTopic()
{
    if (CombinedLidarTopic.IsValid())
    {
        CombinedLidarTopic->Unadvertise();
        CombinedLidarTopic->Unsubscribe();
        CombinedLidarTopic->MarkAsDisconnected();
        CombinedLidarTopic->ConditionalBeginDestroy();
        CombinedLidarTopic.Reset();
    }
}
 
void ALidarManager::SaveCombinedPointcloud(const std::vector<FPointData>& points)
{
    if (!SaveCombinedCloudToDisk)
    {
        return;
    }
 
    FString Filename = FString::Printf(TEXT("%sCombinedPointcloud_%d.ply"), *FileSavePath, LidarSaves);
    ++LidarSaves;
 
    UPointcloudUtilities::SaveVectorArrayAsPlyAsync(Filename, points);
}
 
void ALidarManager::SendCombinedPointcloud(const std::vector<FPointData>& points)
{
    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);
}